#include "constants.h"
#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

#define LOG // Comment this line to turn off logs

static void task_sensors (void *pv);
static void task_switch (void *pv);
static void enable_sensors ();
static void disable_sensors ();

// ------------------------------------------------
// Event struct
// ------------------------------------------------
typedef struct event_s
{
  event_e type;
} event_t;

// ------------------------------------------------
// Sensor structs
// ------------------------------------------------
typedef struct distance_sensor_s
{
  int pin_echo;
  int pin_trigger;
  float cm;
  distance_state_e state;
} distance_sensor_t;

typedef struct light_sensor_s
{
  int pin;
  int level;
  light_state_e state;
} light_sensor_t;

typedef struct switch_sensor_s
{
  int pin;
  int value;
  switch_state_e state;
} switch_sensor_t;

// ------------------------------------------------
// Actuator structs
// ------------------------------------------------
typedef struct led_actuator_s
{
  int pin;
  bool state;
} led_actuator_t;

typedef struct buzzer_actuator_s
{
  int pin;
  int frequency;
} buzzer_actuator_t;

typedef Servo servo_actuator_t;
typedef LiquidCrystal_I2C lcd_actuator_t;

// ------------------------------------------------
// Global variables
// ------------------------------------------------
state_e current_state;
event_t event;
distance_sensor_t distance_sensor;
switch_sensor_t switch_sensor;
light_sensor_t light_sensor;
led_actuator_t led_actuator;
buzzer_actuator_t buzzer_actuator;
servo_actuator_t servo_actuator;
lcd_actuator_t lcd_actuator (LCD_I2C_ADDRESS, LCD_COLUMNS, LCD_ROWS);
unsigned long last_interrupt_time;
unsigned long current_turn;
unsigned long last_turn;
float speed;
QueueHandle_t events_queue;
QueueHandle_t turns_queue;
static TaskHandle_t h_task_switch;
static TaskHandle_t h_task_sensors;

// ------------------------------------------------
// Logs
// ------------------------------------------------
void
console_log (const char *state, const char *event)
{
#ifdef LOG
  Serial.println ("------------------------------------------------");
  Serial.println (state);
  Serial.println (event);
  Serial.println ("------------------------------------------------");
#endif
}

void
console_log (const char *msg)
{
#ifdef LOG
  Serial.println (msg);
#endif
}

void
console_log (int val)
{
#ifdef LOG
  Serial.println (val);
#endif
}

void
console_log (float val_float)
{
#ifdef LOG
  Serial.println (val_float);
#endif
}

// ------------------------------------------------
// Sensors logic
// ------------------------------------------------
float
read_distance_sensor ()
{
  long pulse_time;
  digitalWrite (PIN_D_ULTRASONIC_TRIG, LOW);
  delayMicroseconds (DELAY_MICROSECONDS_2);
  digitalWrite (PIN_D_ULTRASONIC_TRIG, HIGH);
  delayMicroseconds (DELAY_MICROSECONDS_10);
  digitalWrite (PIN_D_ULTRASONIC_TRIG, LOW);
  pulse_time = pulseIn (PIN_D_ULTRASONIC_ECHO, HIGH, ULTRASONIC_TIMEOUT_US);

  // convert to cm
  return pulse_time * SPEED_OF_SOUND;
}

event_t
activate_distance_sensor ()
{
  event_t new_event;

  distance_sensor.cm = read_distance_sensor ();
  distance_state_e new_state;

  if (distance_sensor.cm < THRESHOLD_TOO_CLOSE)
    new_state = DIST_TOO_CLOSE;
  else if (distance_sensor.cm < THRESHOLD_CLOSE)
    new_state = DIST_CLOSE;
  else if (distance_sensor.cm < THRESHOLD_MEDIUM)
    new_state = DIST_MEDIUM;
  else
    new_state = DIST_FAR;

  switch (new_state)
    {
    case DIST_TOO_CLOSE:
      if (distance_sensor.state != DIST_TOO_CLOSE)
        new_event.type = EV_TOO_CLOSE_DISTANCE;
      else
        new_event.type = EV_CONTINUE;
      break;

    case DIST_CLOSE:
      new_event.type = EV_CLOSE_DISTANCE;
      break;

    case DIST_MEDIUM:
      if (distance_sensor.state != DIST_MEDIUM)
        new_event.type = EV_MEDIUM_DISTANCE;
      else
        new_event.type = EV_CONTINUE;
      break;

    case DIST_FAR:
      if (distance_sensor.state != DIST_FAR)
        new_event.type = EV_FAR_DISTANCE;
      else
        new_event.type = EV_CONTINUE;
      break;
    }

  distance_sensor.state = new_state;
  return new_event;
}

