#include <avr/io.h>

// notes - http://www.phy.mtu.edu/~suits/notefreqs.html

const float NOTEC3 = 130.81;
const float NOTEC4 = 261.63;
const float NOTED4 = 293.66;
const float NOTEE4 = 329.63;
const float NOTEG4 = 392;
const float NOTEA4 = 440;
const float NOTEB4 = 493.88;
const float NOTEC5 = 523.25;
const float NOTEG5 = 783.99;

long startTime;

const int ledPin = 5;
const int switchPin = 7;
const int spkrPin = 6;
boolean switchState, prevState;

// BEGIN SPI INTRO

#define DATAOUT 11//MOSI
#define DATAIN  12//MISO 
#define SPICLOCK  13//sck
#define SLAVESELECT 10//ss for /LIS3LV02DQ, active low
#define RTC_CHIPSELECT 9// chip select (ss/ce) for RTC, active high

byte clr;

// END SPI INTRO

// BEGIN TOUCH VARIABLES

const int touchPinA = 4;
const int touchPinB = 2;

int touchReferenceReading, touchReferenceDelta;

// END TOUCH VARIABLES


// BEGIN ACCELEROMETER VARIABLES

long n, nStroke, strokeEntropyX = 0, strokeEntropyY = 0, strokeEntropyZ = 0, maxX, maxY, initialVectorX, initialVectorY; // # of samples collected
long totalX = 0, totalY = 0, totalZ = 0; // sum of X readings 
long avgX, avgY, avgZ, calibX, calibY, calibZ, vectorX, vectorY, vectorZ; // average of X

int x_val, y_val, z_val;
byte x_val_l, x_val_h, y_val_l, y_val_h, z_val_l, z_val_h;
  
const int nVectorBuffer = 5;  

long vectorBufferX[nVectorBuffer];
long vectorBufferY[nVectorBuffer];

boolean inStroke = false;

const int motionThreshold = 30, entropyThreshold = 150, strokeThreshold = 500;

long vBufXAvg, vBufYAvg;

// END ACCELEROMETER VARIABLES

void setup() {


// BEGIN TOUCH SETUP
  
    touchReferenceReading = touchGetReading(); // ignore first one - was getting bogus values
  delay(100);
  touchReferenceReading = touchGetReading();
  touchReferenceDelta = touchReferenceReading / 3;
  
// END TOUCH SETUP
  
    // sound setup
    startTime = millis();
    pinMode(ledPin, OUTPUT);
    pinMode(switchPin, INPUT);
    pinMode(spkrPin, OUTPUT);
    prevState = false;
    
    // accelerom setup
    
      char in_byte;
  clr = 0;
  in_byte = clr;
  Serial.begin(9600);
  
    // set direction of pins
  pinMode(DATAOUT, OUTPUT);
  pinMode(DATAIN, INPUT);
  pinMode(SPICLOCK,OUTPUT);
  pinMode(SLAVESELECT,OUTPUT);
  digitalWrite(SLAVESELECT,HIGH); //disable device
  pinMode(10, OUTPUT);
  // SPCR = 01010000
  // Set the SPCR register to 01010000
  //interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
  //sample on leading edge of clk,system clock/4 rate
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPOL)|(1<<CPHA);
  clr=SPSR;
  clr=SPDR;
  // query the WHO_AM_I register of the LIS3LV02DQ
  // this should return 0x3A, a factory setting
  /* in_byte = read_register(15);
  Serial.print("WHO_AM_I [");
  Serial.print(in_byte, HEX);
  Serial.println("]"); */
  
  // start up the device
  // this essentially activates the device, powers it on, enables all axes, and turn off the self test
  // CTRL_REG1 set to 10000111
  write_register(0x20, 135);
   // query the WHO_AM_I register of the LIS3LV02DQ
  // this should return 0x3A, a factory setting
  in_byte = read_register(15);
  /* Serial.print("WHO_AM_I [");
  Serial.print(in_byte, HEX);
  Serial.println("]"); 
  Serial.println("----");
  delay(250); */ 
  

  
  
  // sound ack
  // doSpkr(NOTEC3, 0.5, 150);
  spkrBeep(spkrPin, NOTEC3, 150);
  delay(150);
  spkrBeep(spkrPin, NOTEC3, 150);
//  doSpkr(NOTEC3, 0.5, 150);
}

