#include <Adafruit_NeoPixel.h>

#define DEBUG
//#define SIGNALTEST

#define BRAKE_FLASHING true

#define TWO_PI 6.283185307179586476925286766559
#define NUM_LEDS_LEFT 47
#define LED_PIN_LEFT 5
#define LIGHT_SIZE   6

#define PRESSED 0
#define RELEASED 1

// Usd for fash_state
#define ON 1
#define OFF 0

// Array Position DEFINE //
#define LEFT 0
#define RIGHT 1
#define BRAKE 2
#define SCROLL_SPEED 0
#define RUNNING_LIGHT_BRIGHTNESS 1
#define SPARE 2

#define RIGHT_INPUT_PIN 2
#define BRAKE_INPUT_PIN 3
#define LEFT_INPUT_PIN 4
#define ANOLOG_SET_PIN 22
#define SCROLL_SPEED_INPUT_PIN 28
#define RUNNING_LIGHT_BRIGHTNESS_INPUT_PIN 27
#define SPARE_INPUT_PIN 26

#define BRAKE_FLIP_DELAY 100
#define BRAKE_FLIP_DELAY_REDUCTION 10
#define BRAKE_FLIP_DELAY_MIN 20

//#define AMBER WS2812B.Color(255, 191, 3)
#define AMBER 0xFFBF03
//#define RED WS2812B.Color(255, 0, 0)
#define RED 0xFF0000
//#define BLACK WS2812B.Color(0, 0, 0)
#define BLACK 0x000000
//#define LIGHTRED WS2812B.Color(25, 0, 0)
#define LIGHTRED 0x150000
//#define SUPERLIGHTRED WS2812B.Color(3, 0, 0)
#define SUPERLIGHTRED 0x030000

#if defined(SIGNALTEST)
  unsigned long flasher_time = 0;
#endif

#define SIGNAL_FREQ 1
int signal_period = 1000 / SIGNAL_FREQ;

#define SIN_SWEEP 0
#define BURST 1
#define FLASH 2

int signal_style = FLASH;

unsigned long last_change = 0;
int flashstate = OFF;

// Variables to track button state
uint32_t buttonState = 0;
uint32_t lastButtonState = 0;

unsigned int SIN_Map[NUM_LEDS_LEFT + LIGHT_SIZE];
unsigned int COS_Map[NUM_LEDS_LEFT + LIGHT_SIZE];
/*
  void Fill_SIN_Map(unsigned char min = 0, unsigned char max = NUM_LEDS_LEFT){
  float inc = TWO_PI/NUM_LEDS_LEFT;
  unsigned char midpoint = (max-min)/2;
  for(int x = 0; x < NUM_LEDS_LEFT; x++){
    // sin() take an angle in radians.
    SIN_Map[x] = min + midpoint + sin(x * inc) * midpoint;
  }
  }
*/
void Fill_Map(unsigned int min = 0, unsigned int max = NUM_LEDS_LEFT) {
  float inc = TWO_PI / NUM_LEDS_LEFT;
  unsigned int midpoint = (max - min) / 2;
  for (int x = 0; x <= NUM_LEDS_LEFT; x++) {
    // sin() take an angle in radians.
    SIN_Map[x] = min + midpoint + sin(x * inc) * midpoint;
    COS_Map[x] = min + midpoint + cos(x * inc) * midpoint;
  }
}

int Signal_Input_PINs[] = {LEFT_INPUT_PIN, RIGHT_INPUT_PIN, BRAKE_INPUT_PIN, ANOLOG_SET_PIN};
int Analog_Input_Pins[] = {SCROLL_SPEED_INPUT_PIN, RUNNING_LIGHT_BRIGHTNESS_INPUT_PIN, SPARE_INPUT_PIN};

int Signal_Input_PIN_State[] = {RELEASED, RELEASED, RELEASED};
int Signal_Input_PIN_State_Previous[] = {RELEASED, RELEASED, RELEASED};

