#include <ArduinoJson.h>
#include <ArduinoJson.hpp>

#include <TCA9555.h>
#include <AS5600.h>
#include <FastLED.h>
#include <PWMAudio.h>
#include <I2S.h>
#include <SPI.h>
#include <SD.h>
#include "codec.h"
#include "ringbuffer.h"

#define HAPTIC 1
#define AURAL 1
#define UI_SAMPLERATE 22050

#define SIZE 1024
#define RINGBUFFER 16384
#define ECHO 24000

#define NSTREAMS 8

int16_t buffer[SIZE];
int16_t buffer2[ECHO];

RingBuffer<int16_t> ringbuffer[NSTREAMS];

File streams[NSTREAMS];
bool streams_loaded = false;

I2S i2s(INPUT_PULLUP);

TLV320AIC3204 codec;

TCA9555 TCA(0x20, &Wire1);
AS5600 ENC(&Wire1);

PWMAudio ui_snd(8);

enum BUTTON {CVINL, CVINR, INL, INR, OUTR, OUTL, CVOUTR, CVOUTL, RIGHT, LEFT, SELECT, DEBUG1, DEBUG2, DEBUG3};

int lut_ring_cw[48] = {39,38,37,36,35,34,33,32,31,30,29,28,27,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,50,49,48,47,46,45,44,43,42,41,40};
int lut_ring_ccw[48] = {40,41,42,43,44,45,46,47,48,49,50,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39};
int lut_matrix[13] = {56,55,57,58,59,62,61,60,63,64,65,67,66};

int active = 0;
int active_led_ring = 0;

uint32_t lastTime = 0;
int32_t position = 0;

CRGB ui_leds[74];
CRGB edge_leds[11];

volatile bool flag = false;
volatile bool click = false;
volatile bool amp = false;

int counter = 0;

bool buttons[16] = {false};

int16_t beep[UI_SAMPLERATE];
uint16_t beep_length = 0;

bool sd_card_detected = false;

struct Config {
  int edge_color_r = 0;
  int edge_color_g = 0;
  int edge_color_b = 50;
};

Config config;

void loadConfiguration(Config& config) {
  File file = SD.open("/config.txt");
  JsonDocument doc;

  DeserializationError error = deserializeJson(doc, file);
  if(error) Serial.println(F("Failed to read file, using default configuration"));

  config.edge_color_r = doc["edge"]["color"]["r"] | 0;
  config.edge_color_g = doc["edge"]["color"]["g"] | 0;
  config.edge_color_b = doc["edge"]["color"]["b"] | 50;
  
  //strlcpy(config.hostname, doc["hostname"] | "example.com", sizeof(config.hostname));

  file.close();
}

size_t count;
size_t tape_write = 0;

void codec_transmit() {
  // for(int i = 0; i < count; i++){
  //   buffer2[tape_write] = buffer[i];
  //   tape_write++;
  //   if(tape_write == ECHO) tape_write = 0;
    
  //   int tape_read = tape_write + 1;
  //   if(tape_read < 0) tape_read += ECHO;
  //   buffer[i] += buffer2[tape_read % ECHO];
  // }
  
  for(int i = 0; i < count; i++){
    int16_t sample = 0;
//    for(int j = 0; j < NSTREAMS; j++){
      int j = active % NSTREAMS;
      if(!ringbuffer[j].isEmpty()) sample = ringbuffer[j].pop();
      buffer[i] += (sample);
  //  }
  }
  
  i2s.write((const uint8_t *)&buffer, count * sizeof(int16_t));
}

void codec_receive(){
  count = i2s.read((uint8_t *)&buffer, SIZE * sizeof(int16_t)) * sizeof(uint32_t) / sizeof(int16_t);
  for(int i = 0; i < count; i++){
    buffer[i] *= -1;
  }
}

void pwm_audio_callback() {
    while (ui_snd.availableForWrite()) {
      if(click) {
        ui_snd.write(beep[counter]);
        counter++;
        if(counter == beep_length) {
          counter = 0;
          click = false;
        }
      } else {
        digitalWrite(7, LOW);
        ui_snd.write(0);
        counter = 0;
      }
    }
}

void tca_irq() {
  flag = true;
}