void loop()Ê{
  

  delay(20);
  
  switchState = touchIsTouched();
  pinMode(ledPin, OUTPUT);
  if (switchState != prevState) { 
    if (switchState == true) {
      
        // calibrate accelerometer 
  totalX = 0;
  totalY = 0;
  totalZ = 0;
  for (n = 0; n < 100; n++) {
    readAccelValues();
    totalX += x_val;
    totalY += y_val;
    totalZ += z_val;
    delay(1);
  }
  calibX = totalX / 100;
  calibY = totalY / 100;
  calibZ = totalZ / 100;
      
      Serial.println("CAPTURE START");
      
      digitalWrite(ledPin, HIGH);
      
      
      prevState = true;
      spkrBeep(spkrPin, NOTEC4, 75);
      spkrBeep(spkrPin, NOTEG4, 75);
      /* doSpkr(NOTEC4, 0.5, 50);
      doSpkr(NOTEG4, 0.5, 50); */
      n = 0;
      totalX = 0;
      totalY = 0;
      
      for (int vBufNdx = 0; vBufNdx < nVectorBuffer; vBufNdx++) {
        vectorBufferX[vBufNdx] = 0;
        vectorBufferY[vBufNdx] = 0;
      }
      inStroke = false;
      
    } else {
      digitalWrite(ledPin, LOW);
      prevState = false;
      
      spkrBeep(spkrPin, NOTEC5, 75);
      spkrBeep(spkrPin, NOTEG4, 75);
      
      /* doSpkr(NOTEC5, 0.5, 100);
      doSpkr(NOTEG4, 0.5, 50); */
      
      avgX = totalX / n;
      avgY = totalY / n;
      avgZ = totalZ / n;
      vectorX = avgX - calibX;
      vectorY = avgY - calibY;
      vectorZ = avgZ - calibZ;
      
      if (inStroke) {
        accelCloseStroke();
      }
      spkrBeep(spkrPin, NOTEC4, 125);
      
      // Z was doing weird shit here
      /* if ((abs(vectorX) < motionThreshold) && (abs(vectorY) < motionThreshold)) {
        // doSpkr(NOTEC4, 0.5, 100);
   
      } else {
        spkrBeep(spkrPin, NOTEB4, 100);
        spkrBeep(spkrPin, NOTEC5, 100);
        /* doSpkr(NOTEB4, 0.5, 100);
        doSpkr(NOTEC5, 0.5, 100); */
        // Serial.print("n: "); Serial.print(n, DEC);
        /* Serial.print(", totalX: "); Serial.print(totalX, DEC);
        Serial.print(", totalY: "); Serial.print(totalY, DEC);
        Serial.print(", calibX: "); Serial.print(calibX, DEC);
        Serial.print(", calibY: "); Serial.print(calibY, DEC);
        Serial.print(", calibZ: "); Serial.print(calibZ, DEC);
        Serial.print(", avgX: "); Serial.print(avgX, DEC);
        Serial.print(", avgY: "); Serial.print(avgY, DEC);
        Serial.print(", avgZ: "); Serial.print(avgZ, DEC); */
        /* Serial.print(", vectorX: "); Serial.print(vectorX, DEC);
        Serial.print(", vectorY: "); Serial.print(vectorY, DEC);
        Serial.print(", vectorZ: "); Serial.println(vectorZ, DEC);
      } */

      Serial.println("CAPTURE END");
      
    }
  }
  
  if (switchState == true) {
    // BEGIN READER

    readAccelValues();
  
    // END READER
    /* if (n % 20 == 0) {
      // print every 10000th sample
      Serial.print("x_val: "); Serial.print(x_val, DEC);
      Serial.print("   y_val: "); Serial.print(y_val, DEC);
      Serial.print("   z_val: "); Serial.println(z_val, DEC);
    } */
    

    
    long vBufXSum = 0, vBufYSum = 0;
    
    for (int vBufNdx = 0; vBufNdx < (nVectorBuffer-1); vBufNdx++) {
      vectorBufferX[vBufNdx] = vectorBufferX[vBufNdx + 1];
      vectorBufferY[vBufNdx] = vectorBufferY[vBufNdx + 1];
      
      vBufXSum += vectorBufferX[vBufNdx];
      vBufYSum += vectorBufferY[vBufNdx];
      
    }

    vectorBufferX[nVectorBuffer-1] = x_val - calibX;
    vectorBufferY[nVectorBuffer-1] = y_val - calibY;

    vBufXSum += abs(vectorBufferX[nVectorBuffer - 1]);
    vBufYSum += abs(vectorBufferY[nVectorBuffer - 1]);
    
    
    // Serial.print(vectorBufferX[nVectorBuffer-1]); Serial.print(" ");
    
    vBufXAvg = vBufXSum / nVectorBuffer;
    vBufYAvg = vBufYSum / nVectorBuffer;
    
    
    if (!inStroke) {
      if ((abs(vBufXAvg) > motionThreshold) || (abs(vBufYAvg) > motionThreshold)) {
        
        
        inStroke = true;

        accelResetStroke();

        initialVectorX = vBufXAvg;
        initialVectorY = vBufYAvg;

        
        // spkrBeep(spkrPin, NOTED4, 100);

        // Serial.println("STROKE START");
        
      }
    } else {
      
      
      n++;
      nStroke++;
      totalX += x_val - calibX;
      totalY += y_val - calibY;
      totalZ += z_val - calibZ;
      
      strokeEntropyX += log(abs(vectorBufferX[nVectorBuffer - 1] - vectorBufferX[nVectorBuffer - 2]));
      strokeEntropyY += log(abs(vectorBufferY[nVectorBuffer - 1] - vectorBufferY[nVectorBuffer - 2]));
      
      if (abs(vBufXAvg) > abs(maxX)) { maxX = vBufXAvg; }
      if (abs(vBufYAvg) > abs(maxY)) { maxY = vBufYAvg; }
      
      /* if (nStroke % 10 == 0) {
        Serial.print("X: ");
        for (int d = 0; d < nVectorBuffer; d++) {
          Serial.print(vectorBufferX[d]); Serial.print(" ");
        }
        Serial.println();
        Serial.print("Y: ");
        for (int d = 0; d < nVectorBuffer; d++) {
          Serial.print(vectorBufferY[d]); Serial.print(" ");
        }
        Serial.println();
        
        /* Serial.println(vectorBufferX);
        Serial.println(vectorBufferY);
      } */
      
      
      accelCloseStroke();

    }
    
    
    /* multiple "strokes" here
    if (abs(x_val-calibX) < 100) {
      doSpkr(NOTEC5, 0.5, 50);
    } */
    // delay(50);
  }
  
}