/*
  Analog is almost NEVER read, and should only be read on start
  However, since that would be hugely inconveniant
  Lets read it every 500 milliseconds
*/
int Analog_Input_Value[] = {50, 3, 0};

/*
  This will allow variations in the amount of time __NEEDS__ to pass before there can be a STATE change
  The high Released value for the Signal Lights will (Hopefully) stop the flashing from the Signal causing issues.
  It may need to be adjusted
*/
// {LEFT_INPUT_PIN, RIGHT_INPUT_PIN, BRAKE_INPUT_PIN}
int Debounce_Delay_Pressed[] = { 50, 50, 50 };
// {LEFT_INPUT_PIN, RIGHT_INPUT_PIN, BRAKE_INPUT_PIN}
int Debounce_Delay_Released[] = { 1500, 1500, 50 };
// {LEFT_INPUT_PIN, RIGHT_INPUT_PIN, BRAKE_INPUT_PIN}
unsigned long State_Changed_Time[] = { 0, 0, 0 };  // -- This is the current time --
unsigned long Debounce_Delay = 0;                  // This holds the current Debounce_Delay amount in the main loop

unsigned long Brakelight_Flip_Delay = BRAKE_FLIP_DELAY;
unsigned long Brakelight_LastFlip_Time = 0;
boolean Brake_State_On = false;

bool LEDs_Cleared[] = { false, false, false };

uint32_t RunningLights_Color = SUPERLIGHTRED;
uint32_t BrakeLights_Color = RED;
uint32_t Clear_Color = RunningLights_Color;

int pos = 0;
int max_pos = NUM_LEDS_LEFT;
int min_pos = 0;

Adafruit_NeoPixel Strip_Left(NUM_LEDS_LEFT, LED_PIN_LEFT, NEO_GRB + NEO_KHZ800);

/*
  @brief  Sweep from top to bottom continuously
  @param  *Cur_Strip:  Pointer to the NeoPixel Strip
  @param  Position:    The Position of the Virtual LED
  @param  Color:       The Virtual Light Color
  @param  Light_Size:  The number od LEDs in the virtual light
  @param  min:         The minimum point of the Virtual Light
  @param  max:         The maximum point of the Virtual Light
  @return void
  @note Self explanitory
*/
void Sin_Sweep(Adafruit_NeoPixel *Cur_Strip, int Position, uint32_t Color, int Light_Size = LIGHT_SIZE, uint8_t min = 0, uint8_t max = NUM_LEDS_LEFT) {
  unsigned int Mid_pos = (int)((SIN_Map[Position] - 3));  // This will give the adjusted Position
  // The Virtual Light needs to start from Position and go 1/2 before * 1/2 After
  //int x = -1 * Light_Size;
  // The Virtual Light is moving towards min, therefore the "TAIL" should be more positive
  int pos = Mid_pos;
  int Light_Start = 0;
  int Light_Finish = 6;
  for (int x = Light_Start; x <= Light_Finish; x++) {
    // Make sure not to color outside the lines
    pos = Mid_pos + x;
    // pos needs to be between the max & min
    if (pos <= max && pos >= min)
      Cur_Strip->setPixelColor(pos, Color);
  }
}

volatile boolean readAnalogValuesFLAG = false;
// This function is called by the hardware interupt 
void set_analog_values(){
  readAnalogValuesFLAG = true;
}

