#include <Adafruit_NeoPixel.h>
#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
// #include "Adafruit_DRV2605.h"
// #include "ICM42688.h"

#define INT_PIN 14 // Example pin for INT

// ICM42688 IMU(SPI, 13);
// Adafruit_DRV2605 drv;
#define PIN 18
#define NUMPIXELS 8
Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);

// Define primary colors
#define RED_COLOR pixels.Color(255, 0, 0)
#define DARK_ORANGE_COLOR pixels.Color(255, 140, 0)
#define YELLOW_COLOR pixels.Color(255, 255, 0)
#define GREEN_COLOR pixels.Color(0, 255, 0)

// Define secondary color (for blinking)
#define BLUE_COLOR pixels.Color(0, 0, 255)

typedef enum {
  Red,
  Red_2,
  DarkOrange,
  DarkOrange_2,
  Yellow,
  Yellow_2,
  Green,
  Green_2
} color_t;

typedef enum {
  One,
  Two,
  Three,
  Four,
  Five,
  Six,
  Seven,
  Eight
} leds_t;

uint8_t leds_left[] = {7, 6, 5, 4, 3, 2, 1, 0};

unsigned long previousMillis = 0;
const long interval = 200;
String direction = "Left";

void setup() {
  SPI.begin();
  Serial.begin(9600);
  pinMode(INT_PIN, INPUT); // Initialize INT pin as input
  // attachInterrupt(digitalPinToInterrupt(INT_PIN), triggerHandler, RISING); // Attach interrupt handler
  while (!Serial) {}
  
  // int status = IMU.begin();
  // if (status < 0) {
  //   Serial.println("IMU initialization unsuccessful");
  //   Serial.println("Check IMU wiring or try cycling power");
  //   Serial.print("Status: ");
  //   Serial.println(status);
  //   while (1) {}
  // }
  
  pixels.begin();
  // IMU.setAccelODR(ICM42688::odr12_5);
  // IMU.setGyroODR(ICM42688::odr12_5);
  
  // drv.begin();
  // drv.setMode(DRV2605_MODE_INTTRIG);
  // drv.selectLibrary(1);
  // drv.setWaveform(0, 17);
  // drv.setWaveform(0, 71);
  
  Serial.println("ax,ay,az,gx,gy,gz,temp_C");
}

void loop() {
  float acceleration = 4.5;
  haptic_output(acceleration);
}

void triggerHandler() {
  // Handle trigger event
  // Trigger haptic effect on DRV2605
  // drv.go();
}

// float readAccelerometer() {
//   IMU.getAGT();
//   return sqrt(pow(IMU.accX(), 2) + pow(IMU.accY(), 2) + pow(IMU.accZ(), 2));
// }

void set_led_color(leds_t led, color_t color) {
  switch (color) {
    case Red:
      pixels.setPixelColor(led, RED_COLOR);
      break;
    case Red_2:
      pixels.setPixelColor(led, RED_COLOR);
      break;
    case DarkOrange:
      pixels.setPixelColor(led, DARK_ORANGE_COLOR);
      break;
    case DarkOrange_2:
      pixels.setPixelColor(led, DARK_ORANGE_COLOR);
      break;
    case Yellow:
      pixels.setPixelColor(led, YELLOW_COLOR);
      break;
    case Yellow_2:
      pixels.setPixelColor(led, YELLOW_COLOR);
      break;
    case Green:
      pixels.setPixelColor(led, GREEN_COLOR);
      break;
    case Green_2:
      pixels.setPixelColor(led, GREEN_COLOR);
      break;
  }
}

void turn_on_leds(leds_t leds, color_t color) {
  set_led_color(leds, color);
  pixels.show();
}

void turn_on_leds_color_right(uint8_t leds) {
  for(uint8_t i=0; i<leds; i++) {
    turn_on_leds((leds_t)i, (color_t)i);
  }
}

void turn_off_leds_color_right(uint8_t leds) {
  for(uint8_t i=leds; i!=0; i--) {
    delay(100);
    pixels.setPixelColor((i-1), 0);
    pixels.show();
  }
}

void turn_on_leds_color_left(uint8_t leds) {
  for(uint8_t i=0; i<leds; i++) {
    turn_on_leds((leds_t)(leds_left[i]), (color_t)i);
  }
}

void turn_off_leds_color_left(uint8_t leds) {
  for(int8_t i=0; i<8; i++) {
    delay(100);
    pixels.setPixelColor(i, 0);
    pixels.show();
  }
}

void haptic_output(float level) {
  if (level > 4.6 ) {
    triggerHandler(); // Call triggerHandler directly
    if(direction == "Right") {
      turn_on_leds_color_right(8);
    } else {
      turn_on_leds_color_left(8);
    }
    delay(200);
    for (byte i = 0; i < 4; i++) {
      for (byte j = 0; j < 8; j++) {
        pixels.setPixelColor(j, BLUE_COLOR); // Use secondary color for blinking
        pixels.show();
        delay(10);
        pixels.clear();
        // drv.stop();
      }
      delay(50);
      triggerHandler(); // Call triggerHandler directly
    }
  }
  else if (level > 1.2 && level <= 1.6) {
    if(direction == "Right") {
      turn_on_leds_color_right(1);
    } else {
      turn_on_leds_color_left(1);
    }
  }
  else if (level > 1.6 && level <= 2) {
    if(direction == "Right") {
      turn_on_leds_color_right(2);
      turn_off_leds_color_right(2);
    } else {
      turn_on_leds_color_left(2);
      turn_off_leds_color_left(2);
    }
  }
  else if (level > 2 && level <= 2.4) {
    if(direction == "Right") {
      turn_on_leds_color_right(3);
      turn_off_leds_color_right(3);
    } else {
      turn_on_leds_color_left(3);
      turn_off_leds_color_left(3);
    }
  }
  else if (level > 2.4 && level <= 2.8) {
    if(direction == "Right") {
      turn_on_leds_color_right(4);
      turn_off_leds_color_right(4);
    } else {
      turn_on_leds_color_left(4);
      turn_off_leds_color_left(4);
    }
  }
  else if (level > 2.8 && level <= 3.3) {
    if(direction == "Right") {
      turn_on_leds_color_right(5);
      turn_off_leds_color_right(5);
    } else {
      turn_on_leds_color_left(5);
      turn_off_leds_color_left(5);
    }
  }
  else if (level > 3.3 && level <= 3.9) {
    if(direction == "Right") {
      turn_on_leds_color_right(6);
      turn_off_leds_color_right(6);
    } else {
      turn_on_leds_color_left(6);
      turn_off_leds_color_left(6);
    }
  }
  else if (level > 3.9 && level <= 4.6) {
    if(direction == "Right") {
      turn_on_leds_color_right(7);
      turn_off_leds_color_right(7);
    } else {
      turn_on_leds_color_left(7);
      turn_off_leds_color_left(7);
    }
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    pixels.clear();
    pixels.show();
    previousMillis = currentMillis;
  }
}