void accelCloseStroke() {
     if (
     ((abs(maxX) > strokeThreshold) || (abs(maxY) > strokeThreshold))
     
     /* && (((initialVectorX > 0) && (maxX > initialVectorX) || (initialVectorX < 0) && (maxX < initialVectorX))
     || ((initialVectorY > 0) && (maxY > initialVectorY) || (initialVectorY < 0) && (maxY < initialVectorY))) */
     
     
     // (abs(vBufXAvg) < motionThreshold) && (abs(vBufYAvg) < motionThreshold)
     // && (nStroke > 15)
     && ((abs(strokeEntropyX) > entropyThreshold) || (abs(strokeEntropyY) > entropyThreshold))
     // && (abs(avgX) > motionThreshold) && (abs(avgY) > motionThreshold)
     ) {
       
       
        inStroke = false;
        

        
        
        spkrBeep(spkrPin, NOTEE4, 100);
        // Serial.println("STROKE END");

        avgX = totalX / n;
        avgY = totalY / n;
        avgZ = totalZ / n;
        // Serial.println(); // DEBUG
        Serial.print(n);
        Serial.print(" ");
        Serial.print(nStroke);
        Serial.print(" ");
        Serial.print(avgX);
        Serial.print(" ");
        Serial.print(avgY);
        Serial.print(" ");
        Serial.print(maxX);
        Serial.print(" ");
        Serial.print(maxY);
        Serial.print(" ");
        Serial.print(strokeEntropyX);
        Serial.print(" ");
        Serial.print(strokeEntropyY);
        Serial.print(" ");
        Serial.print(vBufXAvg);
        Serial.print(" ");
        Serial.println(vBufYAvg);
        
        
                accelResetStroke();
        delay(300);
        spkrBeep(spkrPin, NOTEG4, 100);
      }
}

void accelResetStroke() {
        totalX = 0;
        totalY = 0;
        totalZ = 0;
        nStroke = 0;
        strokeEntropyX = 0;
        strokeEntropyY = 0;
        maxX = 0;
        maxY = 0;

        for (int vBufNdx = 0; vBufNdx < nVectorBuffer; vBufNdx++) {
          vectorBufferX[vBufNdx] = 0;
          vectorBufferY[vBufNdx] = 0;
        }
}


