#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */

#include "target.h"
#include "usb_cdc.h"

#include "iic.h"                  // IIC macros
#include "mma845x.h" 

#define buffer_length 16
#define Seconds 0x02
#define sODR 4
word records_pointer;
unsigned char value[buffer_length];  
unsigned char buffer[buffer_length];
byte buff_i=0; 
byte i;
char c=0;
char init=1;
char ODR=0;
char maxg;

BIT_FIELD RegisterFlag; 
char DRDY = 0;
char afterFF=0;
char FF=0;

char TT=0;

char pocitej=0;
byte FF_MT_THS;
byte FF_MT_COUNT;
word address=0x0000;
word lastaddress=0x0000;
word cascelkem;
byte SlaveAddressIIC=addrmma;
byte int_source,ff_source,tr_source;
byte cas; 


typedef enum { RED,
               GREEN
             } LED;
LED led;

void blikej(LED ktera){
  led=ktera;
  RTCSC_RTCPS=8;
  RTCMOD=0;
  switch (led) {
    case RED: RED_LED=ON; break;
    case GREEN: GREEN_LED=ON;  break;
    default: RED_LED=OFF; GREEN_LED=OFF;
  
  }

}

void MMA845x_Init (void)
{
  MMA845x_Standby();
  /*
  **  Configure sensor for:
  **    - System Output Data Rate of 200Hz (5ms)
  **    - Fast read
  */
  EEPROM_ReadN(addr24c256,0, 0, 5, &value[0]);

  ODR=value[0];
  FF_MT_THS=value[1];
  FF_MT_COUNT=value[2];
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG1, (sODR<<3) + FREAD_MASK); //DATA_RATE_20MS
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG2, MODS_MASK);     //low power
  IIC_RegWrite(SlaveAddressIIC, XYZ_DATA_CFG_REG, FULL_SCALE_8G );
  IIC_RegWrite(SlaveAddressIIC, FF_MT_CFG_1_REG, XEFE_MASK+YEFE_MASK+ZEFE_MASK); //ELE_MASK
  IIC_RegWrite(SlaveAddressIIC, FT_MT_THS_1_REG, FF_MT_THS+DBCNTM_MASK);  //4
  IIC_RegWrite(SlaveAddressIIC, FF_MT_COUNT_1_REG, FF_MT_COUNT);  //6
  
  
  IIC_RegWrite(SlaveAddressIIC, TRANSIENT_CFG_REG, XTEFE_MASK+YTEFE_MASK+ZTEFE_MASK+HPF_BYP_MASK+TELE_MASK);
  IIC_RegWrite(SlaveAddressIIC, TRANSIENT_THS_REG, value[3]);
  IIC_RegWrite(SlaveAddressIIC, TRANSIENT_COUNT_REG, value[4]);
                    
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG4,INT_EN_FF_MT_1_MASK+INT_EN_TRANS_MASK);    //      
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG5,INT_CFG_TRANS_MASK+INT_CFG_FF_MT_1_MASK+INT_CFG_DRDY_MASK); //


  for (records_pointer=0x0020; 
       (EEPROM_Read( addr24c256,records_pointer>>8, records_pointer)!=0xff) && (records_pointer<0x7ff0) ;
       records_pointer+=0x0010);
 
}
  

 
void usb_operation(void) {

  hw_init();
  usb_cfg_init();
  cdc_init();


IICF_MULT = 0;
IICF_ICR = 0x00; //SCL divider of 20
IICC  = 0x80;   //Enable IIC 

 while(1)
  {                      
do {
RED_LED=PTED_PTED0;

if (PTED_PTED1) {
 asm("JMP 0x00"); // -> reset
_Stop;
}
}
while(!cdc_kbhit()); 


c=cdc_getch();
if (c=='r') {

  IIC_ReadN(addr24c256, buffer_length, &value[0]);         
     for (i=0;i<buffer_length;i++) {
     cdc_putch(value[i]);
     }
     cdc_process();
    } else if (c=='o') {
  value[0]=cdc_getch();
  value[1]=cdc_getch();
  IIC_WriteN(addr24c256, 2, &value[0]);
    } else if (c=='w') {
  c=cdc_getch();
  for (i=0;i<c+2;i++) {
  value[i]=cdc_getch();
  }
  
  IIC_WriteN(addr24c256, c+2, &value[0]);
    } else if (c=='b') {
  IIC_WriteTest(addr24c256, 64) ;

    } else if (c=='t'){
    IICF_ICR = 0x14; 
    IIC_RegReadN(addrpcf,Seconds,7, &value[0]);
    IICF_ICR = 0x00; 
    for (i=0;i<7;i++) {
     cdc_putch(value[i]);
     }
     cdc_process();
    
    }
    else if (c=='s'){
      for (i=0;i<7;i++) {
     value[i]=cdc_getch();
     }
    
    IICF_ICR = 0x14; 
    IIC_RegWriteN(addrpcf,Seconds,7, &value[0]);
    IICF_ICR = 0x00; 
  
     cdc_process();
    
    }
    
    
 } 
 
}