event_t
deactivate_distance_sensor ()
{
  event_t new_event;
  new_event.type = EV_CONTINUE;
  return new_event;
}

int 
read_light_sensor(int pin)
{
  return analogRead(pin);
}

event_t
check_light_sensor()
{
  light_sensor.level = read_light_sensor(light_sensor.pin);
  light_state_e new_state;
  event_t new_event;

  if (light_sensor.level < THRESHOLD_LIGHT_LOW)
    new_state = LIGHT_LOW;
  else if (light_sensor.level > THRESHOLD_LIGHT_HIGH)
    new_state = LIGHT_HIGH;
  else
    new_state = light_sensor.state;

  switch (new_state)
  {
  case LIGHT_LOW:
    if (light_sensor.state != LIGHT_LOW)
      new_event.type = EV_LOW_LIGHTING;
    else
      new_event.type = EV_CONTINUE;
    break;

  case LIGHT_HIGH:
    if (light_sensor.state != LIGHT_HIGH)
      new_event.type = EV_HIGH_LIGHTING;
    else
      new_event.type = EV_CONTINUE;
    break;
  }

  light_sensor.state = new_state;
  return new_event;
}

void IRAM_ATTR
read_halleffect_sensor ()
{
  unsigned long interrupt_time = micros ();
  if (interrupt_time - last_interrupt_time > HALL_EFFECT_DEBOUNCE_US)
    {
      last_interrupt_time = interrupt_time;
      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
      xQueueSendFromISR (turns_queue, &interrupt_time, &xHigherPriorityTaskWoken);
      if (xHigherPriorityTaskWoken)
        {
          portYIELD_FROM_ISR ();
        }
    }
}

event_t
check_halleffect_sensor ()
{
  unsigned long current_time;
  event_t new_event;
  if (xQueueReceive (turns_queue, &current_time, QUEUE_NO_WAIT) == pdTRUE)
    {
      last_turn = current_turn;
      current_turn = current_time;
      if (current_turn > last_turn)
        {
          float distanceTraveled = PI * WHEEL_DIAMETER;
          speed = (distanceTraveled / ((current_turn - last_turn) / MICROS_IN_SEC)) * MS_TO_KMH;
        }
      else
        {
          speed = INIT_SPEED;
        }

      new_event.type = EV_WHEEL_TURN;
      return new_event;
    }

  current_time = micros ();
  if (current_time - current_turn > SPEED_TIMEOUT_US)
    {
      speed = INIT_SPEED;
      last_turn = current_turn;
      current_turn = current_time;
      new_event.type = EV_TIMEOUT;
      return new_event;
    }
  new_event.type = EV_CONTINUE;
  return new_event;
}

int
read_switch_sensor (int pin)
{
  return digitalRead (pin);
}

event_t
check_switch_sensor ()
{
  switch_sensor.value = read_switch_sensor (switch_sensor.pin);
  event_t new_event;
  switch_state_e new_state;
  if (switch_sensor.value == LOW)
    new_state = SWITCH_ON;
  else
    new_state = SWITCH_OFF;

  switch (new_state)
    {
    case SWITCH_OFF:
      if (switch_sensor.state != SWITCH_OFF)
        new_event.type = EV_TURN_OFF;
      else
        new_event.type = EV_CONTINUE;
      break;

    case SWITCH_ON:
      if (switch_sensor.state != SWITCH_ON)
        new_event.type = EV_TURN_ON;
      else
        new_event.type = EV_CONTINUE;
      break;
    }

  switch_sensor.state = new_state;
  return new_event;
}

// ------------------------------------------------
// Actuators logic
// ------------------------------------------------
void
turn_on_screen ()
{
  lcd_actuator.backlight ();
  lcd_actuator.clear ();
  lcd_actuator.setCursor (LCD_CURSOR_COL_0, LCD_CURSOR_ROW_0);
  lcd_actuator.print ("Velocidad:");
  lcd_actuator.setCursor (LCD_CURSOR_COL_0, LCD_CURSOR_ROW_1);
  lcd_actuator.print ("  0.0 km/h");
}

