#include <TinyDebug.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <FastLED.h>

#define KILL_SW_INTERRUPT_PIN PCINT1  // This is PB1 per the schematic
#define KILL_SW_INT_PIN PB1           // Interrupt pin of choice: PB1 (same as PCINT1) - Pin 6
#define BRAKE_SW_INTERRUPT_PIN PCINT2  // This is PB2 per the schematic
#define BRAKE_SW_INT_PIN PB2           // Interrupt pin of choice: PB2 (same as PCINT1) - Pin 7
#define PCINT_VECTOR PCINT0_vect      // This step is not necessary - it's a naming thing for clarit

#define LEFT_PIN    PB4 // Digital input pin for left turn signal
#define RIGHT_PIN   PB3 // Digital input pin for right turn signal

#define LED_PIN PB0                   // PB0 - Pin 5
#define NUM_LEDS    20 // Number of LEDs in the strip

CRGB leds[NUM_LEDS];

bool killSwitchEnabled = false; // false - low, true - high
bool brakeSwitchEnabled = false; // false - low, true - high

int myLineCounter = 0;
void myDebugPrint(String message)
{
  Debug.print(String(myLineCounter));
  Debug.println(" :" + message);
  myLineCounter++;
}


void setup() {
  // debug section
  Debug.begin();
  myDebugPrint("Hello, TinyDebug!");

  // led indicator section
  // Setup adresable leds
  FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);
  //pinMode(LED_PIN, OUTPUT);

  // indicator section
  pinMode(LEFT_PIN, INPUT);
  pinMode(RIGHT_PIN, INPUT);
  // pinMode(LEFT_PIN, INPUT_PULLUP);   // Set our left indicator input with a pullup to keep it stable
  // pinMode(RIGHT_PIN, INPUT_PULLUP);  // Set our right indicator input with a pullup to keep it stable

  // kill and brake switch section
  cli();                            // Disable interrupts during setup
  PCMSK |= (1 << KILL_SW_INTERRUPT_PIN);    // Enable interrupt handler (ISR) for our chosen interrupt pin (PCINT1/PB1/pin 6)
  PCMSK |= (1 << BRAKE_SW_INT_PIN);    // Enable interrupt handler (ISR) for our chosen interrupt pin (PCINT2/PB2/pin 7)
  GIMSK |= (1 << PCIE);             // Enable PCINT interrupt in the general interrupt mask
  // pinMode(KILL_SW_INT_PIN, INPUT_PULLUP);   // Set our interrupt pin as input with a pullup to keep it stable
  // pinMode(BRAKE_SW_INT_PIN, INPUT_PULLUP);   // Set our interrupt pin as input with a pullup to keep it stable
  sei();                            //last line of setup - enable interrupts after setup

}




#define BRIGHTNESS  100
#define FADE_DELAY  30
#define SPEED       1
#define TAIL_LENGTH 3
uint16_t sinBeat;

uint16_t phase = 0; // 0 -> left, 32767 -> right
uint32_t time = 750; // 0 -> left, 750 -> right

bool latch = true;

void skrec()
{


  // HAZARD Lights
  sinBeat = beatsin16(30, 0, NUM_LEDS - 1, 0, phase);
  leds[sinBeat] = CRGB::Blue;
  fadeToBlackBy(leds, NUM_LEDS, 10);
  FastLED.show();
  myDebugPrint(String(sinBeat));
  phase += 32767;





  // LEFT SIGNAL
  // sinBeat = beatsin16(30, 0, NUM_LEDS - 1, 0, phase);
  // leds[sinBeat] = CRGB::Blue;
  // fadeToBlackBy(leds, NUM_LEDS, 10);
  // FastLED.show();

  // if (sinBeat == NUM_LEDS - 1)
  // {
  //   if (latch)
  //   {
  //     myDebugPrint(String(phase));
  //     phase += 32768;
  //     latch = false;
  //   }
  // }
  // else
  // {
  //   latch = true;
  // }
  


  // RIGHT SIGNAL
  // sinBeat = beatsin16(30, 0, NUM_LEDS - 1, 0, phase);
  // leds[sinBeat] = CRGB::Blue;
  // fadeToBlackBy(leds, NUM_LEDS, 10);
  // FastLED.show();

  // if (sinBeat == 0)
  // {
  //   if (latch)
  //   {
  //     myDebugPrint(String(phase));
  //     phase += 32768;
  //     latch = false;
  //   }
  // }
  // else
  // {
  //   latch = true;
  // }



  // LEFT SIGNAL
  // sinBeat = beatsin16(30, 0, NUM_LEDS - 1, time, 0);
  // leds[sinBeat] = CRGB::Blue;
  // fadeToBlackBy(leds, NUM_LEDS, 10);
  // FastLED.show();

  // if (sinBeat == NUM_LEDS - 1)
  // {
  //   if (latch)
  //   {
  //     myDebugPrint(String(time));
  //     time += 750;
  //     latch = false;
  //   }
  // }
  // else
  // {
  //   latch = true;
  // }



  // // RIGHT SIGNAL
  // sinBeat = beatsin16(30, 0, NUM_LEDS - 1, time, 0);
  // leds[sinBeat] = CRGB::Blue;
  // fadeToBlackBy(leds, NUM_LEDS, 10);
  // FastLED.show();

  // if (sinBeat == 0)
  // {
  //   if (latch)
  //   {
  //     myDebugPrint(String(time));
  //     time += 750;
  //     latch = false;
  //   }
  // }
  // else
  // {
  //   latch = true;
  // }



}


