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

#include "math.h"

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

#include "BluefruitConfig.h"

#define FACTORYRESET_ENABLE 1
#define DIM_FACTOR          50

uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
float parsefloat(uint8_t *buffer);
void printHex(const uint8_t * data, const uint32_t numBytes);

// the packet buffer
extern uint8_t packetbuffer[];

Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

void bluefruit_setup(){
  Serial.print(F("Initialising the Bluefruit LE module: "));
  if ( !ble.begin(VERBOSE_MODE) ) {
    Serial.println(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
    while(1); // halt
  }
  Serial.println( F("OK!") );
  if ( FACTORYRESET_ENABLE ) {
    /* Perform a factory reset to make sure everything is in a known state */
    Serial.println(F("Performing a factory reset: "));
    if ( ! ble.factoryReset() ){
      Serial.println(F("Couldn't factory reset"));
      while(1); // halt
    }
  }
  ble.println(F("AT+GAPDEVNAME=Suit LEDs"));
  ble.echo(false); // disable command echo
  ble.verbose(false); // disable debug info
  //Serial.println("Requesting Bluefruit info:");
  //ble.info();
}

void bluetooth_wait(){
  Serial.println(F("Waiting for bluetooth to connect..."));
  while (! ble.isConnected()) {
    delay(500);
  }
  Serial.println(F("Connected"));
  ble.setMode(BLUEFRUIT_MODE_DATA);
  //Serial.println(F("Connected. Switched to DATA mode."));
}

uint32_t pack_color(uint8_t r, uint8_t g, uint8_t b) {
  //r = r >> 6;
  //g = g >> 6;
  //b = b >> 6;
  return ((uint32_t)r << 16) | ((uint32_t)g <<  8) | b;
}
uint32_t pack_color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
  return ((uint32_t)w << 24) | ((uint32_t)r << 16) | ((uint32_t)g <<  8) | b;
}
uint8_t red(uint32_t color){
  return (color >> 16) & 0xFF;
}
uint8_t green(uint32_t color){
  return (color >> 8) & 0xFF;
}
uint8_t blue(uint32_t color){
  return color & 0xFF;
}
uint8_t white(uint32_t color){
  return (color >> 24) & 0xFF;
}

/* jesus fuck.. save my eyes */
/*uint32_t dim_color(uint32_t color){
  return pack_color(
    red(color) / DIM_FACTOR,
    green(color) / DIM_FACTOR,
    blue(color) / DIM_FACTOR,
    white(color) / DIM_FACTOR
  );
}*/


class Strip {
  private:
  uint8_t pin;
  uint8_t num_pixels;
  uint16_t wait;
  unsigned long last;
  uint8_t offset;
  Adafruit_NeoPixel pixel;
  /*uint32_t pattern[6] = {
    pack_color(255, 0,   0),   //   0 degrees, red
    pack_color(255, 255, 0),   //  60 degrees, yellow
    pack_color(0,   255, 0),   // 120 degrees, green
    pack_color(0,   255, 255), // 180 degrees, cyan
    pack_color(0,   0,   255), // 240 degrees, blue
    pack_color(255, 0,   255)  // 300 degrees, magenta
  };*/

  public:
  Strip(uint8_t led_pin, uint8_t strip_len, uint16_t wait_ms) {
    pin = led_pin;
    num_pixels = strip_len;
    wait = wait_ms;
    last = millis();
    offset = 0;
    pixel = Adafruit_NeoPixel(num_pixels, pin, NEO_GRBW + NEO_KHZ800);
  }

  public:
  void off(){
    Serial.println(F("off()"));
    pixel.begin();
    for(uint8_t i=0; i<num_pixels; i++){
      pixel.setPixelColor(i, pack_color(0,0,0));
    }
    pixel.show();
  }

  /*private:
  uint32_t avg_bias(uint32_t col1, uint32_t col2, float perc){
    uint8_t w1 = white(col1);
    uint8_t r1 = red(col1);
    uint8_t g1 = green(col1);
    uint8_t b1 = blue(col1);
    uint8_t w2 = white(col2);
    uint8_t r2 = red(col2);
    uint8_t g2 = green(col2);
    uint8_t b2 = blue(col2);
    float remain = 1.0 - perc;
    return pack_color(
      (uint8_t)(remain * r1 + perc * r2),
      (uint8_t)(remain * g1 + perc * g2),
      (uint8_t)(remain * b1 + perc * b2),
      (uint8_t)(remain * w1 + perc * w2)
    );
  }

  private:
  uint32_t trig_hue(uint8_t pix_index){
    uint8_t r = (uint8_t)(sin(PI * (pix_index + 72) / 96) * 255);
    uint8_t g = (uint8_t)(sin(PI * (pix_index + 12) / 96) * 255);
    uint8_t b = (uint8_t)(sin(PI * (pix_index + 144) / 96) * 255);
    Serial.print(r);
    Serial.print(',');
    Serial.print(g);
    Serial.print(',');
    Serial.print(b);
    Serial.println();
    return dim_color(pack_color(r, g, b));
  }

  private:
  void cycle_trig_fade(){
    pixel.begin();
    for (uint8_t led_index=0; led_index<num_pixels; led_index++){
      pixel.setPixelColor(led_index, trig_hue(led_index));
    }
    pixel.show();
  }

  private:
  void cycle_fade(){
    pixel.begin();
    for(uint8_t led_index=0; led_index<num_pixels; led_index++){
      float index_float = 6.0 * led_index / num_pixels;
      uint8_t index_int = int(index_float);
      uint32_t color_val;
      if (index_int == index_float){
        color_val = pattern[index_int];
      } else {
        uint8_t index_upper = index_int + 1;
        if (index_upper >= 6){
          index_upper = 0;
        }
        float percent = index_float - index_int;
        color_val = avg_bias(pattern[index_int], pattern[index_upper], percent);
      }
      if (offset >= 144){
        continue;
      }
      uint8_t loc;
      if (offset + led_index > num_pixels - 1){
        loc = led_index + offset - num_pixels + 1;
      } else {
        loc = offset + led_index;
      }
      pixel.setPixelColor(loc, dim_color(color_val));
    }
    pixel.show();
  }*/

  private:
  void rainbow(){
    uint8_t r = 255;
    uint8_t g = 0;
    uint8_t b = 0;
    pixel.begin();
    for (uint8_t index=0; index<num_pixels; index++){
      uint8_t loc;
      if (offset + index > num_pixels - 1){
        loc = index + offset - num_pixels;
      } else {
        loc = offset + index;
      }
      pixel.setPixelColor(
        loc,
        pack_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();
  }

  public:
  void update(){
    unsigned long now = millis();
    if (now - wait > last){
      last = now;
      //cycle_trig_fade();
      //cycle_fade();
      rainbow();
      offset += 1;
      if (offset == num_pixels){
        offset = 0;
      }
    }
  }
};

Strip strip = Strip(6, 144, 100);

void setup(void) {
  while (!Serial); // required for Flora & Micro
  Serial.begin(115200);
  bluefruit_setup();
  strip.off();
  Serial.println(F("setup() finished"));
  bluetooth_wait();
}

void loop(void) {
  strip.update();
  uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
  if (len == 0) return;
  Serial.println(F("data"));
  if (packetbuffer[1] == 'P'){
    Serial.println(char(packetbuffer[2]));
  } else {
    printHex(packetbuffer, len);
  }
}