#include "NXCDefs.h"

#define u8  unsigned char
#define s8  char
#define u16 unsigned int
#define s16 int
#define u32 unsigned long
#define s32 long

u8 pressure;
s8 result;

u8 bytes_ready;
u8 i2c_read[] = { 0x44 };
u8 i2c_rsp[];

s32 amplitude_total;
s32 amplitude_f;
u8  amplitude_iter;

sub msp430_read()
{   
  //ClearScreen();
  //TextOut(0,30,"waiting");
  
  //Wait(1000);

  while (true) {
    result = LowspeedStatus(IN_3, bytes_ready);
    if (result == 0)  break;
    if (result == 32) continue;
    TextOut(0,30,"I2C Error 1:");
    NumOut(72,30,result);
    Wait(2000);
    return;
  }

  //ClearScreen();
  //TextOut(0,30,"sending");
  //Wait(1000);

  result = LowspeedWrite(IN_3, 9, i2c_read);

  while (true) {
    result = LowspeedStatus(IN_3, bytes_ready);
    if (result == 0)  break;
    if (result == 32) continue;
    TextOut(0,30,"I2C Error 2:");
    NumOut(72,30,result);
    Wait(2000);
    return;
  }

  //TextOut(0,30,"xread st: ");  
 // NumOut(72,30,result);  
  //TextOut(0,20,"xread by: ");  
  //NumOut(72,20,bytes_ready);  
  //Wait(4000);

  if (bytes_ready < 9) {
    TextOut(0,30,"I2C no data");
    Wait(1000);
    return;
  }
  
  //ClearScreen();
  //TextOut(0,30,"reading");
  //Wait(1000);

  result = LowspeedRead(IN_3, 9, i2c_rsp);
  
  /*
  ClearScreen();
  TextOut(0,30,"read st: ");  
  NumOut(72,30,result);  
  TextOut(0,20,"read by: ");  
  NumOut(72,20,bytes_ready);  
  TextOut(0,10,"size rsp: ");  
  NumOut(72,10,ArrayLen(i2c_rsp));  
  Wait(4000);
  */
  
  amplitude_total = (i2c_rsp[3] << 24) 
                   |(i2c_rsp[2] << 16) 
                   |(i2c_rsp[1] <<  8) 
                   |(i2c_rsp[0]      );
                    
  amplitude_f     = (i2c_rsp[7] << 24) 
                   |(i2c_rsp[6] << 16) 
                   |(i2c_rsp[5] <<  8) 
                   |(i2c_rsp[4]      );
  amplitude_iter  = i2c_rsp[8];
  
  ClearScreen();
  TextOut(0,30,"        ");  
  NumOut(0,30,amplitude_total);  
  TextOut(0,20,"        ");  
  NumOut(0,20,amplitude_f);  
  TextOut(0,10,"        ");  
  NumOut(0,10,amplitude_iter);
  if (amplitude_f > 5000 && amplitude_f*8 > amplitude_total)
    TextOut(0,0,"***"); 
  else 
    TextOut(0,0,"---"); 
  
  /*
  TextOut(50,50,"x     ");  
  NumOut(70,50,i2c_rsp[3]);  
  TextOut(50,40,"y     ");  
  NumOut(70,40,i2c_rsp[2]);  
  TextOut(50,30,"z     ");  
  NumOut(70,30,i2c_rsp[1]);  
  TextOut(50,20,"w     ");  
  NumOut(70,20,i2c_rsp[0]);  
  */
}

u8 i2c_cmd[] = { 0x44, 0x00 };

sub msp430_send(u8 cmd)
{    
  ClearScreen();

  TextOut(0,30,"send waiting");
  
  //Wait(200);

  while (true) {
    result = LowspeedStatus(IN_3, bytes_ready);
    if (result == 0)  break;
    if (result == 32) continue;
    TextOut(0,30,"I2C Error 1:");
    NumOut(72,30,result);
    Wait(2000);
    return;
  }

  TextOut(0,30,"send sending");
  
  i2c_cmd[1] = cmd;
  result = LowspeedWrite(IN_3, 0, i2c_cmd);

  while (true) {
    result = LowspeedStatus(IN_3, bytes_ready);
    if (result == 0)  break;
    if (result == 32) continue;
    TextOut(0,30,"I2C Error 2:");
    NumOut(72,30,result);
    Wait(2000);
    return;
  }
    
  //Wait(2000);

  TextOut(0,30,"send done      ");
}

int state;
task main()
{

  ClearScreen();

  SetSensorType(IN_3, IN_TYPE_LOWSPEED);
  SetSensorMode(IN_3, IN_MODE_RAW);
  ResetSensor(IN_3);
  Wait(250);

/*
  for (b=0; b<0; b++) {
    while (!ButtonState(BTNCENTER));
    msp430_send(0x02);
    TextOut(0,60,"X");
    while ( ButtonState(BTNCENTER));
    msp430_send(0x03);
    TextOut(0,60," ");    
  }
*/

  TextOut(0,60,"Goertzel");    
  msp430_send(0x01);

  state = 1;
  while (1) {
    switch (state) {
    	case 1:
        if (ButtonState(BTNCENTER)) {
          msp430_send(0x02); // start sound
          state = 2;
        } else
          msp430_read();
        break;
      case 2:
        if (!ButtonState(BTNCENTER)) {
          msp430_send(0x03); // stop sound
          msp430_send(0x01); // start monitoring
          state = 1;
        }
      break;
     }
   }
}