#include "MIDIUSB.h"
#include <SD.h>
#include <Wire.h>

#include "main.h"
#include "config.h"
#include "engine.h"

int led = 13;
int g_reading_config = 0;
int g_writing_config = 0;
//Sd2Card card;
char g_revision;

enum {
  MODE_STANDALONE = 0,
  MODE_REPORTPOINTS = 1,
  MODE_REPORTPOINTS_PLAY = 2
} g_mode = MODE_STANDALONE;

uint32_t now() {
  return millis();
}

void setup_pwm() {
  // Set up the generic clock (GCLK4) used to clock timers
  REG_GCLK_GENDIV = GCLK_GENDIV_DIV(1) |          // Divide the 48MHz clock source by divisor 2: 48MHz/2=24MHz
                    GCLK_GENDIV_ID(4);            // Select Generic Clock (GCLK) 4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
                     GCLK_GENCTRL_GENEN |         // Enable GCLK4
                     GCLK_GENCTRL_SRC_DFLL48M |   // Set the 48MHz clock source
                     GCLK_GENCTRL_ID(4);          // Select GCLK4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  // Feed GCLK4 to TC4 and TC5
  REG_GCLK_CLKCTRL = GCLK_CLKCTRL_CLKEN |         // Enable GCLK4 to TC4 and TC5
                     GCLK_CLKCTRL_GEN_GCLK4 |     // Select GCLK4
                     GCLK_CLKCTRL_ID_TC4_TC5;     // Feed the GCLK4 to TC4 and TC5
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  REG_TC4_CTRLA |= TC_CTRLA_MODE_COUNT8;          // Set the counter to 8-bit mode
  while (TC4->COUNT8.STATUS.bit.SYNCBUSY);        // Wait for synchronization 

  REG_TC4_COUNT8_CC0 = 0;                         // Set the TC4 CC0 register to some arbitary value
  while (TC4->COUNT8.STATUS.bit.SYNCBUSY);        // Wait for synchronization
  REG_TC4_COUNT8_CC1 = 0;                         // Set the TC4 CC1 register to some arbitary value
  while (TC4->COUNT8.STATUS.bit.SYNCBUSY);        // Wait for synchronization
  REG_TC4_COUNT8_PER = 1;                         // Set the PER (period) register to flip each time
  while (TC4->COUNT8.STATUS.bit.SYNCBUSY);        // Wait for synchronization

  NVIC_DisableIRQ(TC4_IRQn);
  NVIC_ClearPendingIRQ(TC4_IRQn);
  // NVIC_SetPriority(TC4_IRQn, 0);    // Set the Nested Vector Interrupt Controller (NVIC) priority for TC4 to 0 (highest)
  // NVIC_EnableIRQ(TC4_IRQn);         // Connect TC4 to Nested Vector Interrupt Controller (NVIC) 

  REG_TC4_INTFLAG |= TC_INTFLAG_MC1 | TC_INTFLAG_MC0 | TC_INTFLAG_OVF;        // Clear the interrupt flags
  //REG_TC4_INTENSET = TC_INTENSET_MC1 | TC_INTENSET_MC0 | TC_INTENSET_OVF;    // Enable TC4 interrupts
  REG_TC4_INTENCLR = TC_INTENCLR_MC1 | TC_INTENCLR_MC0 | TC_INTENCLR_OVF;     // Disable TC4 interrupts

  int outpin = 15;

  // Enable the port multiplexer for the digital pin D0 
  PORT->Group[g_APinDescription[outpin].ulPort].PINCFG[g_APinDescription[outpin].ulPin].bit.PMUXEN = 1;

  // Connect the TC4 timer to the port output D0 - port pins are paired odd PMUXO and even PMUXE
  // Peripheral E specifies the TC timers, on this pin: TC4
  PORT->Group[g_APinDescription[outpin].ulPort].PMUX[g_APinDescription[outpin].ulPin >> 1].reg |= /*PORT_PMUX_PMUXO_E |*/ PORT_PMUX_PMUXE_E;

  REG_TC4_CTRLA |= TC_CTRLA_PRESCALER_DIV1  |     // Set prescaler to 1, 24MHz/1 = 24MHz
                   TC_CTRLA_ENABLE;               // Enable TC4
  while (TC4->COUNT8.STATUS.bit.SYNCBUSY);        // Wait for synchronization
}

static void set_cam_register(uint8_t r, uint8_t v) {
  Wire.beginTransmission(0x58);
  Wire.write(r);
  Wire.write(v);
  Wire.endTransmission();
}

static void setup_cam() {
  digitalWrite(g_revision == 2 ? 16 : 19, HIGH);   // Alte Harfe: port 19/A5/PB02/Pin 47
                                                   // Neue Harfe: port 16/A2/PB09/Pin 8
  set_cam_register(0x30, 0x01);
  set_cam_register(0x06, 0x90);
  set_cam_register(0x08, 0xc0);
  set_cam_register(0x1a, 0x40);
  set_cam_register(0x33, 0x01);
  set_cam_register(0x30, 0x08);
}