void
turn_off_screen ()
{
  lcd_actuator.clear ();
  lcd_actuator.noBacklight ();
}

void
update_screen ()
{
  lcd_actuator.setCursor (LCD_CURSOR_COL_0, LCD_CURSOR_ROW_1);
  char buffer[LCD_COLUMNS];
  snprintf (buffer, sizeof (buffer), "%5.1f", speed);
  lcd_actuator.print (buffer);
  lcd_actuator.print (" km/h");
}

void
play_buzzer (int frequency)
{
  buzzer_actuator.frequency = frequency;
  ledcWriteTone (PIN_P_BUZZER, frequency);
}

void
stop_buzzer ()
{
  buzzer_actuator.frequency = FREQUENCY_OFF;
  ledcWriteTone (PIN_P_BUZZER, FREQUENCY_OFF);
}

void
turn_on_led ()
{
  led_actuator.state = HIGH;
  digitalWrite (PIN_D_LED, HIGH);
}

void
turn_off_led ()
{
  led_actuator.state = LOW;
  digitalWrite (PIN_D_LED, LOW);
}

void
activate_brakes (float distance)
{
  int angle = get_angle ((int)distance, THRESHOLD_CLOSE, THRESHOLD_TOO_CLOSE,
                         MIN_ANGLE, MAX_ANGLE);
  servo_actuator.write (angle);
}

void
deactivate_brakes ()
{
  servo_actuator.write (MIN_ANGLE);
}

int
get_angle (float distance, float max_distance, float min_distance,
           int min_angle, int max_angle)
{
  if (distance <= min_distance)
    return max_angle;

  float ratio = (max_distance - distance) / (max_distance - min_distance);
  int angle = min_angle + (max_angle - min_angle) * ratio;
  return angle;
}

void
init_sensors ()
{
  light_sensor.state = LIGHT_HIGH;
  distance_sensor.state = DIST_FAR;
}

// ------------------------------------------------
// Event detection
// ------------------------------------------------
int sensor_index = SENSOR_INIT_INDEX;
event_t (*check_sensor[NUM_SENSORS]) () = {deactivate_distance_sensor, check_light_sensor, check_halleffect_sensor };

static void 
task_sensors (void *pv) // Disabled when ST_OFF
{
  while (TRUE) 
    {
      event_t new_event = check_sensor[sensor_index]();

      if (new_event.type != EV_CONTINUE)
        xQueueSend(events_queue, &new_event.type, QUEUE_NO_WAIT);
      
      sensor_index = ++sensor_index % NUM_SENSORS;
      vTaskDelay (pdMS_TO_TICKS (TMP_EVENTS_MS));
    }
}

static void 
task_switch (void* pv) // Always enabled
{
  while(TRUE)
    {
      event_t new_event = check_switch_sensor ();
      if (new_event.type != EV_CONTINUE)
        xQueueSend (events_queue, &new_event.type, QUEUE_NO_WAIT);
      vTaskDelay (pdMS_TO_TICKS (TMP_EVENTS_MS));
    }
}

static inline void
enable_sensors()  
{
  if (h_task_sensors) 
  {
    vTaskResume (h_task_sensors);
    gpio_intr_enable((gpio_num_t)PIN_D_HALL_EFFECT);
  }  
}

static inline void
disable_sensors() 
{ 
  if (h_task_sensors)
  {
    gpio_intr_disable((gpio_num_t)PIN_D_HALL_EFFECT);
    vTaskSuspend (h_task_sensors);
  } 
}