void setup() {
  //Serial.begin();
  //delay(1000);

  i2s.setFrequency(48000);

  for(int i = 0; i < NSTREAMS; i++){
    ringbuffer[i].setSize(RINGBUFFER);
  }

  FastLED.addLeds<NEOPIXEL, 4>(edge_leds, 11);
  FastLED.addLeds<NEOPIXEL, 5>(ui_leds, 74);

  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  digitalWrite(12, LOW);
  digitalWrite(13, LOW);

  pinMode(21, INPUT_PULLUP);
  sd_card_detected = !digitalRead(21);
  delay(500);

  bool sdInitialized = SD.begin(22, 23, 24);
  delay(100);
  if(!sdInitialized) sdInitialized = SD.begin(22, 23, 24); // hack to prevent SD card from not initializing after soft reset

  if(sdInitialized) loadConfiguration(config);

  if(sdInitialized && SD.exists("/click.wav")) {
    File click = SD.open("/click.wav");
    int click_bytes = 0;
    int16_t maxv = 0;
    while (click.available() || click_bytes == UI_SAMPLERATE-1) {
      int16_t sample = click.read() * 32;
      if(abs(sample) > maxv) maxv = sample;
      beep[click_bytes] = sample;
      click_bytes++;
    }
    click.close();
    
    beep_length = click_bytes;
    Serial.print("Read ");
    Serial.print(beep_length);
    Serial.print(" bytes from click.wav into beep array. maxV was ");
    Serial.println(maxv);
  } else {
    for(int i = 0; i < UI_SAMPLERATE; i++){
      float sine_pos = (2.0f * M_PI * 8000.0f * (float)i) / (float)UI_SAMPLERATE;
      beep[i] = (int16_t)(sin(sine_pos) * 8000.0f);
    }
    Serial.println("Synthesized 8kHz sine into beep array.");
    beep_length = UI_SAMPLERATE;
  }

  Serial.println("INIT WIRE");
  Wire1.setSDA(2);
  Wire1.setSCL(3);
  Wire1.begin();
  Serial.println("SUCCESS");

  Serial.println("INIT TCA");
  TCA.begin();
  TCA.pinMode16(0xFFFF);
  TCA.setPolarity16(0x0000);
  Serial.println("SUCCESS");

  Serial.println("INIT INTERRUPT");
  pinMode(1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(1), tca_irq, FALLING);
  Serial.println("SUCCESS");

  // Rotary Encoder
  ENC.begin();  //  set direction pin.
  ENC.setDirection(AS5600_COUNTERCLOCK_WISE);

  pinMode(6, OUTPUT);  // Vibration Motor
  pinMode(7, OUTPUT);  // UI Amp Enable

  ui_snd.onTransmit(pwm_audio_callback);
  ui_snd.begin(UI_SAMPLERATE);

  digitalWrite(7, LOW);

  pinMode(19, OUTPUT); // MCLK enable
  digitalWrite(19, HIGH); // enable MCLK

  pinMode(20, OUTPUT); // CODEC reset
  digitalWrite(20, HIGH);

  codec.begin(&Wire1);

  i2s.onTransmit(codec_transmit);
  i2s.onReceive(codec_receive);

  i2s.setDOUT(15);
  i2s.setDIN(14);
  i2s.setBCLK(16); // Note: LRCLK = BCLK + 1
  i2s.setMCLK(18);
  i2s.setMCLKmult(256); // 12.288.000Hz

  i2s.swapClocks();
  i2s.setBitsPerSample(16);

  i2s.setBuffers(6, SIZE * sizeof(int16_t) / sizeof(uint32_t));

  for(int i = 0; i < NSTREAMS; i++){
    ringbuffer[i].begin();
  }

  if(!i2s.begin(48000)){
    Serial.println("I2S error!");
    while(100);
  }

  if(sdInitialized) {
    for(int i = 0; i < NSTREAMS; i++){
      char filename[40];
      sprintf(filename, "/sound/%d.wav", i+1);
      if(SD.exists(filename)){
        streams[i] = SD.open(filename);
        streams[i].seek(44, SeekSet);
      } else {
        for(int k = 0; k < 3; k++){
          digitalWrite(6, HIGH);
          delay(20);
          digitalWrite(6, LOW);
          delay(50);
        }
      }
    }
    streams_loaded = true;
  }

  // if(!ringbuffer.isFull()){
  //   int cnt = 0;
  //   while (!ringbuffer.isFull() && cnt < RINGBUFFER) {
  //     int16_t sample = stream1.read();
  //     ringbuffer.push(sample);
  //     if(!stream1.available()) stream1.seek(48000*6, SeekSet); 
  //     cnt++;
  //   }
  // }


  digitalWrite(6, HIGH);
  delay(50);
  digitalWrite(6, LOW);
}