void read_config() {
  config_reset();
  if (!SD.begin(4)) {
    Serial.println("initialization failed!");
    return;
  }
  File configfile = SD.open("laserhar.cfg", FILE_READ);
  if (!configfile) {
    Serial.println("opening config failed");
    return;
  }

  char command[256];
  int command_len = 0;
  int skip_command = 0;

  while (configfile.available()) {
    char c = command[command_len++] = configfile.read();
    if (c=='\n') {
        if (!skip_command) {
            command[command_len] = 0;
            // Serial.write(command);
            config_handle_line(command);
        }
        skip_command = 0;
        command_len = 0;
    }
    if (command_len==sizeof(command)) {
      // If we end up at end of buffer, ignore everying to next \n
      skip_command = 1;
      command_len = 0;
    }
  }

  configfile.close();
}

void write_config() {
  Serial.println("- removing config file");
  SD.remove("laserhar.cfg");
  Serial.println("- ... done, open new config file");
  File configfile = SD.open("laserhar.cfg", FILE_WRITE);
  if (!configfile) {
    Serial.println("- opening config failed");
    return;
  }
  Serial.println("- ... done, writing globals");

  char text[256];
  size_t len = config_dumpglobals(text, sizeof(text));
  configfile.write(text, len);
  Serial.println("- ... done, writing globals");
  for (int i=0; i< g_string_count; ++i) {
      len = config_dumpstring(i, text, sizeof(text));
      configfile.write(text, len);
  }
  Serial.println("-- DONE");
  configfile.close();
}

void flashy_death() {
  while(1) {
    digitalWrite(led, LOW);   // turn the LED on (HIGH is the voltage level)
    delay(500);
    digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
    delay(500);
  }
}

void setup() {
  pinMode(5, INPUT_PULLUP);
  delay(10);
  g_revision = digitalRead(5) == LOW ? 2 : 1;

  Serial.begin(115200);
  pinMode(led, OUTPUT);

  Wire.begin();
  digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
  setup_pwm();

  // Let PWM settle for a bit
  delay(100);
  setup_cam();
  delay(1000);
  read_config();
  digitalWrite(led, LOW);   // turn the LED on (HIGH is the voltage level)
}

void handle_wire() {
  uint8_t ic2_result[32];
  char text[64];

  Wire.beginTransmission(0x58);
  Wire.write(0x36);
  Wire.endTransmission();
  Wire.requestFrom(0x58, 11);

  for (int i=0; Wire.available() && i<=11; ++i)
    ic2_result[i] = Wire.read();

  LPoint p1, p2, p3, p4;

  int Ix1,Iy1,Ix2,Iy2;
  int Ix3,Iy3,Ix4,Iy4;
  int s;

  s   = ic2_result[3];
  p1.x = ((int)ic2_result[1]) | ( ( s << 4) & 0x300);
  p1.y = ((int)ic2_result[2]) | ( ( s << 2) & 0x300);
  p2.x = ((int)ic2_result[4]) | ( ( s << 8) & 0x300);
  p2.y = ((int)ic2_result[5]) | ( ( s << 6) & 0x300);

  s   = ic2_result[8];
  p3.x = ((int)ic2_result[6]) | ( ( s << 4) & 0x300);
  p3.y = ((int)ic2_result[7]) | ( ( s << 2) & 0x300);
  p4.x = ((int)ic2_result[9]) | ( ( s << 8) & 0x300);
  p4.y = ((int)ic2_result[10])| ( ( s << 6) & 0x300);

  if (p1.x==1023 && p2.x==1023 && p3.x==1023 && p4.x==1023)
    return;

  uint32_t now = millis();
  if (g_mode==MODE_STANDALONE || g_mode==MODE_REPORTPOINTS_PLAY) {
    if (p1.x!=1023)
      engine_handle_point(&p1, now);
    if (p2.x!=1023)
      engine_handle_point(&p2, now);
    if (p3.x!=1023)
      engine_handle_point(&p3, now);
    if (p4.x!=1023)
      engine_handle_point(&p4, now);
  }

  if (g_mode==MODE_STANDALONE)
    return;
  if (p1.x!=1023)
    Serial.write(text, sprintf(text, "%04d:%04d ", p1.x, p1.y));
  if (p2.x!=1023)
    Serial.write(text, sprintf(text, "%04d:%04d ", p2.x, p2.y));
  if (p3.x!=1023)
    Serial.write(text, sprintf(text, "%04d:%04d ", p3.x, p3.y));
  if (p4.x!=1023)
    Serial.write(text, sprintf(text, "%04d:%04d ", p4.x, p4.y));
  Serial.println("");
  delay(5);
}

