#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <CAN.h>
#include "esp_log.h"

// Task handles
TaskHandle_t xCANSendTaskHandle = NULL;
TaskHandle_t xSerialConsoleTaskHandle = NULL;

// Task functions
void vCANSendTask(void *pvParameters);
void vSerialConsoleTask(void *pvParameters);

// Define CAN frame IDs and data
struct CANFrame
{
  unsigned long id;
  byte length;
  byte data[8];
};

CANFrame frames[] = {
    {0x418, 8, {0x30, 0x07, 0x00, 0x03, 0x28, 0x28, 0xF8, 0xA0}},
    {0x46B, 8, {0x01, 0x00, 0xA3, 0x00, 0x01, 0x00, 0x00, 0xC0}},
    {0x47C, 8, {0x64, 0x7D, 0x42, 0x01, 0x91, 0x00, 0x70, 0x04}},
    {0x47D, 8, {0x04, 0xC1, 0x77, 0x95, 0x60, 0x01, 0x20, 0xC4}},
    {0x47F, 8, {0x04, 0x0C, 0xFA, 0x00, 0xEA, 0x72, 0x44, 0x00}},
    {0x25B, 6, {0x04, 0x46, 0x00, 0xFF, 0xFF, 0xC0}},
    {0x69F, 4, {0x75, 0x01, 0x33, 0x6F}}}; // Easylink anti-theft protecion, last 7 number of VIN in reverse order + last byte F.

// Define byte indices for CAN frame modification
#define STANDBY_BYTE_INDEX 0
#define MUTE_BYTE_INDEX 2
#define REVERSE_BYTE_INDEX 0
#define BRIGHTNESS_BYTE_INDEX 1
#define BRIGHTNESS_LEVEL_BYTE_INDEX 2
#define VOLUME_BYTE_INDEX 4

// Define GPIO pins
#define IGNITION_GPIO_PIN 6
#define REVERSE_GPIO_PIN 7
#define DEBOUNCE_DELAY_IGNITION 1000
#define DEBOUNCE_DELAY_OTHERS 100
#define STANDBY_DURATION 3600000 // 60 minutes in milliseconds

// Define initial states for variables
bool ignitionState = false;
bool reverseState = false;
bool brightnessState = false; // false: day, true: night
bool muteState = false;
bool standbyState = false;
bool volumeState = false; // false: normal, true: lowered
int brightnessLevel = 50; // Default brightness level (0-100)
bool gpioEnabled = false;
unsigned long lastIgnitionTime = 0;
unsigned long lastReverseTime = 0;
bool gpioIgnitionState = false;     // Tracks the state of GPIO ignition
unsigned long standbyStartTime = 0; // Tracks the start time of standby mode

// Internal variables
bool serial_init = false;
bool can_enabled = false;

// Function to initialize CAN bus
void initCAN()
{
  while (!CAN.begin(500E3))
  {
    ESP_LOGE("CAN Interface", "Initialization failed, retrying...");
    vTaskDelay(100);
  }
  ESP_LOGI("CAN Interface", "Initialization succeeded!");
}

void stopCAN()
{
  CAN.end();
  ESP_LOGI("CAN Interface", "Shutdown succeeded!");
}

// Function to send a CAN message
void sendCANFrame(unsigned long canId, const byte *data, int len)
{
  ESP_LOGD("CAN Interface", "TX ID 0x%02X DATA %02X", canId, data);
  CAN.beginPacket(canId);
  CAN.write(data, len);
  CAN.endPacket();
  ESP_LOGD("CAN Interface", "TX successful");
}

// Function to update CAN frame data based on variables
void updateCANFrameData(CANFrame *frames)
{
  frames[4].data[BRIGHTNESS_BYTE_INDEX] = brightnessState ? 0x8C : 0x0C;
  frames[4].data[VOLUME_BYTE_INDEX] = volumeState ? 0xEE : 0xEA;
  frames[4].data[BRIGHTNESS_LEVEL_BYTE_INDEX] = map(brightnessLevel, 0, 100, 0x00, 0xFA);
  frames[5].data[REVERSE_BYTE_INDEX] = reverseState ? 0x08 : 0x04;
  frames[3].data[MUTE_BYTE_INDEX] = muteState ? 0x00 : 0x77;
  frames[1].data[STANDBY_BYTE_INDEX] = standbyState ? 0x00 : 0x01;
  ESP_LOGD("CAN", "Frame update successful");
}