void setup() {
  // put your setup code here, to run once:
  Serial1.begin(115200);
  Serial1.println("WS2812 Test");
  Fill_Map();
  // Set up the switch pins
  for(int cur_PIN : Signal_Input_PINs){
    pinMode(cur_PIN, INPUT_PULLUP);
  }
  for(int cur_PIN : Analog_Input_Pins){
    pinMode(cur_PIN, INPUT);
  }

  // Attach a hardware interupt so that there is no wasted cpu cycles
  attachInterrupt(digitalPinToInterrupt(ANOLOG_SET_PIN), set_analog_values, RISING);

  //Analog_Input_Value[SCROLL_SPEED] = analogRead(Analog_Input_Pins[SCROLL_SPEED_INPUT_PIN])/10;
  Analog_Input_Value[SCROLL_SPEED] = analogRead(SCROLL_SPEED_INPUT_PIN)/10;
  Serial1.print("SCROLL_SPEED: ");
  Serial1.println(Analog_Input_Value[SCROLL_SPEED]);
  Strip_Left.begin();
  Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
  Strip_Left.show();
}

void loop() {
  uint32_t ALLPins = gpio_get_all();
  // Interrupt service routine for handling button presses

  // Extract the state of individual button pins
  // This function determines if there has been a button pressed
  buttonState = ALLPins & ((1 << Signal_Input_PINs[0]) | (1 << Signal_Input_PINs[1]) | (1 << Signal_Input_PINs[2]) | (1 << Signal_Input_PINs[3]));
  if(lastButtonState != buttonState){
    // There's been a change
    // lastButtonState = buttonState;

    Serial1.print("buttonState Changed: ");
    Serial1.println(lastButtonState);

  // --------------------------------------------- Wrapping the PIN STATE FUNCTIONS ---------------------------------------------\\
  // REMOVE IF IT BREAKS THE FUNCTIONS

  // REMOVE IF IT BREAKS THE FUNCTIONS
  // --------------------------------------------- Wrapping the PIN STATE FUNCTIONS ---------------------------------------------\\
  
  // put your main code here, to run repeatedly:
  if(readAnalogValuesFLAG){
    Analog_Input_Value[SCROLL_SPEED] = analogRead(SCROLL_SPEED_INPUT_PIN)/10;
    Serial1.print("Scroll Speed: ");
    Serial1.println(Analog_Input_Value[SCROLL_SPEED]);

    Analog_Input_Value[RUNNING_LIGHT_BRIGHTNESS] = analogRead(RUNNING_LIGHT_BRIGHTNESS_INPUT_PIN)/8;
    RunningLights_Color = Clear_Color = Analog_Input_Value[RUNNING_LIGHT_BRIGHTNESS] << 16;
    Serial1.print("Runnng light brightness: ");
    Serial1.println(Analog_Input_Value[RUNNING_LIGHT_BRIGHTNESS], HEX);
    Serial1.print("Runnng light color: ");
    Serial1.println(RunningLights_Color, HEX);
    readAnalogValuesFLAG = false;
  }

// 
//   // // Serial1.print("\e[2J");
//   // // Serial1.print("\e[1;1H");
//   String tempString = "LEFT: " + (String)(ALLPins >> LEFT_INPUT_PIN & 1) + "\tBRAKE: " + (String)(ALLPins >> BRAKE_INPUT_PIN & 1)+ "\tRIGHT: " + (String)(ALLPins >> RIGHT_INPUT_PIN & 1);
//   Serial1.print("gpio_get_all(): ");
//   Serial1.print(ALLPins, BIN);
//   Serial1.println(tempString);
// 

  if(!Signal_Input_PIN_State[LEFT] && ALLPins>>LEFT_INPUT_PIN&1 && millis() > State_Changed_Time[LEFT] + Debounce_Delay_Released[LEFT]){
    // BUTTON can be released
    Signal_Input_PIN_State[LEFT] = RELEASED;
    lastButtonState = buttonState;
    State_Changed_Time[LEFT] = millis();
    LEDs_Cleared[BRAKE] = false;
    Serial1.println("LEFT Signal RELEASED");
  } else if(!(ALLPins>>LEFT_INPUT_PIN&1) && millis() > State_Changed_Time[LEFT] + Debounce_Delay_Pressed[LEFT]){
    // BUTTON can be pressed
    Signal_Input_PIN_State[LEFT] = PRESSED;
    lastButtonState = buttonState;
    State_Changed_Time[LEFT] = millis();
    Serial1.println("LEFT Signal PRESSED");
  }
  if(!Signal_Input_PIN_State[RIGHT] && ALLPins>>RIGHT_INPUT_PIN&1 && millis() > State_Changed_Time[RIGHT] + Debounce_Delay_Released[RIGHT]){
    // BUTTON can be released
    Signal_Input_PIN_State[RIGHT] = RELEASED;
    lastButtonState = buttonState;
    State_Changed_Time[RIGHT] = millis();
    LEDs_Cleared[BRAKE] = false;
    Serial1.println("RIGHT Signal RELEASED");
  } else if(!(ALLPins>>RIGHT_INPUT_PIN&1) && millis() > State_Changed_Time[RIGHT] + Debounce_Delay_Pressed[RIGHT]){
    // BUTTON can be pressed
    Signal_Input_PIN_State[RIGHT] = PRESSED;
    lastButtonState = buttonState;
    State_Changed_Time[RIGHT] = millis();
    Serial1.println("RIGHT Signal PRESSED");
  }
  if(!Signal_Input_PIN_State[BRAKE] && ALLPins>>BRAKE_INPUT_PIN&1 && millis() > State_Changed_Time[BRAKE] + Debounce_Delay_Released[BRAKE]){
     // BUTTON can be released
    Signal_Input_PIN_State[BRAKE] = RELEASED;
    lastButtonState = buttonState;
    State_Changed_Time[BRAKE] = millis();
    Clear_Color = RunningLights_Color;
    LEDs_Cleared[BRAKE] = false;
    Brakelight_Flip_Delay = BRAKE_FLIP_DELAY; // <- This should be adjustable and may be set by the Potentiometer
    Serial1.println("BRAKE RELEASED");
  } else if(Signal_Input_PIN_State[BRAKE] && !(ALLPins>>BRAKE_INPUT_PIN&1) && millis() > State_Changed_Time[BRAKE] + Debounce_Delay_Pressed[BRAKE]){
    // BUTTON can be pressed
    //Signal_Input_PIN_State[BRAKE] = Signal_Input_PIN_State[BRAKE];
    Signal_Input_PIN_State[BRAKE] = PRESSED;
    lastButtonState = buttonState;
    State_Changed_Time[BRAKE] = millis();
    Clear_Color = BrakeLights_Color;
    LEDs_Cleared[BRAKE] = false;
    Serial1.println("BRAKE PRESSED");
  }
}
  // This is the Brake Flash algorithm 
  if(BRAKE_FLASHING && Signal_Input_PIN_State[BRAKE] == PRESSED && millis() > Brakelight_LastFlip_Time + Brakelight_Flip_Delay){
    if(Brakelight_Flip_Delay > BRAKE_FLIP_DELAY_MIN && Brake_State_On){
      Brake_State_On = false;
      LEDs_Cleared[BRAKE] = false;
      Clear_Color = RunningLights_Color;
      Brakelight_LastFlip_Time = millis();
      Brakelight_Flip_Delay -= BRAKE_FLIP_DELAY_REDUCTION;
    }else if(Brakelight_Flip_Delay > BRAKE_FLIP_DELAY_MIN && !Brake_State_On){
      Brake_State_On = true;
      LEDs_Cleared[BRAKE] = false;
      Clear_Color = BrakeLights_Color;
      Brakelight_LastFlip_Time = millis();
      Brakelight_Flip_Delay -= BRAKE_FLIP_DELAY_REDUCTION;
    }else if(Brakelight_Flip_Delay <= BRAKE_FLIP_DELAY_MIN && !Brake_State_On){
      // The Brakelight should be in it's fully on state, but isn't.
      // Turn the Brakelight on, and issue a "Clear", and make sure the Color is correct.
      Brake_State_On = true;
      LEDs_Cleared[BRAKE] = false;
      Clear_Color = BrakeLights_Color;
    }else{
      // The Brakelight is fully on.
      // No need to "Clear" the LEDS
    }
  }
//   /*
//     We don't want to "Clear" or "Reset" the Brake Light, or the Driving Light, every time, that would be wastefull.
//     We'll check to see if the LEDs_Cleared flag for the brake light has been set.
//     If the LEDs_Cleared is true then we don't need to clear it, but if it's false, CLEAR IT!!
//   */
  if (!LEDs_Cleared[BRAKE]) {
    // Fill both of the Strips with the default color
    Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
    // Strip_Right.fill(Clear_Color, 0, NUM_LEDS_LEFT);
    LEDs_Cleared[BRAKE] = true;
    //pos = min_pos;
    Strip_Left.show();
    // Strip_Right.show();
  }

// #if defined(SIGNALTEST)
//   if(millis() > flasher_time + signal_period){
//     flasher_time = millis();
//     if(Signal_Input_PIN_State[LEFT] == PRESSED){
//       Signal_Input_PIN_State[LEFT] = RELEASED;
//     }else{
//       Signal_Input_PIN_State[LEFT] = PRESSED;
//     }
//   }
// #endif

  // This is the Signals subroutine 
  // if(Signal_Input_PIN_State[LEFT] == PRESSED || Signal_Input_PIN_State[RIGHT] == PRESSED){
  if(Signal_Input_PIN_State[LEFT] == PRESSED){
    switch(signal_style){
      case SIN_SWEEP:
        // This is a Sweeping LED cluster //
        pos++;
        if(pos > max_pos)pos = min_pos;
        // Fill both of the Strips with the default color
        // This is where each of the LED Strips are seperated
        // This ensures that the "pos" variabvariable always matches for left and right
        if(Signal_Input_PIN_State[LEFT] == PRESSED){
          Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
          Sin_Sweep(&Strip_Left, pos, AMBER);
        }
        // ** NO "RIGHT" STRIP YET **
        // if(Signal_Input_PIN_State[RIGHT] == PRESSED){
        //   Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
        //   Sin_Sweep(&Strip_Left, pos, AMBER);
        // }
        break;
      case BURST:
        // ToDo
        pos++;
        if(pos > max_pos)pos = min_pos;
        // Fill both of the Strips with the default color
        // This is where each of the LED Strips are seperated
        // This ensures that the "pos" variabvariable always matches for left and right
        if(Signal_Input_PIN_State[LEFT] == PRESSED){
          Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
          Sin_Sweep(&Strip_Left, pos, AMBER);
        }
        // ** NO "RIGHT" STRIP YET **
        // if(Signal_Input_PIN_State[RIGHT] == PRESSED){
        //   //Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
        //   //Sin_Sweep(&Strip_Left, pos, AMBER);
        // }
       break;
      case FLASH:
        // This is a GENERIC Signal Light Flash //
        if(millis() > last_change + signal_period ){
          last_change = millis();
          if(flashstate){
            flashstate = false;
            if(Signal_Input_PIN_State[LEFT] == PRESSED){
              Strip_Left.fill(AMBER, 0, NUM_LEDS_LEFT);
            }
            // ** NO "RIGHT" STRIP YET **
            // if(Signal_Input_PIN_State[RIGHT] == PRESSED){
            //   // Strip_Left.fill(AMBER, 0, NUM_LEDS_LEFT);
            // }
          }else{
            flashstate = true;
            if(Signal_Input_PIN_State[LEFT] == PRESSED){
              Strip_Left.fill(Clear_Color, 0, NUM_LEDS_LEFT);
            }
            // ** NO "RIGHT" STRIP YET **
            // if(Signal_Input_PIN_State[RIGHT] == PRESSED){
            //   // Strip_Left.fill(AMBER, 0, NUM_LEDS_LEFT);
            // }
          }
        }
        break;
    }
    Strip_Left.show();
  }
  delay(Analog_Input_Value[SCROLL_SPEED]); // this speeds up the simulation
}