void init_board(){

MCGC2_BDIV=0; //16 MHz
PTADD=0xff;
PTBD=0b00110000;
PTBDD=0xff;
PTCDD=0xfc;
PTDDD=0xff;
PTEDD=0xff;
PTFDD=0xff;
PTGDD=0xff;

SPMSC1_LVDSE=0;
SOPT1=SOPT1_STOPE_MASK; //vypni watchdog, zapni stop  
EnableInterrupts
}

void buffer_write(char what) {
buffer[buff_i]=what;
buff_i++;
if (buff_i==buffer_length) {
buff_i=0;
EEPROM_WriteN(addr24c256,lastaddress>>8,lastaddress,buffer_length, &buffer[0]);
lastaddress=address+1;
}
address++;
if (address>32768) {
MMA845x_Standby;
RED_LED=ON;
IRQSC_IRQIE=0;
_Stop;
}              
}

void log_operation(void) {
IICF_MULT = 0;
IICF_ICR = 0x00; //SCL divider of 20
IICC  = 0x80;   //Enable IIC 

   
  MMA845x_Init();
  
  MMA845x_Active(); 

 IRQSC=IRQSC_IRQPDD_MASK+IRQSC_IRQPE_MASK+IRQSC_IRQIE_MASK;
 
  blikej(GREEN); 

while(1) {
  _Stop;
}
}

signed char abs(signed char co){
  if (co<0) {
    return -co;
  } else {
    return co;
  }
}

interrupt 2 void IRQ_ISR(void){
IRQSC_IRQACK=1;

int_source=IIC_RegRead(SlaveAddressIIC, INT_SOURCE_REG);

if ((int_source  & SRC_DRDY_MASK)  == SRC_DRDY_MASK   )  {
  IIC_RegReadN(addrmma, OUT_X_MSB_REG, 3, &value[0]);
if (FF)
{ 
  if (abs(value[0])>FF_MT_THS) FF=0;
  if (abs(value[1])>FF_MT_THS) FF=0; 
  if (abs(value[2])>FF_MT_THS) FF=0;
  
  if (!FF) {
  GREEN_LED=OFF;
  RED_LED=ON;
  pocitej=0;
  cascelkem=((cas<<8)+RTCCNT)+20*FF_MT_COUNT; //ODR perioda * pocet pred vyvolanim preruseni
  RTCSC_RTCPS=13;
  RTCMOD=0x2;
  afterFF=1;
  maxg=0;
  }
} 
if (afterFF | TT) {
  if (abs(value[0])>maxg) maxg=abs(value[0]);
  if (abs(value[1])>maxg) maxg=abs(value[1]); 
  if (abs(value[2])>maxg) maxg=abs(value[2]);
}

}

if ((int_source & SRC_FF_MT_1_MASK) == SRC_FF_MT_1_MASK) {
  ff_source=IIC_RegRead(SlaveAddressIIC, FF_MT_SRC_1_REG); 
 // IRQSC_IRQEDG=1;
  FF=1;
  pocitej=1;
  RTCMOD=0xff;
  RTCSC_RTCPS=8;
  cas=0;
  GREEN_LED=ON;
  MMA845x_Standby();
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG1, (ODR<<3) + FREAD_MASK); //fast DATA_RATE
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG4,INT_EN_DRDY_MASK);  // 
  MMA845x_Active();

}
if  ((int_source & SRC_TRANS_MASK) == SRC_TRANS_MASK) {
  tr_source=IIC_RegRead(SlaveAddressIIC, TRANSIENT_SRC_REG);
  RED_LED=ON;
  IIC_RegReadN(addrmma, OUT_X_MSB_REG, 3, &value[0]);
  maxg=0;
  if (abs(value[0])>maxg) maxg=abs(value[0]);
  if (abs(value[1])>maxg) maxg=abs(value[1]); 
  if (abs(value[2])>maxg) maxg=abs(value[2]);
  MMA845x_Standby();
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG1, (ODR<<3) + FREAD_MASK); //fast DATA_RATE
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG4,INT_EN_DRDY_MASK); 
  MMA845x_Active();
  TT=1;
  RTCSC_RTCPS=13;
  RTCMOD=0x2;
  
}
                  
}