// Function to handle GPIO inputs
void handleGPIO()
{
  // If GPIO is disabled, return without processing GPIO inputs
  if (!gpioEnabled)
  {
    return;
  }
  unsigned long currentMillis = millis();

  // Ignition input
  if (currentMillis - lastIgnitionTime >= DEBOUNCE_DELAY_IGNITION)
  {
    bool newIgnitionState = digitalRead(IGNITION_GPIO_PIN);
    if (newIgnitionState != ignitionState)
    {
      if (!newIgnitionState)
      {
        ESP_LOGI("GPIO State machine", "Ignition turned off");
        lastIgnitionTime = currentMillis;
        ignitionState = false;
        // Shut down CAN interface if ignition is off
        if (!ignitionState)
        {
          stopCAN();
        }
      }
      else
      {
        ESP_LOGI("GPIO State machine", "Ignition turned on");
        ignitionState = true;
        // Initialize CAN interface if ignition is on
        if (ignitionState)
        {
          initCAN();
        }
      }
    }
  }

  // Handle reverse input
  if (gpioEnabled && currentMillis - lastReverseTime >= DEBOUNCE_DELAY_OTHERS)
  {
    bool newReverseState = digitalRead(REVERSE_GPIO_PIN);
    if (newReverseState != reverseState)
    {
      if (newReverseState)
      {
        // Reverse engaged
        ESP_LOGI("GPIO State machine", "Reversing engaged");
        reverseState = true;
        volumeState = true;
      }
      else
      {
        // Reverse disengaged
        ESP_LOGI("GPIO State machine", "Reversing disengaged");
        reverseState = false;
        volumeState = false;
      }
    }
  }
}

// Function to print summary of all serial variables
void printVariableSummary()
{
  Serial.println("---CAN status summary----");
  Serial.println("-------------------------");
  Serial.print("Ignition: ");
  Serial.println(ignitionState ? "on" : "off");
  Serial.print("Standby:");
  Serial.println(standbyState ? "yes" : "no");
  Serial.print("Reverse: ");
  Serial.println(reverseState ? "on" : "off");
  Serial.print("Brightness: ");
  Serial.println(brightnessState ? "night" : "day");
  Serial.print("Mute: ");
  Serial.println(muteState ? "on" : "off");
  Serial.print("Volume: ");
  Serial.println(volumeState ? "lowered" : "normal");
  Serial.print("Brightness Level: ");
  Serial.println(brightnessLevel);
  Serial.print("GPIO: ");
  Serial.println(gpioEnabled ? "enable" : "disable");
  Serial.println("-------------------------");
}

void vCANSendTask(void *pvParameters)
{
  (void)pvParameters;
  while (1)
  {
    handleGPIO();

    // Send CAN frames at 10Hz if ignition is on
    if (ignitionState || gpioIgnitionState)
    {
      unsigned long currentMillis = millis();
      static unsigned long lastTxTime = 0;
      if (currentMillis - lastTxTime >= 100)
      {
        // Update CAN frame data based on variables
        updateCANFrameData(frames);
        // Send all CAN frames
        int i = 0;
        if (standbyState)
        {
          i = 1;
        }
        else
        {
          i = 0;
        }
        for (i; i < 6; i++)
        {
          sendCANFrame(frames[i].id, frames[i].data, frames[i].length);
        }
        lastTxTime = currentMillis;
      }

      // Send CAN frame at 1Hz
      static unsigned long lastTxTime69F = 0;
      if (currentMillis - lastTxTime69F >= 1000)
      {
        sendCANFrame(frames[6].id, frames[6].data, frames[6].length); // Send anti-theft frame
        lastTxTime69F = currentMillis;
      }
    }

    // Check if ignition should go into standby mode
    if (!gpioIgnitionState && ignitionState && !standbyStartTime)
    {
      standbyStartTime = millis(); // Start the standby timer
    }

    // Check if standby duration has elapsed and gpio ignition is still false
    if (standbyStartTime && millis() - standbyStartTime >= STANDBY_DURATION && !gpioIgnitionState)
    {
      ignitionState = false; // Set ignition to off after standby duration
      ESP_LOGI("State machine", "Ignition turned off after standby duration");
      standbyState = false;
      standbyStartTime = 0; // Reset standby timer
    }

    // If GPIO ignition is true, reset standby timer
    if (gpioIgnitionState)
    {
      standbyStartTime = 0; // Reset standby timer
      standbyState = false;
    }
    vTaskDelay(10);
  }
}

