#include <DigiUSB.h>
#include <TinyWireM.h>
#include <EEPROM.h>

#define I2C_EXPANDER_ADDR 0x20
#define I2C_BLINKM_ADDR 0x09
#define EXPANDER_BUTTONS 1

// When panel switch is in "read" position. ("zero" == jitter)
// 0 - pot controlled, 1 = USB controlled, 2 = adjust hue, (3 == cycle colors?)
byte readMode;
byte ledHue;

byte expanderStatus = B11111111; // all high, default power-on state.

#define DEBOUNCE_INTERVAL_MS 50
unsigned long buttonDebounceTime[EXPANDER_BUTTONS];

#define DEBOUNCE_STABLE_VAL 0x1
#define DEBOUNCE_LAST_VAL   0x2
#define DEBOUNCE_IS_STABLE  0x4
byte buttonState[EXPANDER_BUTTONS];
/*
 * Button debouncing. The DEBOUNCE_STABLE_VAL is the bit representing the debounced
 * value of the button. Consumers should check buttonState[i] & DEBOUNCE_STABLE_VAL.
 * DEBOUNCE_LAST_VAL is the last raw (unbounced) value read for the button. When the
 * DEBOUNCE_IS_STABLE bit is set, the raw state hasn't changed within the last
 * debounce interval milliseconds. When clear, it has, and we're waiting for it to
 * stablize (i.e., remain unchanged for a whole debounce interval number of ms).
 */
byte pinForButton[EXPANDER_BUTTONS];



byte blinkmCommand(byte *cmd, byte len) {
  byte err;
  unsigned int sendCount = 0;
  
  do {
    //digitalWrite(1, HIGH);
    TinyWireM.beginTransmission(I2C_BLINKM_ADDR);
    for (byte i = 0; i < len; i++)
      TinyWireM.send(cmd[i]);
    err = TinyWireM.endTransmission();
    //digitalWrite(1, LOW);
    sendCount++;
    for (int foo = 10; foo; foo--); // tiny delay
  } while (err && sendCount < 50);
  
  //DigiUSB.refresh();
  //usb_report_int("# sends: ", sendCount);
  
  return err;
}

void resetEverything() {
  EEPROM.write(0, 0);   // mode
  EEPROM.write(1, 255); // R
  EEPROM.write(2, 6);   // G
  EEPROM.write(3, 0);   // B (color we boot-fade to below)
 
  // setup BlinkM
  // Makes script 0 fade from off (black) to reddish-orange

  // stop any running script
  byte stopScript[] = { 'o' };
  blinkmCommand(stopScript, sizeof(stopScript));
  
  // Autoplay script 0, once, fade speed = 2, time adj = 0
  byte bootMode[] = {'B', 1, 0, 1, 2, 0 };
  blinkmCommand(bootMode, sizeof(bootMode));
  delay(40);
  
  // Script 0 line 0
  // writecmd, script #, line# (0-49), duration, cmd, arg1, arg2, arg3
  byte script0[] = {'W', 0, 0, 10, 'n', 0, 0, 0 }; // immediate rgb(0)
  blinkmCommand(script0, sizeof(script0));
  delay(40);
  
  // Script 0 line 1
  byte script1[] = {'W', 0, 1, 48, 'c', 255, 2, 0 }; // fade to mostly red
  blinkmCommand(script1, sizeof(script1));
  delay(40); 
  
  // Script 0 line 2
  byte script2[] = {'W', 0, 2, 100, 'c', 255, 6, 0 }; // fade to final red-orange
  blinkmCommand(script2, sizeof(script2));
  delay(40);
  
  // Set script 0 length to 3 lines, run once.
  byte setScriptLength[] = { 'L', 0, 3, 1 };
  blinkmCommand(setScriptLength, sizeof(setScriptLength));
  delay(40);

  // To indicate programming is done (and that we were programming!)
  byte greenNow[] = { 'n', 0x20, 0xff, 0x00 };
  blinkmCommand(greenNow, sizeof(greenNow));
}

