/*
 Perfect Pitch: code for the sensor and sounder

 This file contains the code that runs on the sensor.

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

 Copyright 2007, Sivan Toledo (except for the i2c routines)
*/

#include <msp430x20x2.h>

typedef unsigned char      uint8;
typedef signed char        int8;
typedef unsigned short     uint16;
typedef signed short       int16;
typedef unsigned long      uint32;
typedef signed long        int32;

// Hamming window coefficients and Goertzel sine/cosine pair
// Generated automatically by a Matlab program
#include "coefficients.h"

void Init_USI_I2C_SLAVE(void);
void USI2_BUGFIX(void);

//define LED_ENABLE (P1DIR |= 0x01)
#define GREEN_LED_ON     (P1OUT |= 0x01)
#define GREEN_LED_OFF    (P1OUT &= ~0x01)
#define GREEN_LED_TOGGLE (P1OUT ^= 0x01)
#define RED_LED_ON       (P1OUT |= 0x02)
#define RED_LED_OFF      (P1OUT &= ~0x02)
#define RED_LED_TOGGLE   (P1OUT ^= 0x02)

volatile uint16 dummy;

volatile uint16 sample;
//volatile uint8  sample_ready = 0;

// variables for the low-pass averaging filter
uint16 sample_count;
uint16 sample_sum;

// commands with a 0 upper nibble are
// i2c commands that should be sent to
// the main loop

typedef enum {
	idle             = 0xff,
	goertzel_start   = 0x01,
	sound_start      = 0x02,
	sound_stop       = 0x03,
	goertzel_iter    = 0x10,
	error            = 0x20
} cmd_t;
	

volatile uint8 main_loop_cmd;

 
int32 amplitude_total;
int32 amplitude_f;
uint8 amplitude_iter;
int32 d1, d2, y1, y2, sum_abs;
int16 s;
uint8 i;
int16 shift;

