#include <FastLED.h>

// Declaring the type of LEDs system and their settings
#define LED_TYPE NEOPIXEL
#define COLOR_ORDER GRB

#define DATA_PIN 13

// This will be set with a potentiometer but we also have to limit the value
// so not to draw too much power
#define BRIGHTNESS 64

// Declaring the METRO LINES STATIONS from the wiki Page :
// https://en.wikipedia.org/wiki/Montreal_Metro

// Note that this indicates the number of stops for each lines for a total of 73
// However there are stations that are presents on multiple lines:
// Ex : Berri Uqam, Linoel Groulx, Jean talon, Snowdonw.
// so the total number of Stations is 68
#define NUM_LEDS_GREEN 27
#define NUM_LEDS_ORANGE 31
#define NUM_LEDS_YELLOW 3
#define NUM_LEDS_BLUE 12

// Following the Official STM Metro lines Index :
// GREEN | ORANGE | YELLOW | BLUE |
#define GREEN_LINE 1
#define ORANGE_LINE 2
#define YELLOW_LINE 4
#define BLUE_LINE 5

// LEDs Constants
#define LED_ON 1
#define LED_OFF 0

#define US 1 // US:UpStream Direction
#define DS 0 // DS:DownStream Direction

#define UP 1    // LEDs going into the US Direction
#define DOWN -1 // LEDs going into the DS Direction

#define TOTAL_NUM_LEDS NUM_LEDS_ORANGE + NUM_LEDS_BLUE + NUM_LEDS_GREEN + NUM_LEDS_YELLOW // TOTAL : 73

// Number of frames per sec -- OBSOLETE
#define MAX_FRAMES 60
int REFRESH_RATE = 1000 * (60 / MAX_FRAMES);

// Declaring Metro lines on the LEDStrip
uint8_t LEDs_ORANGE[NUM_LEDS_ORANGE];
uint8_t LEDs_BLUE[NUM_LEDS_BLUE];
uint8_t LEDs_GREEN[NUM_LEDS_GREEN];
uint8_t LEDs_YELLOW[NUM_LEDS_YELLOW];

// Eventually we would like to only have 1 array  that will combine the data of static_led and moving_led
// Ex: ledstatus = [0 1 0 0 -1 0 0 2]
// Where :
// 1 : There's 1 metro going upstream
// - : There's 1 metro going downstream
// 2 : 2 Trains going to the opposite directions are in the same station
// 0 : No trains
uint8_t static_leds[TOTAL_NUM_LEDS];
uint8_t moving_leds[TOTAL_NUM_LEDS];

// Declaring the in-realtime Variables
int8_t METRO_Location[TOTAL_NUM_LEDS];

// ******************** Theses parameters will be used for offline train simulation ********************

// Distance between Metro Stations : https://en.wikipedia.org/wiki/List_of_Montreal_Metro_stations
// Following the Official STM Order :
// GREEN  | Angrignon -> Honoré-Beaugrand |
// ORANGE | Côte-Vertu -> Montmorency |
// YELLOW | Berri–UQAM -> Longueuil–Université-de-Sherbrooke |
// BLUE   | Snowdon -> Saint-Michel |
/*
uint16_t Distances_Between_Stations[] =
    {844, 1063, 761, 564, 812, 707, 1077, 1388, 682, 593, 297, 346, 354, 337, 379, 495, 1158, 1004,
     383, 767, 622, 896, 782, 519, 622, 717, 777, 1282, 787, 988, 451, 693, 884, 1407, 1451, 580,
     759, 531, 382, 393, 357, 371, 721, 579, 932, 500, 746, 541, 712, 977, 826, 1280, 772, 1102,
     2074, 848, 2362, 1572, 960, 765, 668, 1091, 729, 728, 491, 472, 840, 645, 608};
*/
// | WARNING max value for uint16_t : 65,535 -> 65 km

uint16_t Distances_US[] =
    {0, 844, 1907, 2668, 3232, 4044, 4751, 5828, 7216, 7898, 8491, 8788, 9134, 9488,
     9825, 10204, 10699, 11857, 12861, 13244, 14011, 14633, 15529, 16311, 16830, 17452, 18169, 0,
     777, 2059, 2846, 3834, 4285, 4978, 5862, 7269, 8720, 9300, 10059, 10590, 10972, 11365,
     11722, 12093, 12814, 13393, 14325, 14825, 15571, 16112, 16824, 17801, 18627, 19907, 20679, 21781,
     23855, 24703, 0, 2362, 3934, 0, 960, 1725, 2393, 3484, 4213, 4941, 5432, 5904,
     6744, 7389, 7997};