void setup() {
  pinMode(5, INPUT); // P5 for analog input, uses analogRead(0);
  pinMode(1, OUTPUT); // onboard LED, debug with digitalWrite(1, HIGH);
  TinyWireM.begin();
  DigiUSB.begin();
  initButtons();
  
  // re-init EEPROM and BlinkM boot mode
  //resetEverything();

  readMode = EEPROM.read(0);
  
  // Set configured blinkm color. We fade to reddish-orange on boot (should be
  // done by the time we get here), and then fade to the last stored color.
  byte ledColor[] = { 'c', 0, 0, 0 };
  ledColor[1] = EEPROM.read(1); // R
  ledColor[2] = EEPROM.read(2); // G
  ledColor[3] = EEPROM.read(3); // B
  blinkmCommand(ledColor, sizeof(ledColor));
}

unsigned int potValue = 0; // 0-1024
unsigned int lastPotValue = 1024;

void loop() {
  DigiUSB.refresh();
  
  byte potChanged = 0;
  potValue = analogRead(0);
  if (potValue != lastPotValue) {
    potChanged = 1;
    lastPotValue = potValue;
  }

  scanButtons();
  
  // Check the front panel ZERO/READ switch (is respectively 1/0)
  // XXX expander's state can be a little confusing? buttons should set pins to low when pressed?
  byte isZeroMode = buttonState[0] & DEBOUNCE_STABLE_VAL;

  // Switch controls meter mode -- "ZERO" for jitter or "READ" for value from pot ("ZERO ADJ.")
  if (isZeroMode) {
    byte valMap[] = { 0, 2, 4, 5, 5, 6, 6, 7, 7, 7, 7, 8, 8, 9, 11, 15, 20 };
    byte val = valMap[random(sizeof(valMap))] * 2;
    /*
    // This also works well...
    byte val = 5 + random(15);
    // bias it lower, with a couple rare big jumps.
    if (val <= 9)
      val = 0;
    else if (val == 10)
      val = 5;
    else if (val == 19)
      val = 25;
    else if (val == 20)
      val = 30;
    */
    analogWrite(1, val);
    
    if (potChanged) {
      if (potValue > 512) {
        // LEDs on
        expanderWrite(5, LOW);
        expanderWrite(6, LOW);
      } else {
        // LEDs off
        expanderWrite(5, HIGH);
        expanderWrite(6, HIGH);
      }
    }

  } else { /* !isZeroMode */
    /*
     * Colorfade mode. Pot controls hue.
     */
    analogWrite(1, potValue / 4);

    if (potChanged) {
       // Immediate fade time. This actually doesn't seem to become laggy until ~64. But neither does
       // setting some fade time make the adjustments feel smoother.
       byte fadeFast[] = { 'f', 255 };
       blinkmCommand(fadeFast, sizeof(fadeFast));
       byte ledColor[] = { 'h', 0, 255, 255 }; // H,S,B
       ledColor[1] = potValue / 4;
       blinkmCommand(ledColor, sizeof(ledColor));
     }
  }
  

  
/*
  // Pot always controls meter, switch turns white LED on/off
  // pin5 is the left LED, pin6 is the right.
  // XXX with both LEDs on, meter deflection is:
  // * at ~0, barely noticible (maybe 1/10 a division?)
  // * at ~5, about 80% of a division lower
  // * at ~10, about 120% of a division lower
  // Oh, and that's with the RGB let at the default "orange" setting.
  // With just the RGB between 000/fff, the swing is about 90% of a div lower at ~10,
  // about 50% at ~5, and barely noticible at ~0.
  // With both white LEDs + RGB led, swing is still barely noticible at ~0,
  // 100% of a div lower at ~5, and 200% (2 divs) lower at ~10.
  byte ledColor[] = { 'n', 0, 0, 0 };
  analogWrite(1, potValue / 4);
  if (isZeroMode) {
    // LEDs on
    expanderWrite(5, LOW);
    expanderWrite(6, LOW);
    ledColor[1] = ledColor[2] = ledColor[3] = 255;
  } else {
    // LEDs off
    expanderWrite(5, HIGH);
    expanderWrite(6, HIGH);
    ledColor[1] = ledColor[2] = ledColor[3] = 0;
  }
  blinkmCommand(ledColor, sizeof(ledColor));
*/

  delay(20);
}



