#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#include <Adafruit_NeoPixel.h>
#include "Adafruit_BluefruitLE_SPI.h"

#if SOFTWARE_SERIAL_AVAILABLE
  #include <SoftwareSerial.h>
#endif

#include "BluefruitConfig.h"

#define REQUIRE_SERIAL false
#define BT_SCAN_MS     50
#define MIN_FIRMWARE   "0.7.0"
#define FACTORYRESET   true
#define DIM_FACTOR     80

#define SHIFTING_STOPPED 0
#define SHIFTING_FORWARD 1
#define SHIFTING_REVERSE 2

// Updated with  bluetooth connection state
volatile bool conn = false;

const char msg_pattern_off[] PROGMEM = "pattern off";
const char msg_pattern_rainbow[] PROGMEM = "pattern rainbow";
const char msg_pattern_invalid[] PROGMEM = "pattern invalid";
const char msg_speed_out_of_range[] PROGMEM = "out of range (0,200)";
const char msg_speed_changed[] PROGMEM = "speed changed";

// Interrupts are needed to TX without deadlocking, so instead of doing it
// inside another interrupt, save the MSG_ ID here for loop() to do it
volatile int to_rx = NULL;

Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

void bluetooth_setup(void){
  if ( !ble.begin(VERBOSE_MODE) ) {
    Serial.println(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
    while(1); // halt
  }
  if ( FACTORYRESET ) {
    if ( ! ble.factoryReset() ){
      Serial.println(F("Couldn't factory reset"));
      while(1); // halt
    }
  }
  if ( !ble.isVersionAtLeast(MIN_FIRMWARE) ){
    Serial.print(F("Callback requires at least"));
    Serial.println(F(MIN_FIRMWARE));
    while(1); // halt
  }
  pinMode(BLUEFRUIT_SPI_IRQ, INPUT_PULLUP);
  //ble.echo(false); // disable command echo
  //ble.verbose(false); // disable debug info
  ble.sendCommandCheckOK(F("AT+GAPDEVNAME=Suit LEDs")); // Change name displayed in bluetooth scans
  ble.sendCommandCheckOK(F("AT+HWMODELED=2")); // Show TX/RX activity on LED
  
  //ble.info();
  ble.setConnectCallback(on_connect);
  ble.setDisconnectCallback(on_disconnect);
  ble.setBleUartRxCallback(BleUartRX);
  ble.setMode(BLUEFRUIT_MODE_DATA);
}

void on_connect(void){
  // Status light will go solid blue because of AT+HWMODELED=2
  Serial.println(F("Bluetooth Connected"));
  conn = true;
}
void on_disconnect(void){
  // Blue status light will shut off because of AT+HWMODELED=2
  Serial.println(F("Bluetooth Disconnected"));
  conn = false;
}

void BleUartRX(char payload[], uint16_t payload_len){
  // Red status light should flicker because of AT+HWMODELED=2
  if (payload_len < 3){
    Serial.print("packet length ");
    Serial.print(payload_len);
    Serial.println(" is too short");
    return;
  }
  // first byte is the command
  uint8_t cmd_byte = payload[0];
  // last byte is the CRC
  uint8_t crc_byte = payload[payload_len-1];
  // all other bytes are the data for that command
  uint16_t data_len = payload_len-2;
  uint8_t data[data_len];
  uint16_t payload_index;
  uint16_t data_index;
  for (payload_index=1, data_index=0; payload_index<payload_len-1; payload_index++, data_index++){
    data[data_index] = payload[payload_index];
  }
  // Print parts
  Serial.print(F("[RX] cmd=" ));
  Serial.print((char)cmd_byte);
  Serial.print(F(" crc="));
  Serial.print(crc_byte);
  Serial.print(F(" data="));
  Serial.write(data, data_len);
  Serial.println();
  // check CRC
  uint8_t crc = 0;
  for (uint16_t i=0; i<payload_len-1; i++){
    crc += payload[i];
  }
  crc = ~crc;
  if (crc_byte != crc){
    Serial.print("CRC mismatch; expected ");
    Serial.println(crc);
    return;
  }
  // run whichever command was requested
  if (cmd_byte == 's'){
    set_wait(data, data_len);
  } else if (cmd_byte == 'p') {
    set_pattern(data, data_len);
  } else if (cmd_byte == 'a') {
    set_amplitude(data, data_len);
  } else {
    Serial.println(F("Unrecognized cmd"));
  }
}

class Strip {
  private:
  uint8_t pin;
  uint8_t num_pixels;
  unsigned long last;
  uint8_t offset = num_pixels;
  uint8_t shifting = SHIFTING_FORWARD;
  Adafruit_NeoPixel pixel;

  public:
  uint16_t wait;
  char pattern;
  uint8_t amplitude = 0;

  Strip(uint8_t led_pin, uint8_t strip_len) {
    pattern = 'r';
    pin = led_pin;
    num_pixels = strip_len;
    wait = 0;
    last = millis();
    offset = 0;
    pixel = Adafruit_NeoPixel(num_pixels, pin, NEO_GRBW + NEO_KHZ800);
  }

  void off(void){
    pixel.begin();
    for(uint8_t i=0; i<num_pixels; i++){
      pixel.setPixelColor(i, pixel.Color(0,0,0));
    }
    pixel.show();
  }

  void set_shift_direction(uint8_t new_shift){
    if (new_shift == SHIFTING_FORWARD){
      // FIXME: convert offset
    } else if (new_shift == SHIFTING_REVERSE){
      // FIXME: convert offset
    } else {
      offset = 0;
    }
    shifting = new_shift;
  }

  void update(void){
    unsigned long now = millis();
    if (now - wait > last){
      last = now;
      add_offset();
    }
    if (pattern == 'o'){
      off();
    } else {
      rainbow();
    }
  }

  private:
  void add_offset(void){
    /* Increments the offset counter. See get_offset_loc() */
    offset++;
    if (offset == num_pixels){
      offset = 0;
    }
  }
  uint8_t get_offset_loc(uint8_t index){
    /* index is the location on the strip where a pixel would be if offset
     *  was 0. Offset will be from 0 to num_pixels-1 */
    if (offset == 0){
      return index;
    }
    if (shifting = SHIFTING_FORWARD){
      uint16_t loc = offset + index;
      if (loc >= num_pixels){
        loc -= num_pixels;
      }
      return (uint8_t)loc;
    } else if (shifting = SHIFTING_REVERSE){
      uint16_t loc = num_pixels - offset + index;
      if (loc >= num_pixels){
        loc -= num_pixels;
      }
      return (uint8_t)loc;
    }
    return index; // SHIFTING_STOPPED
  }
  void rainbow(void){
    uint8_t r = 255;
    uint8_t g = 0;
    uint8_t b = 0;
    pixel.begin();
    for (uint8_t index=0; index<num_pixels; index++){
      pixel.setPixelColor(get_offset_loc(index), pixel.Color(r/DIM_FACTOR, g/DIM_FACTOR, b/DIM_FACTOR));
      if (index < 24){ // red -> yellow shifting
        g += 10;
      } else if (index < 48){ // yellow -> green shifting
        if (index == 24){
          g = 255;
        }
        r -= 10;
      } else if (index < 72){ // green -> cyan shifting
        if (index == 48){
          r = 0;
        }
        b += 10;
      } else if (index < 96){ // cyan -> blue shifting
        if (index == 72){
          b = 255;
        }
        g -= 10;
      } else if (index < 120){ // blue -> magenta shifting
        if (index == 96){
          g = 0;
        }
        r += 10;
      } else { // magenta -> red shifting
        if (index == 120){
          r = 255;
        }
        b -= 10;
      }
    }
    pixel.show();
  }
};

volatile Strip strip = Strip(6, 144);

void set_wait(char data[], uint16_t len){
  String wait = "";
  for (uint16_t index=0; index<len; index++){
    wait += (char)data[index];
  }
  uint16_t wait_int = wait.toInt();
  if (wait_int >= 0 && wait_int <= 200){
    strip.wait = wait_int;
    to_rx = msg_speed_changed;
  } else {
    to_rx = msg_speed_out_of_range;
  }
}

void set_pattern(char data[], uint16_t len){
  if (len != 1){
    to_rx = msg_pattern_invalid;
  } else if (data[0] == 'r'){
    strip.pattern = 'r';
    to_rx = msg_pattern_rainbow;
  } else if (data[0] == 'o') {
    strip.pattern = 'o';
    to_rx = msg_pattern_off;
  } else {
    to_rx = msg_pattern_invalid;
  }
}

void set_amplitude(char data[], uint16_t len){
  if (len == 1){
    strip.amplitude = data[0];
    //Serial.println((uint8_t)data[0]);
    Serial.println(255 - (uint8_t)data[0]);
  } else {
    Serial.println(F("invalid len for amplitude"));
  }
}

void do_rx_send(void){
  /* if to_rx is set to a message, try to send it over bluetooth */
  if (to_rx == NULL){
    return;
  }
  char* msg = flash_msg(to_rx);
  to_rx = NULL;
  if (conn){
    Serial.print(F("[TX] "));
    Serial.println(msg);
    ble.write(msg);
  } else {
    Serial.print(F("Disconnected. Couldn't TX: "));
    Serial.println(msg);
  }
  free(msg);
}

char * flash_msg(const char* addr){
  /* return a char* array from flash memory; be sure to free() it after */
  uint8_t buff_size = strlen_P(addr);
  char * buffer = (char *) malloc (buff_size);
  for (uint8_t index=0; index < buff_size; index++){
    buffer[index] = pgm_read_byte_near(addr + index);
  }
  return buffer;
}

void setup(void) {
  if (REQUIRE_SERIAL){
    while (!Serial);
    Serial.begin(115200);
  }
  bluetooth_setup();
  strip.wait = 50;
  strip.off();
  Serial.println(F("Ready"));
}

void loop(void) {
  ble.update(BT_SCAN_MS);
  do_rx_send();
  strip.update();
}