void loop() {

  skrec();


  // if (!killSwitchEnabled) {
  //   // Only light up if kill switch is enabled

  //   if (brakeSwitchEnabled) {
  //     // Brake signal detected, light up LED strip, inverse logic due to pull up
  //     // myDebugPrint("brakeEnabled");
  //     startBrakeAnimation();
  //   }
  //   else {
  //     //myDebugPrint("Turning");

  //     // Check for left turn signal
  //     if (digitalRead(LEFT_PIN) == HIGH) {
  //       // Left turn signal detected

  //       // Indicate left turn here
  //       // myDebugPrint("startLeftTurnAnimation");
  //       startLeftTurnAnimation();
  //     }

  //     // Check for right turn signal
  //     if (digitalRead(RIGHT_PIN) == HIGH) {
  //       // Right turn signal detected

  //       // Indicate right turn here
  //       // myDebugPrint("startRightTurnAnimation");
  //       startRightTurnAnimation();
  //     }
  //   }
  // }
  // else {
  //   // myDebugPrint("killSwitchEnabled");
  //   fill_solid(leds, NUM_LEDS, CRGB::Black);
  //   FastLED.show();
  // }
}


// This is the interrupt handler called when there is any change on the INT_PIN
// ISR is defined in the headers - the ATtiny85 only has one handler
ISR(PCINT_VECTOR)
{
  // myDebugPrint("ISR");
  // measure kill sw pin
  if (killSwitchEnabled != digitalRead(KILL_SW_INT_PIN))
  {
    killSwitchEnabled = digitalRead(KILL_SW_INT_PIN);
  }

  // measure brake sw pin
  if (brakeSwitchEnabled != digitalRead(BRAKE_SW_INT_PIN))
  {
    brakeSwitchEnabled = digitalRead(BRAKE_SW_INT_PIN);
  }
}



void startLeftTurnAnimation() {
  // Left turn animation code

  while (true)
  {
    for (int i = 0; i < NUM_LEDS; i++) {
      leds[i] = CRGB::Red; // Change color to whatever you like
      FastLED.show();

      // Test this evry iteration in order to be sure
      // that its true and we can animate
      if (brakeSwitchEnabled || killSwitchEnabled || !digitalRead(LEFT_PIN))
      {
        fill_solid(leds, NUM_LEDS, CRGB::Black);
        FastLED.show();
        return;
      }

      delay(100); // Adjust the delay as needed
    }
    // Reset LEDs
    fill_solid(leds, NUM_LEDS, CRGB::Black);
  }
}

void startRightTurnAnimation() {
  // Right turn animation code

  while (true)
  {
    for (int i = NUM_LEDS - 1; i >= 0; i--) {
      leds[i] = CRGB::Red; // Change color to whatever you like
      FastLED.show();

      // Test this evry iteration in order to be sure
      // that its true and we can animate
      if (brakeSwitchEnabled || killSwitchEnabled || !digitalRead(RIGHT_PIN))
      {
        fill_solid(leds, NUM_LEDS, CRGB::Black);
        FastLED.show();
        return;
      }

      delay(100); // Adjust the delay as needed
    }

    // Reset LEDs
    fill_solid(leds, NUM_LEDS, CRGB::Black);
  }
}

