/*
 Perfect Pitch: code for the sensor and sounder

 This file contains the code that runs on the NXT.

 See details in http://www.tau.ac.il/~stoledo/lego/msp430-perfect-pitch/

 Copyright 2007, Sivan Toledo
*/

void MSP430_send(tSensors nPort, unsigned byte cmd) {

  static unsigned byte i2c_write[3] = {2, 0x44, 0x00};

  i2c_write[2] = cmd;

  //nxtDisplayTextLine(3, "starting i2c");

  nI2CBytesReady[nPort] = 0; // Clear any pending bytes
  sendI2CMsg(nPort, i2c_write[0], 0);
  //xtDisplayTextLine(3, "waiting     ");
  while (nI2CStatus[nPort]==STAT_COMM_PENDING)
    wait1Msec(2); // Wait till I2C communication ends

  if (nI2CStatus[nPort]==NO_ERR) {
    nxtDisplayTextLine(3, "i2c no err  ");
   } else {
    nxtDisplayTextLine(5, "i2c err %d", nI2CStatus[nPort]);
  }
}



short MSP430_read(tSensors nPort) {

  static const unsigned byte read[2] = {1, 0x44};

  unsigned byte replyMsg[9];

  long total;
  long f;
  unsigned byte iter;

  //nxtDisplayTextLine(3, "starting i2c");

  nI2CBytesReady[nPort] = 0; // Clear any pending bytes
  sendI2CMsg(nPort, read[0], 9);
  //xtDisplayTextLine(3, "waiting     ");
  while (nI2CStatus[nPort]==STAT_COMM_PENDING)
    wait1Msec(2); // Wait till I2C communication ends

  if (nI2CStatus[nPort]==NO_ERR) {
    nxtDisplayTextLine(3, "i2c no err  ");
    readI2CReply(nPort, replyMsg[0], 9);


    total = ((long) replyMsg[0]) + ((long) replyMsg[1]*(long) 256) + ((long) replyMsg[2]*(long) 65536);// + ((long) replyMsg[3]*16777216);
    f     = ((long) replyMsg[4]) + ((long) replyMsg[5]*(long) 256) + ((long) replyMsg[6]*(long) 65536);// + (replyMsg[7]*16777216);
    iter  = replyMsg[8];

    nxtDisplayTextLine(1, "      ");
    nxtDisplayTextLine(1, "%d", total);
    nxtDisplayTextLine(2, "      ");
    nxtDisplayTextLine(2, "%d", f);
    nxtDisplayTextLine(3, "      ");
    nxtDisplayTextLine(3, "%d", (int) iter);

    if (f > 5000 && f*8 > total)
      nxtDisplayTextLine(3, "***");
    else
      nxtDisplayTextLine(3, "---");

    /*
    nxtDisplayTextLine(1, "      ");
    nxtDisplayTextLine(1, "%d", (int) replyMsg[0]);
    nxtDisplayTextLine(2, "      ");
    nxtDisplayTextLine(2, "%d", (int) replyMsg[1]);
    nxtDisplayTextLine(3, "      ");
    nxtDisplayTextLine(3, "%d", (int) replyMsg[2]);
    nxtDisplayTextLine(4, "      ");
    nxtDisplayTextLine(4, "%d", (int) replyMsg[3]);
    */
    //wait10Msec(25);


    return replyMsg[0];
  } else {
    nxtDisplayTextLine(5, "i2c err %d", nI2CStatus[nPort]);
    return -1;
  }
}

task main() {
	int state = 1;

  SensorType[S3] = sensorI2CCustomStd9V;
  wait10Msec(5);


  while (true)  {
    switch (state) {
    	case 1:
        if (nNxtButtonPressed == kEnterButton) {
          MSP430_send(S3,0x02); // start sound
          state = 2;
        } else
          MSP430_read(S3);
        break;
      case 2:
        if (nNxtButtonPressed != kEnterButton) {
          MSP430_send(S3,0x03); // stop sound
          MSP430_send(S3,0x01); // start monitoring
          state = 1;
        }
      break;
     }
  }
}