uint16_t Distances_DS[] =
    {18169, 17325, 16262, 15501, 14937, 14125, 13418, 12341, 10953, 10271, 9678, 9381, 9035, 8681,
     8344, 7965, 7470, 6312, 5308, 4925, 4158, 3536, 2640, 1858, 1339, 717, 0, 24703, 23926, 22644,
     21857, 20869, 20418, 19725, 18841, 17434, 15983, 15403, 14644, 14113, 13731, 13338, 12981,
     12610, 11889, 11310, 10378, 9878, 9132, 8591, 7879, 6902, 6076, 4796, 4024, 2922, 848,
     0, 3934, 1572, 0, 7997, 7037, 6272, 5604, 4513, 3784, 3056, 2565, 2093, 1253, 608, 0};

uint8_t v_avg[TOTAL_NUM_LEDS - 1];
uint8_t rush_coef[TOTAL_NUM_LEDS - 1]; // To divide by 100

unsigned long TimeStamps; // Current time stamp
unsigned long TimeStamps_US[TOTAL_NUM_LEDS];
unsigned long TimeStamps_DS[TOTAL_NUM_LEDS];

// ******************** End of offline train simulation parameters ********************

int led1 = 0;
int led2 = 20; // 35 ;
int led3 = 45;
int led4 = 65;

// Fetch Current Time over the Internet : https://randomnerdtutorials.com/epoch-unix-time-esp32-arduino/

// Functions headers
uint8_t move_led(uint8_t, int8_t);
uint8_t find_Metro_lim(uint8_t, int8_t);
uint8_t find_metro_line(uint8_t);
void init_metro_arrays(int8_t, unsigned long,
                       uint8_t, uint8_t,
                       uint8_t, uint8_t,
                       uint8_t, uint8_t);

void get_StationsDistances();

// Creating the LEDSTRIP
CRGB leds[TOTAL_NUM_LEDS];

void setup()
{
  Serial.begin(115200);
  Serial.println("Hello, ESP32!\nFRAMERATE : " + String(REFRESH_RATE));
  Serial.println("TOTAL NUMB OF LEDS : " + String(TOTAL_NUM_LEDS));

  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, TOTAL_NUM_LEDS);

  // Initializing the Arrays (checkout the intested function : init_metro_arrays())
  uint8_t led_idx = 0;
  for (int i = 0; i < TOTAL_NUM_LEDS; i++)
  {
    static_leds[i] = 0;
    moving_leds[i] = 0;
  }


  for (int i = 0; i < NUM_LEDS_GREEN; i++)
  {
    LEDs_GREEN[i] = led_idx;
    Serial.print(String(LEDs_GREEN[i]) + " ");
    led_idx++;
  }

  Serial.println(" ");
  for (int i = 0; i < NUM_LEDS_ORANGE; i++)
  {
    LEDs_ORANGE[i] = led_idx;
    Serial.print(String(LEDs_ORANGE[i]) + " ");
    led_idx++;
  }
  
  Serial.println(" ");
  for (int i = 0; i < NUM_LEDS_YELLOW; i++)
  {
    LEDs_YELLOW[i] = led_idx;
    Serial.print(String(LEDs_YELLOW[i]) + " ");
    led_idx++;
  }

  Serial.println(" ");
  for (int i = 0; i < NUM_LEDS_BLUE; i++)
  {
    LEDs_BLUE[i] = led_idx;
    Serial.print(String(LEDs_BLUE[i]) + " ");
    led_idx++;
  }


  Serial.println("\n");

  // Verifications

  Serial.print("Green : " + String(LEDs_GREEN[0]));
  Serial.print(" TO : " + String(LEDs_GREEN[NUM_LEDS_GREEN - 1]) + "\t");

  Serial.print("\nOrrange : " + String(LEDs_ORANGE[0]));
  Serial.print(" TO : " + String(LEDs_ORANGE[NUM_LEDS_ORANGE - 1]) + "\t");

  Serial.print("\nYellow : " + String(LEDs_YELLOW[0]));
  Serial.print(" TO : " + String(LEDs_YELLOW[NUM_LEDS_YELLOW - 1]) + "\n");

  Serial.print("\nBlue : " + String(LEDs_BLUE[0]));
  Serial.print(" TO : " + String(LEDs_BLUE[NUM_LEDS_BLUE - 1]) + "\t");




  // Setup completed
  FastLED.setBrightness(BRIGHTNESS);
  FastLED.clear();
  Serial.println("Settup Complete\n");
}