void loop() {
  position = ENC.getCumulativePosition();

  sd_card_detected = !digitalRead(21);
  if(sd_card_detected) edge_leds[8] = CRGB(0,10,0);
  if(!sd_card_detected) edge_leds[8] = CRGB(10,0,0);

  // for (int i = 0; i < NSTREAMS; i++) {
  //   if(!ringbuffer[i].isFull()){
  //     int cnt = 0;
  //     while (!ringbuffer[i].isFull() && cnt < 10000) {
  //       uint8_t samplebyte[2];
  //       streams[i].read(samplebyte, 2);
  //       if(!streams[i].available()) streams[i].seek(44, SeekSet);
        
  //       int16_t sample = (samplebyte[1] << 8) + samplebyte[0];
  //       ringbuffer[i].push(sample);
  //       cnt++;
  //     }
  //   }
  // }

  // EDGE LEDs
  for (int i = 0; i < 8; i++) {
    edge_leds[i] = CRGB(config.edge_color_r, config.edge_color_g, config.edge_color_b);
  }
  
  // LED Ring leeren
  for (int i = 0; i < 48; i++) {
    ui_leds[i + 3] = CRGB(0, 0, 0);
  }

  // Rotary button LEDs
  ui_leds[0] = CRGB(0, 0, 0);
  ui_leds[1] = CRGB(0, 0, 0);
  ui_leds[2] = CRGB(0, 0, 0);

  // flag = true when a button is pushed
  if (flag) {
    int val = TCA.read16();
    
    for(int i = 0; i < 16; i++){
      buttons[i] = ~(val >> i) & 0x01;
    }
    
    // Make vibration
    if (HAPTIC) {
      digitalWrite(6, HIGH);
      delay(50);
      digitalWrite(6, LOW);
    }

    // Make beep
    if (AURAL) {
      digitalWrite(7, HIGH);
      click = true;
    }

    // Flash encoder leds
    ui_leds[0] = CRGB(0, 100, 50);
    ui_leds[1] = CRGB(0, 100, 50);
    ui_leds[2] = CRGB(0, 100, 50);

    // Switch through LED matrix
    if(buttons[RIGHT]) active++;
    if(buttons[LEFT]) active--;
    if(active == 13) active = 0;
    if(active == -1) active = 12;
    
    flag = false;
  }

  // empty LED matrix
  for (int i = 0; i < 13; i++) {
    ui_leds[lut_matrix[i]] = CRGB(0, 0, 0);
  }
  
  // set active LED matrix LED
  ui_leds[lut_matrix[active]] = CRGB(100, 100, 100);

  if(active == 1) {
    digitalWrite(12, HIGH);
    digitalWrite(13, HIGH);
  } else {
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);
  }

  if(position < 0) position += 4096;
  active_led_ring = (position / 32) % 48;
  
  // set active LED ring LED
  for(int i = 0; i < active_led_ring; i++){
    ui_leds[lut_ring_ccw[i]] = CRGB(50, 0, 25);
  }

  FastLED.show();
  //delay(1); // wait 1ms
}

void setup1(){
  
}

void loop1(){
  if(streams_loaded) {
    for (int i = 0; i < NSTREAMS; i++) {
      if(!ringbuffer[i].isFull()){
        int cnt = 0;
        while (!ringbuffer[i].isFull() && cnt < 10000) {
          uint8_t samplebyte[2];
          streams[i].read(samplebyte, 2);
          if(!streams[i].available()) streams[i].seek(44, SeekSet);
          
          int16_t sample = (samplebyte[1] << 8) + samplebyte[0];
          ringbuffer[i].push(sample);
          cnt++;
        }
      }
    }
  }
}