void main(void) {

  WDTCTL = WDTPW+WDTHOLD;                    // Stop watchdog timer

  DCOCTL = CALDCO_16MHZ;                     // Set DCO for 1MHz using
  BCSCTL1 = CALBC1_16MHZ;                    // calibration registers
  
  //BCSCTL3 |= LFXT1S1; // use VLOCLK for ACLK
  //BCSCTL3 |= XCAP_2; // 10pf

  // set up the IO ports

  P1SEL = 0x00; // P1.x port function = GPIO
  P1DIR = 0x07; // P1.x direction: output on 0,1 (LEDs) and 2 (sounder), input on rest 
  P1OUT = 0x00; // All P1.x reset
  
  P1REN = 0x08;  // enable pull up/down on P1.3 (the switch)
  P1OUT |= 0x08; // pull it up
  
  P1IE  = 0x08; // enable interrupt on P1.3
  P1IES = 0x08; // high-to-low transition
  
  Init_USI_I2C_SLAVE(); // Initialize I2C as Slave

  // set up the ADC
  
  ADC10CTL0 = SREF_0     // Vss to Vcc
            + ADC10SHT_0 // 4 cycle sample hold hold
            + ADC10SR    // only up to 50ksps
            + ADC10ON    // turn it on
            + ADC10IE    // interrupt enable
            ;
  ADC10CTL1 = INCH_5      // A5 input pin
            + ADC10SSEL_3 // SMCLK
            ;
  ADC10AE0  |= (1 << 5); // enable analog input 5
  ADC10CTL0 |= ENC; // enable;

  // set up sampling to get the shift to zero
  sample_count = 0;
  sample_sum = 0;
  // we first stop the timer  
  TACTL &= ~MC_0;

  TACCR0  = 2 - 1;
  TACTL   = TASSEL0+TACLR+MC_1+TAIE;   // ACLK, Clear TA, up mode, enable interrupt
  TACCTL0 |= CCIE;
  
  // sample to estimate the shift

  __enable_interrupt();

  y1 = 0;
  for (shift=0; shift<1024; shift++) {
    LPM0;
    __disable_interrupt(); 
    
    s = sample;
    //sample_ready = 0;
    
    __enable_interrupt();
    
    y1 += s;
  }
  // stop the timer
  TACTL   &= ~MC_0;
  shift = y1 >> 10;
  
  main_loop_cmd = goertzel_start;
 
  while (1) {
   	switch (main_loop_cmd) {
   	  case sound_start:
   	    //GREEN_LED_ON;
   	    // stop the timer
        TACTL &= ~MC_0;

        // now set the PWM output TA1 for 2048Hz, 50% duty cycle
        TACCR0  = 16 - 1;
        TACTL   = TASSEL0+TACLR;               // ACLK, Clear TA
        //TACTL   = TASSEL1+TACLR;             // SMCLK, Clear TA
        TACCR1  = 8;                           // CCR1 PWM Duty Cycle
        TACCTL1 = OUTMOD_7;                    // CCR1 reset/set
        TACCTL0 &= ~CCIE;                      // no interrupts
        TACTL |= MC0;                          // Start TA in up mode
        P1SEL |= 0x04; // enable TA1 on P1.2
     	main_loop_cmd = idle;
   	    break;
   	  case sound_stop:
   	    //GREEN_LED_OFF;

        P1SEL &= ~0x04; // disable TA1 on P1.2, return to GPIO
        P1OUT &= ~0x04; // set to 0

   	    // stop the timer
        TACTL &= ~MC_0;
    	main_loop_cmd = idle;
   	    break;
   	  case goertzel_start:
   	    amplitude_iter = 0;
        // mark the sample buffer as empty  
        //sample_ready = 0;

        // initialize the low-pass averaging filter
        sample_count = 0;
        sample_sum = 0;
        
        // initialize the Goerzel algorithm
        d1      = 0;
        d2      = 0;
        sum_abs = 0;
        i       = 0;

        TACTL &= ~MC_0;

        TACCR0  = 2 - 1;
        TACTL   = TASSEL0+TACLR+MC_1+TAIE;   // ACLK, Clear TA, up mode, enable interrupt
        TACCTL0 |= CCIE;

    	main_loop_cmd = goertzel_iter;
   	    break;
   	  case goertzel_iter:
        __disable_interrupt(); 
        s = sample;
        //sample_ready = 0;
        __enable_interrupt();  

        s -= shift;
    
        y1 = ((int32) scaling[i]) * ((int32) s);
        y1 >>= log2_one_rep;

        y2 = realW * d1;
        y2 >>= (log2_one_rep - 1);

        y2 += y1 - d2;

        if (y1 > 0)
          sum_abs += y1;
        else
          sum_abs -= y1;

        d2 = d1;
        d1 = y2;

        i++;
    
        if (i == window_size) {
          y1 = realW * d1;
          y1 >>= log2_one_rep;
          y1 = y1 - d2;
          // this is the real part of the Fourier coefficient
      
          y2 = imagW * d1;
          y2 >>= log2_one_rep-1;
          // this is the complex part
      
          if (y1 < 0) y1 = -y1;
          if (y2 < 0) y2 = -y2;
      
          y1 += y2;
      
          if (y1>5000 && y1 > (sum_abs >> 3))
            GREEN_LED_ON;
          else
            GREEN_LED_OFF;
          
          __disable_interrupt(); 
      
          amplitude_total = sum_abs;
          amplitude_f     = y1;
          amplitude_iter++;
          
          __enable_interrupt();
     
      
          // initialize for the next window
          d1      = 0;
          d2      = 0;
          sum_abs = 0;
          i       = 0;
        }
   	    
   	    break;
   	  default:
   	    break;   
   	}
   	LPM0;
  }  
}

//volatile uint16 c = 0;
#ifdef __ICC430__
#pragma vector=ADC10_VECTOR
#endif
__interrupt void Adc10 (void)
{
  /*
  c++;
  if (c==32768) {
  	RED_LED_TOGGLE;
  	c = 0;
  }
  */
  sample_count++;
  sample_sum += ADC10MEM;
  if (sample_count == 4) {
  	sample = sample_sum;
  	/*
    if (sample_ready) {
      RED_LED_ON;
    } else {
      sample       = sample_sum;
      sample_ready = 1;
    }
    */
    LPM0_EXIT;
    sample_count      = 0;
    sample_sum = 0;
  }
}
#ifdef __TI_COMPILER_VERSION__
ADC10_ISR(Adc10)
#endif

//volatile uint16 tc = 0;
#ifdef __ICC430__
#pragma vector=TIMERA0_VECTOR
#endif
__interrupt void Timer_A (void)
{
  ADC10CTL0 |= ADC10SC;
  /*
  tc++;
  if (tc == 32768) {
    tc = 0;
    GREEN_LED_TOGGLE;
  }
  */
}
#ifdef __TI_COMPILER_VERSION__
TIMERA0_ISR(Timer_A)
#endif

#ifdef __ICC430__
#pragma vector=TIMERA1_VECTOR
#endif
__interrupt void Timer_Ax (void)
{
  ADC10CTL0 |= ADC10SC;

  dummy = TAIV;  // read to clear interrupt flag
  /*
  tc++;
  if (tc == 32768) {
    tc = 0;
    GREEN_LED_TOGGLE;
  }
  */
}
#ifdef __TI_COMPILER_VERSION__
TIMERA1_ISR(Timer_Ax)
#endif

