/*
Skateboard Lights v0.1
Matthew Wong
4/9/22
SLO Hacks 2022
 */

// -- Accelerometer --

  #include <MPU6050_tockn.h>
  #include <Wire.h>

  MPU6050 mpu6050(Wire);


# include <FastLED.h>
# define NUM_LEDS 26
# define LED_DATA_PIN 7
# define BRIGHTNESS 100

CRGB leds[NUM_LEDS];
/*
leds[0:9] = left
leds[10:19] = right
leds[20:25] = back
 */

const int TURN_DELAY = 125; //period of turn (each inc) in ms)
const int BRAKE_DELAY = 50; //period of brake light in ms

const int DEBUG_LEFT = 5;
const int DEBUG_RIGHT = 6; // short to GND to activate

int   turn_state_left = 0; //int 0-5 defining state of left turn
int   turn_state_right = 0;
int   brake_state = 0;
bool  turn_active_left = false;
bool  turn_active_right = false;
bool  brake_active = false;
int   time_turn = 0;
int   time_brake = 0;

void setup() {
  delay(2000); //Delay 2s
  Serial.begin(9600);
  FastLED.addLeds<NEOPIXEL, LED_DATA_PIN>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
  pinMode(DEBUG_LEFT, INPUT_PULLUP);
  pinMode(DEBUG_RIGHT, INPUT_PULLUP);

  //--I2C Init--
  Wire.begin();
  Serial.println("I2C Initialized...");
  mpu6050.begin();
  Serial.println("Calculating Gyro Offsets...");
  mpu6050.calcGyroOffsets(true); //Zeros All Sensors, Will zero to orientation
  Serial.println("Complete!");

}

void loop() {

/////////////////////////////////////////////////////////////////////////////////////////////
// Poll Accell
  mpu6050.update();
  turn_active_right = readRight();
  brake_active = readBrake();
  turn_active_left = readLeft();

  //Serial.print(mpu6050.getAngleY());
  //Serial.print(mpu6050.getAccY());

  //turn_active_left = !digitalRead(DEBUG_LEFT);
  //turn_active_right = !digitalRead(DEBUG_RIGHT);
  //Serial.print(turn_active_left);
  //Serial.print("  ");
  //Serial.print(turn_active_right);
  //Serial.print("  ");
  //Serial.println(brake_active);

  // Static Color when not turning
  if (turn_active_left == false) {
    for (int i = 0; i < 10; i++){
      leds[i].setRGB(255, 0, 0);
    }
    turn_state_left = 0;
    FastLED.show();
  }
  if (turn_active_right == false) {
    for (int i = 0; i < 10; i++){
      leds[i + 10].setRGB(255, 0, 0);
    }
    turn_state_right = 0;
    FastLED.show();
  }
  if ((brake_active == false) && (brake_state == 0)) {
    for (int i = 0; i < 6; i++){
      leds[i + 20].setRGB(255, 0, 0);
    }
    FastLED.show();
  }
/////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////
// LED Updates

//Turn Activation
  if (millis() - time_turn >= TURN_DELAY) {
    time_turn = millis();
   /* Serial.println("LED Values: ");
      for (int i = 0; i < 26; i++){
        Serial.print(leds[i].r);
        Serial.print(" || ");
        Serial.print(leds[i].g);
        Serial.print(" || ");
        Serial.print(leds[i].b);
        Serial.println("");
      } */
    if (turn_active_left) {
      //Serial.print("Turn State Left ");
      //Serial.println(turn_state_left);
      updateTurnState(0, turn_state_left);

      if (turn_state_left < 5) {
        turn_state_left = turn_state_left + 1;
      }
      else if (turn_state_left >= 5) {
        turn_state_left = 0;
      }
    }

    if (turn_active_right) {
      //Serial.print("Turn State Right ");
      //Serial.println(turn_state_right);
      updateTurnState(1, turn_state_right);

      if (turn_state_right < 5) {
        turn_state_right = turn_state_right + 1;
      }
      else if (turn_state_right >= 5) {
        turn_state_right = 0;
      }
    }
  }
// Brake Activation
  if ((millis() - time_brake >= BRAKE_DELAY) && brake_active) {
    time_brake = millis();
    if (brake_state > 0) {
      brakeLightActive(brake_state);
      brake_state = brake_state - 1;
    }
  }
}
/////////////////////////////////////////////////////////////////////////////////////////////

void updateTurnState(int side, int turn_state_local) {

  // sides
  // left = 0
  // right = 1

  int side_comp;
  if (side == 0) {
    side_comp = 0;
  }
  else {
    side_comp = 10;
  }
  for (int i = 0; i < 2*turn_state_local; i = i + 2) {
    leds[side_comp + i].setRGB( 255, 0, 0);
    leds[side_comp + i + 1].setRGB( 255, 0, 0);
  }
  for (int i = 10; i > 2*turn_state_local; i = i - 2) {
    leds[side_comp + i].setRGB( 0, 0, 0);
    leds[side_comp + i - 1].setRGB( 0, 0, 0);
  }

  FastLED.show();

}

void brakeLightActive(int brake_state){
  if (brake_state % 2 == 0) {
    for (int i = 20; i < 26; i++){
      leds[i].setRGB(255, 0, 0);
    }
  }
  else {
    for (int i = 20; i < 26; i++){
      leds[i].setRGB(0, 0, 0);
    }
  }

  FastLED.show();

}

bool readBrake() {
  // brake -> +y
  bool brake = false;
  if (mpu6050.getAccY() > 0.25) {
    brake = true;
  }
  Serial.print("Brake Flag: ");
  Serial.print(brake);
  return brake;

}

bool readRight() {
  // Turning right -> 179 and 160
  bool right = false;
  float angle = mpu6050.getAccAngleY();
  if ((90 < angle) && (angle < 165)) {
    right = true;
  }
  Serial.print(" Right Flag: ");
  Serial.print(right);
  return right;
}

bool readLeft() {
  // angleY between -179 and -160
  bool left = false;
  float angle = mpu6050.getAccAngleY();
  if ((-165 < angle) && (angle < -90)) {
    left = true;

  }
  Serial.print(" Left Flag: ");
  Serial.print(left);
  Serial.print(" Y Accel Angle ");
  Serial.println(mpu6050.getAccAngleY());
  return left;
}