/* Do a fast nibble to hex representation conversion */
static unsigned char fromhex(unsigned char x) {
  x-='0'; if( x<=9) return x;
  x&=~0x20; x-='A'-'0';
  if( x<6 ) return x+10;
  return 0xff;
}

void handle_midi(char *command) {
//  midiEventPacket_t noteOn = {0x09, 0x90 | channel, pitch, velocity};
  uint8_t m[6];
  /* expect 6 hex encoded MIDI bytes and a newline */
  for (int i=0; i<6; ++i)
    if ((m[i] = fromhex(command[i])) == 0xff)
        return;
  if (command[6] != '\n')
    return;
  midiEventPacket_t p = { m[0], (m[0]<<4) | m[1], (m[2]<<4) | m[3], (m[4]<<4) | m[5] };
  MidiUSB.sendMIDI(p);
  MidiUSB.flush();
  Serial.println("- MIDI SENT");
}

void midi_playnote(int channel, int note, int octave_offset, int velocity ) {
  midi_pitchbend(channel, 0);
  midiEventPacket_t p = { 0x9, 0x90 | channel, note + 12 * octave_offset, velocity };
  MidiUSB.sendMIDI(p);
}

void midi_silencenote( int channel, int note, int octave_offset ) {
  midiEventPacket_t p = { 0x8, 0x80 | channel, note + 12 * octave_offset, 0x7f };
  MidiUSB.sendMIDI(p);
}

void midi_pitchbend( int channel, int pitch ) {
  pitch += 8192;
  if (pitch < 0) pitch = 0;
  if (pitch > 16383) pitch = 16383;

  midiEventPacket_t p = { 0xe, 0xe0 | channel, 0x7f & pitch, 0x7f & (pitch>>7) };
  MidiUSB.sendMIDI(p);
}

void handle_command(char *command) {
  char text[512];
  size_t len;
//  Serial.write( text, sprintf(text, "- %s\n", command));

  if (g_reading_config ) {
    if ( !memcmp(command, "-- DONE", 7)) {
        Serial.println("- finished import");
        g_reading_config = 0;
        return;
    }
    config_handle_line(command);
    delay(10);
    return;
  }

  switch (*command) {
    case 'M': /* Getting MIDI instruction */
        handle_midi(command+1);
        break;
    case 'S': /* Getting set mode instruction */
        switch (command[1]) {
          case '1':
            g_mode = MODE_STANDALONE;
            break;
          case '2':
            g_mode = MODE_REPORTPOINTS_PLAY;
            break;
          case '3':
            g_mode = MODE_REPORTPOINTS;
            break;
        }
        Serial.println(g_revision == 2 ? "revision 2" : "revision 1");
        break;
    case 'C': /* Import a single config line from host */
        config_handle_line(command+1);
        break;
    case 'E': /* Export config from host, import here */
        config_reset();
        Serial.println("- export from host triggered");
        delay(100);
        g_reading_config = true;
        break;
    case 'I': /* Import config at host */
        delay(1000);
        len = config_dumpglobals(text, sizeof(text));
        Serial.write(text, len);
        for (int i=0; i< g_string_count; ++i) {
            len = config_dumpstring(i, text, sizeof(text));
            delay(100);
            Serial.write(text, len);
        }
        delay(100);
        Serial.println("-- DONE");
        break;
    case 'R': /* Re-read config */
        read_config();
        break;
    case 'W': /* Write local config */
        write_config();
        break;
    default:
        break;
  }
}

void handle_serial() {
  static char command[512];
  static int command_len;
  static int skip_command;

  while (Serial.available()) {
    char c = command[command_len++] = Serial.read();
    if (c=='\n') {
        if (!skip_command) {
            command[command_len] = 0;
            handle_command(command);
        }
        skip_command = 0;
        command_len = 0;
        // Read at max one command to allow polling the cam
        return;
    }
    if (command_len==sizeof(command)) {
      // If we end up at end of buffer, ignore everying to next \n
      skip_command = 1;
      command_len = 0;
    }
  }
}

void loop() {
  static int led_state;

  handle_wire();
  handle_serial();

/*
  Serial.write((const uint8_t*)"Sending note on\r\n", 17);
  noteOn(0, note, 64);   // Channel 0, middle C, normal velocity
  MidiUSB.flush();
  delay(500);
  digitalWrite(led, LOW);   // turn the LED on (HIGH is the voltage level)

  Serial.write((const uint8_t*)"Sending note off\r\n", 18);
  noteOff(0, note, 64);  // Channel 0, middle C, normal velocity
  MidiUSB.flush();
  delay(1500);
*/

  engine_checksilence(millis());
  digitalWrite(led, ++led_state & 0xff < 0x7f ? HIGH : LOW);   // turn the LED on (HIGH is the voltage level)
}