/*** Button Interrupt ***/  

#ifdef __ICC430__
#pragma vector=PORT1_VECTOR
#endif
__interrupt void p1_isr (void)
{
  if (P1IN & 0x08) {
    P1IES = 0x08;   // high-to-low
    main_loop_cmd = sound_stop; 
    //RED_LED_OFF;
  } else {
    P1IES = 0x00;
    main_loop_cmd = sound_start; 
    //RED_LED_ON;
  }
  LPM0_EXIT;

  P1IFG = 0x00; // clear the interrupt
}
#ifdef __TI_COMPILER_VERSION__
PORT1_ISR(p1_isr)
#endif

/***********************************************************/
/* i2c code                                                */
/***********************************************************/

//static short USI_OA; // Storage for Own Address Compare Value
uint8 USI_SA; // Storage for Slave Address Received
uint8 USIData; // Storage for Received Data

uint8 i2c_byte_count;

#define I2C_OA (0x44)
//#define I2C_OA (0x48)

uint16 ProgramMode; // Current program mode
enum { 
  PM_IDLE, // Program Mode - IDLE (Slave Idle Mode)
  PM_SA_RECD, // Program Mode - SA_RECD (Slave Address Receive)
  PM_RCVR_DATA_PREP, // Program Mode - RCVR_DATA_PREP (Receiver Data Control Setup)
  PM_XMIT_DATA_PREP, // Program Mode - XMIT_DATA_PREP (Transmitter Data Control Setup)
  PM_DATA_RECD, // Program Mode - DATA_REC (Data Receive)
  PM_DATA_SENT // Program Mode - DATA_SEND (Data Send)
};

union {
  struct {
    int32 total;
    int32 f;
    uint8 iter;
  };
  uint8 u8[9];
} i2c_send_packet;
//volatile int32 i2c_amplitude_total;
//volatile int32 i2c_amplitude_f;
//int8   i2c_amplitude_iter;

void Init_USI_I2C_SLAVE(void) {
  //USI_OA = I2C_OA*2;
  USIData = 0x00;
    
  ProgramMode = PM_IDLE;
  USISRL = 0xFF;
  USICKCTL = USICKPL; // USICLK=SCL (Inverted)
  USICTL1 |= USII2C+USISTTIE; // USI in I2C Mode, Enable USISTTIFG Interrupts
  USICTL0 = USIPE7+USIPE6+USISWRST; // USI Port Control for P1.7 and P1.6

  P1REN |= 0xC0; // Enable Pull-up/down Option
  P1OUT |= 0xC0; // Select Pull-up for P1.7 and P1.6
  //P1DIR = 0xFF; // P1.0, P1.1, and P1.4 As output
  //P1SEL |= 0xC0; // 

  USICNT = 0x08; // USICNT 8-bits
  USICTL0 &= ~USISWRST; // Clear Reset of USI
}