void loop()
{
  // Clear all the LEDs for good measure
  FastLED.clear();

  // For the current Simulation we are just incrementing the LED postion
  // After a certain delay

  for (int sim_iter = 0; sim_iter < TOTAL_NUM_LEDS - 1; sim_iter++)
  {

    led1 = move_led(led1, DOWN);
    led2 = move_led(led2, UP);
    led3 = move_led(led3, UP);
    led4 = move_led(led4, DOWN);

    // ****** For debugging ******
    Serial.print("\t ##### Iteration : ");
    Serial.print(sim_iter);
    Serial.println(" ##### ");

    Serial.println("Current POS ");
    for (int j = 0; j < TOTAL_NUM_LEDS - 1; j++)
    {
      Serial.print(static_leds[j]);
      Serial.print(" ");
    }
    Serial.println(" ");
    Serial.println("Moving ");
    for (int j = 0; j < TOTAL_NUM_LEDS - 1; j++)
    {
      Serial.print(moving_leds[j]);
      Serial.print(" ");
    }
    Serial.println("\n");

    // ****** End of display & debugging ******

    for (int idx = 0; idx < TOTAL_NUM_LEDS - 2; idx++)
    {

      // First, TUrn all leds off (black)
      leds[idx] = CRGB::Black;
      FastLED.show();

      // Light up the non moving LEDs
      if (static_leds[idx] == LED_ON)
      {
        leds[idx] = CRGB::Green;
        FastLED.show();
      }

      // Do a special animation with the set of Moving LEDs, this helps clarifywhere the train is heading
      // >>> Ideas for animations :
      // 1 - Solid Led different color (current Option)
      // 2 - Blinking Led (fast pace / slow pace )
      // 3 - THe worm animation or a wave
      // 4 - A projectile that runs through the next 3 leds

      if (moving_leds[idx] == LED_ON)
      {
        leds[idx] = CRGB::Aqua;

        FastLED.show();
      }
    }
    // Displays the terminuses in Red for debugging purposes
    leds[LEDs_ORANGE[NUM_LEDS_ORANGE - 1]] = CRGB::Red;
    leds[LEDs_BLUE[NUM_LEDS_BLUE - 1]] = CRGB::Red;
    leds[LEDs_GREEN[NUM_LEDS_GREEN - 1]] = CRGB::Red;
    leds[LEDs_YELLOW[NUM_LEDS_YELLOW - 1]] = CRGB::Red;
    FastLED.show();
    delay(1000);
  }
}

uint8_t move_led(uint8_t idx, int8_t direction)
{
  int US_lim;
  int DS_lim;
  int next_pos;

  static_leds[idx] = LED_OFF;
  moving_leds[idx] = LED_OFF;

  US_lim = find_Metro_lim(idx, US);
  DS_lim = find_Metro_lim(idx, DS);

  next_pos = idx + direction;

  Serial.print("\nLED POS : " + String(idx) + "\t");
  Serial.print("DIRECTION : " + String(direction) + "\t");
  Serial.print("DS LIM : " + String(DS_lim) + "\t");
  Serial.print("US LIM : " + String(US_lim) + "\t");
  Serial.println("NEXT POS : " + String(next_pos));

  if (direction == UP && next_pos > US_lim)
  {
    Serial.println("US LIM REACHED! NEXT POS : " + String(DS_lim) + "\n");
    // next_pos = DS_lim;
    return DS_lim;
  }
  else if (direction == DOWN && next_pos < DS_lim)
  {
    Serial.println("DS LIM REACHED! NEXT POS : " + String(US_lim) + "\n");
    // next_pos = US_lim;
    return US_lim;
  }

  moving_leds[next_pos] = LED_OFF;
  static_leds[next_pos] = LED_ON;
  moving_leds[next_pos + direction] = LED_ON;

  return next_pos;
}

uint8_t find_Metro_lim(uint8_t led_pos, int8_t direction)
{
  int US_lim;
  int DS_lim;

  Serial.print("\nMETRO LINE POS : " + String(led_pos) + "\t  RES : " + String(find_metro_line(led_pos)));

  switch (find_metro_line(led_pos))
  {
  case GREEN_LINE:
    US_lim = LEDs_GREEN[NUM_LEDS_GREEN - 1];
    DS_lim = LEDs_GREEN[0];
    break;
  case ORANGE_LINE:
    US_lim = LEDs_ORANGE[NUM_LEDS_ORANGE - 1];
    DS_lim = LEDs_ORANGE[0];
    break;
  case YELLOW_LINE:
    US_lim = LEDs_YELLOW[NUM_LEDS_YELLOW - 1];
    DS_lim = LEDs_YELLOW[0];
    break;
  case BLUE_LINE:
    US_lim = LEDs_BLUE[NUM_LEDS_BLUE - 1];
    DS_lim = LEDs_BLUE[0];
    break;
  }

  Serial.println("\tMETRO LIM DS : " + String(DS_lim) + "\t LIM US : " + String(US_lim));
  return (direction == US) ? US_lim : DS_lim;
}