void startBrakeAnimation() {
  // Brake animation code

  while (true)
  {
    // Loop for blinking effect
    for (int i = 0; i < 5; i++) {
      // Turn on all LEDs
      fill_solid(leds, NUM_LEDS, CRGB::Red);
      FastLED.show();
      delay(200); // Adjust blinking speed as needed

      // Test this evry iteration in order to be sure
      // that its true and we can animate
      if (killSwitchEnabled || !brakeSwitchEnabled) {
        fill_solid(leds, NUM_LEDS, CRGB::Black);
        FastLED.show();
        return;
      }

      // Turn off all LEDs
      fill_solid(leds, NUM_LEDS, CRGB::Black);
      FastLED.show();
      delay(200); // Adjust blinking speed as needed
    }
  }
}

void animationTest() {
  // Wandering light effect
  for (int i = 0; i < NUM_LEDS + 4; i++) {
    // Update LEDs
    for (int j = 0; j < NUM_LEDS; j++) {
      // Calculate brightness based on distance from wandering light
      int distance = abs(j - i);
      int brightness = constrain(255 - distance * 60, 0, 255);

      // Apply fade in and fade out effect
      if (distance < 3) {
        brightness = map(distance, 0, 2, 0, brightness);
      }

      // Set LED color
      leds[j] = CRGB(brightness, brightness, brightness);
    }

    // Show updated LEDs
    FastLED.show();

    // Pause for a moment
    delay(1 * 30);
  }
}


void animationTest2() {

  uint8_t bpm = 60;               //tempo kierunkowskazu
  uint16_t period = (60 / bpm) * 1000;          //ile trwa okres w [ms]

  uint8_t beat = beatsin8(bpm, 0, 255); // Generate beat value using beatsin8()

  // Wandering light effect
  for (int i = 0; i < NUM_LEDS + 4; i++) {
    // Update LEDs
    for (int j = 0; j < NUM_LEDS; j++) {
      // Calculate brightness based on distance from wandering light
      int distance = abs(j - i);
      int brightness = constrain(255 - distance * 60 + beat, 0, 255); // Add beat effect

      // Apply fade in and fade out effect
      if (distance < 1) {
        brightness = map(distance, 0, 1, 0, brightness);
      }

      // Set LED color
      leds[j] = CRGB(brightness, brightness, brightness);
    }

    // Show updated LEDs
    FastLED.show();

    // Pause for a moment
    delay(1 * 30);
  }
}

bool one_cycle = 0;
bool high = 0;
uint32_t starting_millis;       //zmienna do zerowania sinusa

void animationTest3() {
  uint8_t b_wave = 0;
  uint8_t bpm = 60;               //tempo kierunkowskazu
  starting_millis = 0;
  b_wave = beatsin8(bpm, 0, 255, starting_millis);

  // Serial.print("b_wave = "); Serial.print(b_wave);          //pierwszy serial CRGB(93, 255, 93)
  // Serial.print(" one_cycle= "); Serial.print(1 * 80); //drugi CRGB::Orange
  // Serial.print(" high= "); Serial.println(0 * 160);      //trzeci CRGB::Magenta


  leds[0] = CHSV(60, 255, b_wave);

  for (int i = NUM_LEDS - 1; i > 0; i--) {
    leds[i] = leds[i - 1];	//kopiowanie ustawienia danego piksela do lewego sąsiada
  }

  if (b_wave > 10)
  {
    high = 1;
  }

  if (high && b_wave < 10)
  {
    one_cycle = 0;
  }

  if (one_cycle)
  {
    FastLED.show();
  }
  else {
    FastLED.clear(true);
  }
}

uint8_t b_wave = 0;
void beatwave() {

  EVERY_N_MILLISECONDS(50) {                    //potrzeba wyjasnic dlaczego wnetrze tej funkcji EVERY_N_(...) bez
    leds[0] = CHSV(60, 255, b_wave);            //tej funckji zapala wszystkie naraz, a kiedy jest nią okalone widzimy animację

    for (int i = NUM_LEDS - 1; i > 0; i--) {
      leds[i] = leds[i - 1];	//kopiowanie ustawienia danego piksela do lewego sąsiada
    }
  }

} // beatwave()