// ------------------------------------------------
// Initialization
// ------------------------------------------------
void
start ()
{
  Serial.begin (SERIAL_BAUD_RATE);

  events_queue = xQueueCreate (QUEUE_SIZE_EVENTS, sizeof (event_e));
  turns_queue = xQueueCreate (QUEUE_SIZE_TURNS, sizeof (unsigned long));

  pinMode (PIN_D_SWITCH, INPUT_PULLUP);
  switch_sensor.pin = PIN_D_SWITCH;
  switch_sensor.state = SWITCH_OFF;

  pinMode (PIN_D_ULTRASONIC_TRIG, OUTPUT);
  pinMode (PIN_D_ULTRASONIC_ECHO, INPUT);
  distance_sensor.pin_echo = PIN_D_ULTRASONIC_ECHO;
  distance_sensor.pin_trigger = PIN_D_ULTRASONIC_TRIG;
  distance_sensor.state = DIST_FAR;

  pinMode (PIN_D_HALL_EFFECT, INPUT_PULLUP);
  attachInterrupt (digitalPinToInterrupt (PIN_D_HALL_EFFECT),
                   read_halleffect_sensor, RISING);

  pinMode (PIN_D_LED, OUTPUT);
  led_actuator.pin = PIN_D_LED;

  pinMode (PIN_A_LDR, INPUT);
  light_sensor.pin = PIN_A_LDR;
  light_sensor.state = LIGHT_HIGH;

  buzzer_actuator.pin = PIN_P_BUZZER;
  buzzer_actuator.frequency = FREQUENCY_OFF;
  ESP32PWM::timerCount[PWM_TIMER_FOR_BUZZER] = PWM_CHANNELS_PER_TIMER;
  ledcAttachChannel (PIN_P_BUZZER, BUZZER_START_HZ, BUZZER_RES_BITS, BUZZER_CHANNEL); 

  servo_actuator.setPeriodHertz (SERVO_HZ); 
  servo_actuator.attach (PIN_P_SERVO, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  servo_actuator.write (SERVO_MIN_ANGLE);

  lcd_actuator.init ();

  current_state = ST_OFF;

  current_turn = micros ();
  last_turn = current_turn;
  last_interrupt_time = current_turn;
  speed = INIT_SPEED;

  h_task_sensors = nullptr;
  h_task_switch = nullptr;

  xTaskCreate (task_sensors, "Task Sensors", TASK_STACK_SIZE, NULL, TASK_PRIORITY_SENSORS, &h_task_sensors);
  xTaskCreate (task_switch, "Task Switch", TASK_STACK_SIZE, NULL, TASK_PRIORITY_SWITCH, &h_task_switch);

  if (h_task_sensors)
    disable_sensors ();

  xTaskCreate (task_loop, "Task Loop", TASK_STACK_SIZE, NULL, TASK_PRIORITY_FSM, NULL);
}

// ------------------------------------------------
// Finite State Machine
// ------------------------------------------------
void
fsm ()
{
  switch (current_state)
    {
    case ST_OFF:
      switch (event.type)
        {
        case EV_TURN_ON:
          turn_on_screen ();
          init_sensors ();
          console_log ("ST_OFF", "EV_TURN_ON");
          current_state = ST_STOPPED;
          check_sensor[DISTANCE_SENSOR_INDEX] = deactivate_distance_sensor;
          enable_sensors ();
          break;

        default:
          break;
        }
      break;

    case ST_STOPPED:
      switch (event.type)
        {
        case EV_LOW_LIGHTING:
          turn_on_led ();
          console_log ("ST_STOPPED", "EV_LOW_LIGHTING");
          break;

        case EV_HIGH_LIGHTING:
          turn_off_led ();
          console_log ("ST_STOPPED", "EV_HIGH_LIGHTING");
          break;

        case EV_WHEEL_TURN:
          update_screen ();
          console_log ("ST_STOPPED", "EV_WHEEL_TURN");
          current_state = ST_RIDING;
          distance_sensor.state = DIST_FAR;
          check_sensor[DISTANCE_SENSOR_INDEX] = activate_distance_sensor;
          break;

        case EV_TURN_OFF:
          console_log ("ST_STOPPED", "EV_TURN_OFF");
          disable_sensors ();
          deactivate_brakes ();
          stop_buzzer();
          turn_off_led ();
          turn_off_screen ();
          current_state = ST_OFF;
          break;

        default:
          break;
        }
      break;

    case ST_RIDING:
      switch (event.type)
        {
        case EV_LOW_LIGHTING:
          turn_on_led ();
          console_log ("ST_RIDING", "EV_LOW_LIGHTING");
          break;

        case EV_HIGH_LIGHTING:
          turn_off_led ();
          console_log ("ST_RIDING", "EV_HIGH_LIGHTING");
          break;

        case EV_WHEEL_TURN:
          update_screen ();
          console_log ("ST_RIDING", "EV_WHEEL_TURN");
          break;

        case EV_FAR_DISTANCE:
          deactivate_brakes ();
          stop_buzzer();
          console_log ("ST_RIDING", "EV_FAR_DISTANCE");
          break;

        case EV_MEDIUM_DISTANCE:
          deactivate_brakes ();
          play_buzzer(FREQUENCY_MEDIUM);
          console_log ("ST_RIDING", "EV_MEDIUM_DISTANCE");
          break;

        case EV_CLOSE_DISTANCE:
          activate_brakes (distance_sensor.cm);
          play_buzzer(FREQUENCY_CLOSE);
          console_log ("ST_RIDING", "EV_CLOSE_DISTANCE");
          current_state = ST_BRAKING;
          break;

        case EV_TOO_CLOSE_DISTANCE:
          activate_brakes (distance_sensor.cm);
          play_buzzer(FREQUENCY_TOO_CLOSE);
          console_log ("ST_RIDING", "EV_TOO_CLOSE_DISTANCE");
          current_state = ST_BRAKING;
          break;

        case EV_TIMEOUT:
          deactivate_brakes ();
          stop_buzzer();
          update_screen ();
          check_sensor[DISTANCE_SENSOR_INDEX] = deactivate_distance_sensor;
          console_log ("ST_RIDING", "EV_TIMEOUT");
          current_state = ST_STOPPED;
          break;

        case EV_TURN_OFF:
          console_log ("ST_RIDING", "EV_TURN_OFF");
          disable_sensors ();
          deactivate_brakes ();
          stop_buzzer();
          turn_off_led ();
          turn_off_screen ();
          current_state = ST_OFF;
          break;

        default:
          break;
        }
      break;

    case ST_BRAKING:
      switch (event.type)
        {
        case EV_LOW_LIGHTING:
          turn_on_led ();
          console_log ("ST_BRAKING", "EV_LOW_LIGHTING");
          break;

        case EV_HIGH_LIGHTING:
          turn_off_led ();
          console_log ("ST_BRAKING", "EV_HIGH_LIGHTING");
          break;

        case EV_WHEEL_TURN:
          update_screen ();
          console_log ("ST_BRAKING", "EV_WHEEL_TURN");
          break;

        case EV_FAR_DISTANCE:
          deactivate_brakes ();
          stop_buzzer();
          console_log ("ST_BRAKING", "EV_FAR_DISTANCE");
          current_state = ST_RIDING;
          break;

        case EV_MEDIUM_DISTANCE:
          deactivate_brakes ();
          play_buzzer(FREQUENCY_MEDIUM);
          console_log ("ST_BRAKING", "EV_MEDIUM_DISTANCE");
          current_state = ST_RIDING;
          break;

        case EV_CLOSE_DISTANCE:
          activate_brakes (distance_sensor.cm);
          play_buzzer(FREQUENCY_CLOSE);
          console_log ("ST_BRAKING", "EV_CLOSE_DISTANCE");
          break;

        case EV_TOO_CLOSE_DISTANCE:
          activate_brakes (distance_sensor.cm);
          play_buzzer(FREQUENCY_TOO_CLOSE);
          console_log ("ST_BRAKING", "EV_TOO_CLOSE_DISTANCE");
          break;

        case EV_TIMEOUT:
          deactivate_brakes ();
          stop_buzzer();
          update_screen ();
          check_sensor[DISTANCE_SENSOR_INDEX] = deactivate_distance_sensor;
          console_log ("ST_BRAKING", "EV_TIMEOUT");
          current_state = ST_STOPPED;
          break;

        case EV_TURN_OFF:
          console_log ("ST_BRAKING", "EV_TURN_OFF");
          disable_sensors ();
          deactivate_brakes ();
          stop_buzzer();
          turn_off_led ();
          turn_off_screen ();
          current_state = ST_OFF;
          break;

        default:
          break;
        }
      break;
    }
}

void
task_loop (void *pv)
{
  while (TRUE)
    {
      xQueueReceive (events_queue, &event.type, portMAX_DELAY);
      fsm ();
    }
}

void
setup ()
{
  start ();
}

void
loop ()
{
}