uint8_t find_metro_line(uint8_t led_pos)
{
  if (LEDs_GREEN[0] <= led_pos && led_pos <= LEDs_GREEN[NUM_LEDS_GREEN - 1])
  {
    return GREEN_LINE;
  }
  else if (LEDs_ORANGE[0] <= led_pos && led_pos <= LEDs_ORANGE[NUM_LEDS_ORANGE - 1])
  {
    return ORANGE_LINE;
  }
  else if (LEDs_YELLOW[0] <= led_pos && led_pos <= LEDs_YELLOW[NUM_LEDS_YELLOW - 1])
  {
    return YELLOW_LINE;
  }
  else if (LEDs_BLUE[0] <= led_pos && led_pos <= LEDs_BLUE[NUM_LEDS_BLUE - 1])
  {
    return BLUE_LINE;
  }
  else
  {
    return 0;
  }
}

void init_metro_arrays(int8_t *METRO_Location, unsigned long *TimeStamps,
                       uint8_t *static_leds, uint8_t *moving_leds,
                       uint8_t *LEDs_ORANGE, uint8_t *LEDs_BLUE,
                       uint8_t *LEDs_GREEN, uint8_t *LEDs_YELLOW)
{
  // Following the Official STM Order : GREEN | ORANGE | YELLOW | BLUE |
  for (int led_idx = 0; led_idx < TOTAL_NUM_LEDS; led_idx++)
  {
    static_leds[led_idx] = 0;
    moving_leds[led_idx] = 0;
    TimeStamps[led_idx] = 0;

    if (led_idx < NUM_LEDS_GREEN)
    { // GREEN LINE
      LEDs_GREEN[led_idx] = led_idx;
      Serial.print(String(LEDs_GREEN[led_idx]));
    }
    else if (led_idx < NUM_LEDS_GREEN + NUM_LEDS_ORANGE)
    { // ORANGE LINE
      LEDs_ORANGE[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE)] = led_idx;
      Serial.print(String(LEDs_ORANGE[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE)]));
    }
    else if (led_idx < NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE)
    { // YELLOW LINE
      LEDs_YELLOW[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE)] = led_idx;
      Serial.print(String(LEDs_YELLOW[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE)]));
    }
    else if (led_idx < NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE + NUM_LEDS_YELLOW)
    { // BLUE LINE
      LEDs_BLUE[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE + NUM_LEDS_YELLOW)] = led_idx;
      Serial.print(String(LEDs_BLUE[led_idx - (NUM_LEDS_GREEN + NUM_LEDS_ORANGE + NUM_LEDS_BLUE + NUM_LEDS_YELLOW)]));
    }

    Serial.print(" ");
  }

  // Verifications
  Serial.print("\n\nOrrange : " + String(LEDs_ORANGE[0]));
  Serial.print(" TO : " + String(LEDs_ORANGE[NUM_LEDS_ORANGE - 1]) + "\t");

  Serial.print("Blue : " + String(LEDs_BLUE[0]));
  Serial.print(" TO : " + String(LEDs_BLUE[NUM_LEDS_BLUE - 1]) + "\t");

  Serial.print("Green : " + String(LEDs_GREEN[0]));
  Serial.print(" TO : " + String(LEDs_GREEN[NUM_LEDS_GREEN - 1]) + "\t");

  Serial.print("Yellow : " + String(LEDs_YELLOW[0]));
  Serial.print(" TO : " + String(LEDs_YELLOW[NUM_LEDS_YELLOW - 1]) + "\n");
}

/*
void get_StationsDistances()
{ // uint16_t *Distance_US[], uint16_t *Distance_DS[]){

  int sum_distance_US = 0;
  int sum_distance_DS = 0;

  int i_DS = 0;
  for (int i_US = 0; sizeof(Distance_Between_Stations); i_US++)
  {
    i_DS = sizeof(Distance_Between_Stations) - i_US;

    sum_distance_US += Distance_Between_Stations[i_US];
    sum_distance_DS += Distance_Between_Stations[i_DS];

    Distance_US[i_US] = sum_distance_US;
    Distance_DS[i_DS] = sum_distance_DS;

    // Reset the counters whenever we switch Metro lines
    if (find_Metro_lim(i_US, US) >= i_US + 1)
    {
      sum_distance_US = 0;
    }
    if (find_Metro_lim(i_DS, DS) <= i_DS + 1)
    {
      sum_distance_DS = 0;
    }
  }
}
*/