#ifdef __ICC430__
#pragma vector=USI_VECTOR
#endif
__interrupt 
void usi_isr (void) {
  if (USICTL1 & USISTTIFG) { // Was Start Condition Detected?
    USICTL0 &= ~USIOE; // Clear Output Enable
    USICNT = 0x08; // Load USICNT for 8-bits Slave Address receive
    USICTL1 &= ~USISTTIFG; // Clear USISTTIFG Start Interrupt Flag
    USICTL1 |= USIIE; // Enable USIIFG Interrupt
    ProgramMode = PM_SA_RECD; // Receive Slave Address
  } else {
    switch (ProgramMode) {
      case PM_IDLE : // Program Mode - IDLE (Slave Idle Mode)
        USICNT = 0x08; // Keep Loading USICNT While IDLE
        break;
      case PM_SA_RECD : // Program Mode - SA_RECD (Slave Address Received, If Match then Acknowledge, else Idle)
        USI_SA = USISRL;
        USI_SA &= 0xFE; // Remove Read/Write Bit
        if (USI_SA == 0x44) { // Slave Address Match --> Send Acknowledge
          i2c_byte_count = 0;
          if (USISRL & 0x01) {
            ProgramMode = PM_XMIT_DATA_PREP; // Generate Slave Address Ack -> Transmitter Data Control Setup

            i2c_send_packet.total = amplitude_total;
            i2c_send_packet.f     = amplitude_f;
            i2c_send_packet.iter  = amplitude_iter;
          } else {
            //USIData = 0; // clear data storage
            ProgramMode = PM_RCVR_DATA_PREP; // Generate Slave Address Ack -> Receiver Data Control Setup
          }
          USICTL0 |= USIOE; // Set Output Enable
          USISRL = 0x00; // Load USISRL for Acknowledge
          // USICNT = 0x01; // Load USICNT 1-bit transmit ACK (Expected Operation)
          USI2_BUGFIX(); // Bug Fix for Issue with USI Ack Generation
          break;
        } else { // Slave Address Not Matched --> Do not Acknowledge and return to IDLE
          ProgramMode = PM_IDLE; // Idle State - Wait For USISTTIFG
          USICTL0 &= ~USIOE; // Clear Output Enable
          USISRL = 0xFF; // Load USISRL for No Acknowledge
          // USICNT = USISCLREL+0x01; // Load USICNT 1-bit transmit NAck (Expected Operation)
          // // Disable SCL Holding (Start Condition Must Reset)
          USI2_BUGFIX(); // Bug Fix for Issue with USI NAck Generation
          USICNT = USISCLREL; // Disable SCL Holding (Start Condition Must Reset)
          USICTL1 &= ~USIIE; // Clear USIIFG Interrupt Enable
          break;
        }
      case PM_RCVR_DATA_PREP : // Program Mode - RCVR_DATA_PREP (Receiver Data Setup).
        USICTL0 &= ~USIOE; // Clear Output Enable
        USICNT = 0x08; // Load USICNT for 8-bits Data to Receive
        ProgramMode = PM_DATA_RECD; // Data Received - I2C Slave received Data from Master
        break;
      case PM_XMIT_DATA_PREP : // Program Mode - XMIT_ACK_SENT (Transmitter Acknowledge Sent) --> Transmit Data Setup.
        USIData = i2c_send_packet.u8[i2c_byte_count++];
        USICTL0 |= USIOE; // Set Output Enable
        USISRL = USIData; // Fixed Data Load
        USIData++; // sivan, increment
        USICNT = 0x08; // Load USICNT for 8-bits Data to Send
        ProgramMode = PM_DATA_SENT; // Data Sent - I2C Slave sent Data to Master
        break;
     case PM_DATA_RECD : // Program Mode - DATA_RECD (Data Received) --> Send Acknowledge
        USIData = USISRL;
      	if ((USIData & 0xF0) == 0) { 
       	  main_loop_cmd=USIData; 
       	  LPM0_EXIT; 
       	}
        /*
        if (i2c_byte_count == 0) { // command
       	  if ((USIData & 0xF0) == 0) { 
       	  	main_loop_cmd=USIData; 
       	  	LPM0_EXIT; 
       	  }
        }
        i2c_byte_count++;
        */
        USICTL0 |= USIOE; // Set Output Enable
        USISRL = 0x00; // Load USISRL for Acknowledge
        //USICNT = 0x01; // Load USICNT 1-bit transmit ACK (Expected Operation)
        USI2_BUGFIX(); // Bug Fix for Issue with USI Ack Generation
        ProgramMode = PM_RCVR_DATA_PREP; // Receiver Acknowledge Sent
        break;
     case PM_DATA_SENT : // Program Mode - DATA_SENT (Data Sent) --> Check Acknowledge
        USICTL0 &= ~USIOE; // Clear Output Enable 
        // USICNT = 0x01; // Load USICNT 1-bit transmit ACK (Expected Operation)
        USI2_BUGFIX(); // Bug Fix for Issue with USI Ack Generation 
        if (USISRL & 0x01) {
          USICNT = 0x02; // Master NAck, Load USICNT 2-bit To accept Stop Condition or Handle Re-start
          USICTL1 &= ~USIIE; // Clear USIIFG Interrupt Enable
          ProgramMode = PM_IDLE; // Idle State - Wait For USISTTIFG
        } else 
          ProgramMode = PM_XMIT_DATA_PREP; // Transmitter Acknowledge Sent
        break;
     }
  }
  
}
#ifdef __TI_COMPILER_VERSION__
USI_ISR(usi_isr)
#endif

//################################################################################
// USI2 Bug fix routines
//
//################################################################################
void USI2_BUGFIX(void) {
  //P1OUT ^= 0x02;
  while (P1IN & 0x40){} // Test SCL P1(6) LOW
  USICNT = USISCLREL+0x01;
  while ((P1IN & 0x40) == 0){} // Test SCL P1(6) HIGH
  while (P1IN & 0x40){} // Test SCL P1(6) LOW
  USICNT = 0x00; // Clear USISCLREL
  //P1OUT &= ~0x02;
}