#include <avdweb_AnalogReadFast.h>
#include <RunningAverage.h>


#define PIN_SPEAKER 2

#define PIN_PITCH_PAD_TX 4
#define PIN_PITCH_PAD_RX 6
#define PIN_GND 7


#define SIG_VOL 1023
#define LAMBDA 6UL
#define LAMBDA_MAX 10UL
#define FACTOR_MAX 20000UL

// Define the frequency of music notes (from http://www.phy.mtu.edu/~suits/notefreqs.html):
#define C4_HZ      261.63
#define D4_HZ      293.66
#define E4_HZ      329.63
#define F4_HZ      349.23
#define G4_HZ      392.00
#define A4_HZ      440.00
#define B4_HZ      493.88

// Define a C-major scale to play all the notes up and down.
float scale[] = { C4_HZ, D4_HZ, E4_HZ, F4_HZ, G4_HZ, A4_HZ, B4_HZ, A4_HZ, G4_HZ, F4_HZ, E4_HZ, D4_HZ, C4_HZ };
int scaleSize = sizeof(scale)/sizeof(float);

  
// SYNTHESIZER
uint32_t count = 0;
uint32_t count_max = 0;
int32_t prev_freq = 440;
int32_t freq = 440;
uint32_t freq_samp = 44100;
uint32_t factor = 0;
uint32_t sig_prev = 0;
bool pressed = true;

int tones[] = {261, 277, 294, 311, 330, 349, 370, 392, 415, 440};
//            mid C  C#   D    D#   E    F    F#   G    G#   A
int Ntones = sizeof(tones)/sizeof(int);


unsigned int sampleSize = 1000;
unsigned long workingAverage = 0;
unsigned int samplesTaken = 0;

void setup() {
//  SerialUSB.begin(19200);
  SerialUSB.begin(0);

//  pinMode(PIN_VOLUME_PAD_RX, INPUT);
  pinMode(PIN_PITCH_PAD_RX, INPUT);
  
  //discharge
  pinMode(PIN_PITCH_PAD_TX, OUTPUT);
  digitalWrite(PIN_PITCH_PAD_TX, LOW);
  //discharge
  pinMode(PIN_GND, OUTPUT);
  digitalWrite(PIN_GND, LOW);

  pinMode(PIN_SPEAKER, OUTPUT);
  analogWriteResolution(10);
  analogWrite(PIN_SPEAKER, 0);

    // SYNTHESIZER
  tcConfigure(freq_samp);
  tcStartCounter();

}

void loop() {

  //discharge
  digitalWrite(PIN_PITCH_PAD_TX, LOW);
  
  //reads the sensor pins
  int inputLeft = readCapacitiveSensor(PIN_PITCH_PAD_TX, PIN_PITCH_PAD_RX);
  
  //adds the current reading to the working average
  workingAverage += inputLeft;//map(inputLeft, 400, 500, 3000, 1000);

  //increments number of samples taken
  samplesTaken++;
  
  //if we have taken enough samples, calculate the average and print it to Serial out.
  //these values can then be read from other programs.
  if (samplesTaken == sampleSize) {
    unsigned int averageLeft = workingAverage / sampleSize;
    Serial.println(averageLeft);
    int freq = map(averageLeft, 400, 500, 0, 600);
    update_freq(freq);

    samplesTaken = 0;
    workingAverage = 0;
  }
  
}

int readCapacitiveSensor(int TXPin, int RXPin) {
  //discharge
  digitalWrite(TXPin, LOW);
  //charge
  digitalWrite(TXPin, HIGH);
  //read
  int input = analogReadFast(RXPin);
  return input;
}

void playNote(int freq){
  for (int i = 0; i<= Ntones; i++){
    int note = tones[i];
    int next_note = note;
    if (i != Ntones){
        next_note = tones[i+1];
    }
    int delta = freq - note;
    int next_delta = freq - next_note;
    if (delta > 0){
      if (delta < next_delta){
       update_freq(note);
       }
    update_freq(next_note);
    }
  }
}

void tcConfigure(uint16_t sampleRate) {
  GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5)) ;
  while (GCLK->STATUS.bit.SYNCBUSY);
  tcReset();
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1 | TC_CTRLA_ENABLE; // added from tutorial
  TC5->COUNT16.CC[0].reg = (uint16_t) (48000000UL / sampleRate);
  while (tcIsSyncing());
 
  NVIC_DisableIRQ(TC5_IRQn);
  NVIC_ClearPendingIRQ(TC5_IRQn);
  NVIC_SetPriority(TC5_IRQn, 0);
  NVIC_EnableIRQ(TC5_IRQn);

  // enable interrupt
  TC5->COUNT16.INTENSET.bit.MC0 = 1;
  while (tcIsSyncing());
}

bool tcIsSyncing() {
  return TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY;
}

void tcStartCounter() {
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
  while (tcIsSyncing()); //wait until snyc'd
}

void tcReset() {
  TC5->COUNT16.CTRLA.reg = TC_CTRLA_SWRST;
  while (tcIsSyncing());
  while (TC5->COUNT16.CTRLA.bit.SWRST);
}

void tcDisable() {
  TC5->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE;
  while (tcIsSyncing());
}

void update_freq(uint16_t val) {
  freq = val;
  count_max = freq_samp / freq;
}

void TC5_Handler (void) {
  // busy DAC, abort this sample
  if (DAC->STATUS.bit.SYNCBUSY == 1) {
    // re-enable interrupt
    TC5->COUNT16.INTFLAG.bit.MC0 = 1;
    return;
  }

  uint32_t sig, samp;

  sig = 0;
  if (count >= count_max / 2) {
    // square wave of wanted frequency
    sig += SIG_VOL;
  }
  count++;
  if (count >= count_max) {
    count = 0;
  }
  
  // low pass filter
  sig = (sig * (LAMBDA_MAX - LAMBDA) + sig_prev * LAMBDA)/LAMBDA_MAX;

  sig_prev = sig;

  if (pressed) {
    // attack
    if (factor >= FACTOR_MAX) {
      factor = FACTOR_MAX;
    } else {
      factor += 3;
    }
  } else {
    // decay
    if (factor > 0) {
      factor--;
    }
  }
  
  samp = sig * factor / FACTOR_MAX;
  DAC->DATA.reg = samp & 0x3FF; // 10 bit DAC

  // re-enable interrupt
  TC5->COUNT16.INTFLAG.bit.MC0 = 1;
}