// speaker code from http://www.ladyada.net/make/eshield/examples.html
void spkrBeep(uint8_t pin, uint16_t freq, int32_t us) {
  int usdelay;
  usdelay = 500000 / freq;
  
  pinMode(pin, OUTPUT);
  
  us *= 1000; //in uS
  while (us > 0) {
    digitalWrite(pin, HIGH);
    delayMicroseconds(usdelay);
    digitalWrite(pin, LOW);
    delayMicroseconds(usdelay);
    us -= usdelay*2;
  }  
}


// BEGIN SPI FUNCTIONS

char spi_transfer(volatile char data)
{
  /*
  Writing to the SPDR register begins an SPI transaction
  */
  SPDR = data;
  /*
  Loop right here until the transaction is complete. the SPIF bit is 
  the SPI Interrupt Flag. When interrupts are enabled, and the 
  SPIE bit is set enabling SPI interrupts, this bit will set when
  the transaction is finished.
  */
  while (!(SPSR & (1<<SPIF)))     
  {};
  // received data appears in the SPDR register
  return SPDR;                    
}

// reads a register
char read_register(char register_name)
{
   char in_byte;
   // need to set bit 7 to indicate a read
   register_name |= 128;
   // SS is active low
   digitalWrite(SLAVESELECT, LOW);
   // send the address of the register we want to read first
   spi_transfer(register_name);
   // send nothing, but here's when the device sends back the register's value as an 8 bit byte
   in_byte = spi_transfer(0);
   // deselect the device
   digitalWrite(SLAVESELECT, HIGH); 
   return in_byte;
}

// write to a register
void write_register(char register_name, byte data)
{
  // char in_byte;
   // clear bit 7 to indicate we're doing a write
   register_name &= 127;
   // SS is active low
   digitalWrite(SLAVESELECT, LOW);
   // send the address of the register we want to write
   spi_transfer(register_name);
   // send the data we're writing
   spi_transfer(data);
   digitalWrite(SLAVESELECT, HIGH);
   //return in_byte;
}

// END SPI FUNCTIONS


// BEGIN ACCELEROMETER FUNCTIONS

void readAccelValues() {
  x_val_h = read_register(0x29); //Read outx_h 
  x_val_l  = read_register(0x28);
  x_val = x_val_h;
  x_val <<= 8;
  x_val += x_val_l;
  
  y_val_h = read_register(0x2B); //Read outx_h 
  y_val_l = read_register(0x2A); //Read outx_l
  y_val = y_val_h;
  y_val <<= 8;
  y_val += y_val_l;
  
  z_val_h = read_register(0x2D); //Read outz_h
  z_val_l = read_register(0x2C); //Read outz_l
  z_val = z_val_h;
  z_val <<= 8;
  z_val += z_val_l; 
}

// END ACCELEROMETER FUNCTIONS

// BEGIN TOUCH FUNCTIONS

boolean touchIsTouched() {
  int t = touchGetReading();
  // Serial.print(t, DEC);
  if (abs(t - touchReferenceReading) > touchReferenceDelta) {
    // Serial.println("true");
    return true;
  } else {
    // Serial.println("false");
    return false;
  }
}


int touchGetReading() {
  int count = 0;
  pinMode(touchPinA, OUTPUT);
  pinMode(touchPinB, OUTPUT);
  digitalWrite(touchPinA, LOW);
  digitalWrite(touchPinB, LOW);
  
  // while (digitalRead(touchPinA) != HIGH) {
  while ((PIND & B00010000) != B00010000) {
    
    count++;
    
    DDRD = B00010000; // pinMode(a, output)
    // DDRD &= !B00000100; // pinmode(b, input)
    PORTD |= B00010000; // write(a, high)
    
    /* DDRD = B00000100;
    PORTD &= !B00000100; */
    
    /* pinMode(touchPinB, INPUT);
    pinMode(touchPinA, OUTPUT);
    digitalWrite(touchPinA, HIGH); */
    
    
    pinMode(touchPinA, INPUT);
    pinMode(touchPinB, OUTPUT);
    digitalWrite(touchPinB, LOW);
  }
  return count;
}

// END TOUCH FUNCTIONS