#include <avdweb_AnalogReadFast.h>
#include <RunningAverage.h>
#define PIN_SPEAKER 2
#define PIN_PITCH_PAD_RX 6
#define PIN_PITCH_PAD_TX 7

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

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

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.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;
}

void setup() {
  SerialUSB.begin(0);
  pinMode(PIN_PITCH_PAD_RX, INPUT);
  pinMode(PIN_PITCH_PAD_TX, OUTPUT);

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

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


void loop() {
  digitalWrite(PIN_PITCH_PAD_TX, LOW);
  delay(20);
  digitalWrite(PIN_PITCH_PAD_TX, HIGH);

//  for 
  int pitchValue = analogReadFast(PIN_PITCH_PAD_RX);
  Serial.print("Pitch: ");
  Serial.println(pitchValue);

  int freq = map(pitchValue, 0, 1000, 50, 4000);
  update_freq(freq);
}