void zapis(){
  MMA845x_Standby(); 
  IICF_ICR = 0x14;   //pomalu
  IIC_RegReadN(addrpcf,Seconds,7, &value[1]);
  IICF_ICR = 0x00; 
  value[0]=0; // oznaceni
  value[8]=(cascelkem>>8);
  value[9]=cascelkem;
  value[10]=maxg;
  EEPROM_WriteN(addr24c256,records_pointer>>8,records_pointer,buffer_length, &value[0]); 
  records_pointer+=0x10;
  value[0]=0xff;
  while(Wait_for_ACK(addr24c256)!=0); 
  EEPROM_WriteN(addr24c256,records_pointer>>8,records_pointer,1, &value[0]); 
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG1, (sODR<<3) + FREAD_MASK); //slow DATA_RATE 
  IIC_RegWrite(SlaveAddressIIC, CTRL_REG4,INT_EN_FF_MT_1_MASK+INT_EN_TRANS_MASK);  
  MMA845x_Active();
  RED_LED=OFF;
  blikej(GREEN);

}

interrupt 29 void RTC_ISR(void){
 RTCSC_RTIF=1;
 
 if (init) {
  RTCSC_RTCPS=0;
 } else {
 if (!pocitej) {
  if (RTCSC_RTCPS==8) {
    switch (led) {
      case GREEN: GREEN_LED=OFF; break;
      case RED: RED_LED=OFF; break;
      default: GREEN_LED=OFF; RED_LED=OFF; break;
    }
    RTCSC_RTCPS=15;
    RTCMOD=10;
  } else if (RTCSC_RTCPS==15) {
    switch (led) {
      case GREEN: GREEN_LED=ON; break;
      case RED: RED_LED=ON; break;
      default: GREEN_LED=OFF; RED_LED=OFF; break;
    }
    RTCSC_RTCPS=8;
    RTCMOD=0;
  } 
 }else{
    cas++;
  }
 
 }

if (afterFF){
  afterFF=0;
  zapis();
  
}
 
if (TT){
  TT=0;
  cascelkem=0;
  zapis();  
  }
}     

void main(void){
init_board();
PTEDD_PTEDD1=0;
PTEPE_PTEPE1=1;



RTCSC_RTIE=1;
RTCSC_RTCPS=13;
RTCMOD=2; 
_Stop;


  if (PTED_PTED1==0) {
  GREEN_LED=ON;
  PTEDD_PTEDD0=0;
  PTEPE_PTEPE0=1;
  usb_operation();
  } else {
    PTED_PTED1=0;
    PTEDD_PTEDD1=1;
    PTEPE_PTEPE1=0;
    init=0;
    log_operation();
  }
}