void vSerialConsoleTask(void *pvParameters)
{
  (void)pvParameters;
  while (1)
  {
    if (Serial.available() > 0)
    {
      String input = Serial.readStringUntil('\n');
      input.trim();
      if (input.length() == 0)
      {
        printVariableSummary();
      }
      else
      {
        // Parse input commands
        if (input.startsWith("ignition"))
        {
          if (input.endsWith("on"))
          {
            ignitionState = true;
            ESP_LOGI("State machine", "Ignition turned on");
            standbyState = false;
            if (!can_enabled)
            {
              ESP_LOGD("State machine", "Try to enable CAN Interface");
              initCAN();
            }
          }
          else if (input.endsWith("off"))
          {
            ignitionState = false;
            standbyStartTime = millis();
            ESP_LOGI("State machine", "Ignition turned off");
            if (can_enabled)
            {
              ESP_LOGD("State machine", "Try to disable CAN Interface");
              stopCAN();
            }
          }
          else if (input.endsWith("standby"))
          {
            // Print current standby duration if ignition is in standby mode
            if (standbyStartTime)
            {
              unsigned long remainingTime = STANDBY_DURATION - (millis() - standbyStartTime);
              standbyState = true;
              ESP_LOGI("State machine", "Ignition in standby mode, remaining time: %d minutes", remainingTime / 60000);
            }
            else
            {
              ESP_LOGI("State machine", "Ignition is not in standby mode");
            }
          }
        }
        else if (input.startsWith("gpio"))
        {
          if (input.endsWith("enable"))
          {
            gpioEnabled = true;
            ESP_LOGI("Variables", "GPIO enabled");
          }
          else if (input.endsWith("disable"))
          {
            gpioEnabled = false;
            ESP_LOGI("Variables", "GPIO disabled");
          }
        }
        else if (input.startsWith("mute"))
        {
          if (input.endsWith("on"))
          {
            muteState = true;
            ESP_LOGI("Variables", "MUTE turned on");
          }
          else if (input.endsWith("off"))
          {
            muteState = false;
            ESP_LOGI("Variables", "MUTE turned off");
          }
        }
        else if (input.startsWith("reverse"))
        {
          if (input.endsWith("on"))
          {
            reverseState = true;
            ESP_LOGI("Variables", "REVERSE turned on");
          }
          else if (input.endsWith("off"))
          {
            reverseState = false;
            ESP_LOGI("Variables", "REVERSE turned off");
          }
        }
        else if (input.startsWith("brightness"))
        {
          if (input.endsWith("day"))
          {
            brightnessState = false;
            ESP_LOGI("Variables", "Brightness set to DAY");
          }
          else if (input.endsWith("night"))
          {
            brightnessState = true;
            ESP_LOGI("Variables", "Brightness set to NIGHT");
          }
        }
        else if (input.startsWith("volume"))
        {
          if (input.endsWith("normal"))
          {
            volumeState = false;
            ESP_LOGI("Variables", "VOLUME set to NORMAL");
          }
          else if (input.endsWith("lowered"))
          {
            volumeState = true;
            ESP_LOGI("Variables", "VOLUME set to LOWERED");
          }
        }
        else
        {
          ESP_LOGE("Variables", "Invalid command received");
        }
      }
    }
    vTaskDelay(10);
  }
}

void setup()
{
  Serial.begin(115200);
  Serial.setTimeout(5000);
  ESP_LOGI("SYS", "Easylink CAN Waker application starting up...");

  //Print state of variables during bootup
  printVariableSummary();
  ESP_LOGD("SYS", "Print actual variable states");

  //Start tasks one per CPU, because we have enough ressources :-)
  vTaskStartScheduler();
  ESP_LOGD("SYS", "Start FreeRTOS scheduler");

  xTaskCreatePinnedToCore(vCANSendTask, "CANSendTask", 8096, NULL, 10, &xCANSendTaskHandle, 0);
  ESP_LOGD("SYS", "Start CANSendTask");
  xTaskCreatePinnedToCore(vSerialConsoleTask, "SerialConsoleTask", 8096, NULL, 10, &xSerialConsoleTaskHandle, 1);
  ESP_LOGD("SYS", "Start SerialConsoleTask");
}

void loop()
{
  //We are using tasks instead
}