void initButtons() {
  for (int i = 0; i < EXPANDER_BUTTONS; i++) {
    buttonState[i] = 0x7; // isStable, last == stable == HIGH
    buttonDebounceTime[i] = 0;
  }
  pinForButton[0] = 7; // button 0 is expander pin 7
  // Future:
  // pinForButton[1] = 3; // button 1 is expander pin 3
  // pinForButton[2] = 4; // button 2 is expander pin 4
  // ...
}

void scanButtons() {
  for (int i = 0; i < EXPANDER_BUTTONS; i++) {
    
    byte currVal = expanderRead(pinForButton[i]); // read pin
    byte lastVal = (buttonState[i] & DEBOUNCE_LAST_VAL) >> 1;

    // Has unbounced state changed since we last read it?
    if (currVal == lastVal) {
      // Nope. So we're either already stable (common case), or we need
      // to see if we're done waiting for stabilization.
      if (buttonState[i] & DEBOUNCE_IS_STABLE)
        continue;

      unsigned long currTime = millis();
      // XXX should deal with timer overflow

      if (currTime < buttonDebounceTime[i])
        continue;

      // Mark as stable, and set stable state to current value.  
      buttonState[i] |= DEBOUNCE_IS_STABLE;
      if (currVal)
        buttonState[i] |= DEBOUNCE_STABLE_VAL;
      else
        buttonState[i] &= ~DEBOUNCE_STABLE_VAL;

    } else {
      // The button has changed unbounced state, so flag it as unstable and
      // start waiting for it to stabilize.
      buttonState[i] &= ~DEBOUNCE_IS_STABLE;
      
      if (currVal)
        buttonState[i] |= DEBOUNCE_LAST_VAL;
      else
        buttonState[i] &= ~DEBOUNCE_LAST_VAL;
        
      buttonDebounceTime[i] = millis() + DEBOUNCE_INTERVAL_MS;
    }
  }
}

void expanderWrite(byte pinNumber, boolean state){
  /*
   * N.B. I swapped HIGH/LOW here (compared to the Digistump sample code,
   * because it didn't make sense. I _think_ they might have been trying
   * to hide that it's designed to sink current, and so to run turn on a
   * LED you can use HIGH as normal, even though it's really being set LOW.
   */
  if(state == LOW)
    expanderStatus &= ~(1 << pinNumber);
  else
    expanderStatus |= (1 << pinNumber); 
    
  expanderWrite(expanderStatus);
}

void expanderWrite(byte _data ) {
  TinyWireM.beginTransmission(I2C_EXPANDER_ADDR);
  TinyWireM.send(_data);
  TinyWireM.endTransmission();
}

byte expanderRead(byte pinNumber) {
  //usb_report_int("reading pin ", pinNumber);
  byte readStatus = expanderRead();
  readStatus &= (1 << pinNumber);
  if (readStatus)
    return HIGH;
  return LOW;
}

byte expanderRead() {
  // Note, the expander docs talk about needing to use addr|0x01, but that's
  // just the normal I2C R/W bit, so requestFrom handles that for us.
  TinyWireM.requestFrom(I2C_EXPANDER_ADDR, 1); // request 1 byte
  byte _data = TinyWireM.receive();
  return _data;
}

/*
 * host needs to run 'digiterm'
 *
 * Can do so at any point (not required at startup)
 */
void usb_report_int(char *header, unsigned int val) {
  DigiUSB.refresh();
  DigiUSB.print(header);
  /* Reports the given int as an ascii value */
  DigiUSB.println(val, DEC);
  DigiUSB.delay(100);
}

void usb_echo_input() {
  DigiUSB.println("Waiting for input...");
  
  int lastRead;
  // when there are no characters to read, or the character isn't a newline
  while (true) { // loop forever
    if (DigiUSB.available()) {
      // something to read
      lastRead = DigiUSB.read();
      DigiUSB.write(lastRead);
      
      if (lastRead == '\n') {
        break; // when we get a newline, break out of loop
      }
    }
    
    // refresh the usb port for 10 milliseconds
    DigiUSB.delay(100);
  }
}