#include "Adafruit_FreeTouch.h"

#define PIN_LED_R 15
#define PIN_LED_G 16
#define PIN_LED_B 17
#define PIN_BUZZER 2

#define SIG_VOL 1023

#define LAMBDA 6UL
#define LAMBDA_MAX 10UL

#define FACTOR_MAX 20000UL

#define PAD_THRESH 600

#define N_PAD 5

const int piano_pins[N_PAD] = {3, 4, 5, 6, 7};

const int piano_tone[N_PAD] = {440, 494, 392, 196, 294};

const int piano_rgb[N_PAD][3] = {{1023,    0,    0},
                                 { 512,  512,    0},
                                 {   0, 1023,    0},
                                 {   0,  512,  512},
                                 {   0,    0, 1023}};

// 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;

// QTOUCH
bool error = false;
bool pressed = false;
int i_trig = -1;

bool piano_pressed[N_PAD] = {false, false, false, false, false};

Adafruit_FreeTouch qt_array[N_PAD];

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 setup() {
  SerialUSB.begin(0);

  // RGB LED
  digitalWrite(PIN_LED_R, HIGH);
  digitalWrite(PIN_LED_G, HIGH);
  digitalWrite(PIN_LED_B, HIGH);
  pinMode(PIN_LED_R, OUTPUT);
  pinMode(PIN_LED_G, OUTPUT);
  pinMode(PIN_LED_B, OUTPUT);

  // BUZZER
  pinMode(PIN_BUZZER, OUTPUT);
  analogWriteResolution(10);
  analogWrite(PIN_BUZZER, 0);
  update_freq(440);

  // CAPACITIVE TOUCH
  for (int i = 0; i < N_PAD; i++) {
    qt_array[i] = Adafruit_FreeTouch(piano_pins[i], OVERSAMPLE_4, RESISTOR_50K, FREQ_MODE_NONE);
    if (!qt_array[i].begin()) {
      //SerialUSB.print("Failed to begin qt on pin ");
      //SerialUSB.println(piano_pins[i]);
      error = true;
    }
  }

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

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 show_rgb(const int* rgb) {
  // blue LED is about half the brightness, compensate here
  analogWrite(PIN_LED_R, 1023 - rgb[0] / 2);
  analogWrite(PIN_LED_G, 1023 - rgb[1] / 2);
  analogWrite(PIN_LED_B, 1023 - rgb[2]);
}

void show_black() {
  analogWrite(PIN_LED_R, 1023);
  analogWrite(PIN_LED_G, 1023);
  analogWrite(PIN_LED_B, 1023);
}

void loop() {
  if (error) {
    SerialUSB.println("Error: Capacitive touch initialization failed.");
    return;
  }

  bool piano_pressed_new[N_PAD];
  bool something_pressed = false;

  for (int i = 0; i < N_PAD; i++) {
    int val = qt_array[i].measure();
    piano_pressed_new[i] = val > PAD_THRESH;
    something_pressed |= piano_pressed_new[i];

    // if (piano_pressed_new[i] && !piano_pressed[i]) {
    //   SerialUSB.print("Touchpad ");
    //   SerialUSB.print(i);
    //   SerialUSB.print(" activated with value: ");
    //   SerialUSB.println(val);
    // }
  }

  int i_trig_new = -1;

  if (something_pressed) {
    // retain old state, update most recent press right after this
    i_trig_new = i_trig;
  }

  for (int i = 0; i < N_PAD; i++) {
    if (piano_pressed_new[i] && !piano_pressed[i]) {
      i_trig_new = i;
      // Ausgabe, dass ein neues Touchpad aktiviert wurde
      //SerialUSB.print("New trigger on touchpad ");
      //SerialUSB.println(i);
    }
    piano_pressed[i] = piano_pressed_new[i];
  }

  if (i_trig_new != i_trig) {
    // update
    i_trig = i_trig_new;

    if (i_trig != -1) {
      // new tone
      show_rgb(piano_rgb[i_trig]);
      update_freq(piano_tone[i_trig]);
      factor = 0;
      pressed = true;
      //SerialUSB.print("Playing tone for touchpad ");
      SerialUSB.println(i_trig + 1);
    } else {
      // nothing pressed
      show_black();
      pressed = false;
      SerialUSB.println(0);
    }
  }
}