#include <stdio.h>
#include <math.h>
#include "TDOA.h"
#include "Definitions.h"

#include <esp_timer.h>
#include <esp_err.h>
#include <esp_log.h>

#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

volatile int first_button = 0;

// Arbitrary time differences for testing | Time 1 is always larger
// double timeDelay[] = {378.50 / 343000, 379.04 / 343000, 518.18 / 343000, 518.58 / 343000};
uint64_t timeDelay[] = {0, 0, 0, 0};

static void gpio_isr_handler_red(void* arg) 
{
  gpio_intr_disable(BTN_RED);

  timeDelay[0] = esp_timer_get_time();

  if (first_button < 1)
  {
    first_button = 1;
  }

}

static void gpio_isr_handler_blue(void* arg) 
{
  gpio_intr_disable(BTN_BLUE);

  timeDelay[1] = esp_timer_get_time();

  if (first_button < 1)
  {
    first_button = 2;
  }
}

static void gpio_isr_handler_green(void* arg) 
{
  gpio_intr_disable(BTN_GREEN);

  timeDelay[2] = esp_timer_get_time();

  if (first_button < 1)
  {
    first_button = 3;
  }
}

static void gpio_isr_handler_yellow(void* arg) 
{
  gpio_intr_disable(BTN_YELLOW);

  timeDelay[3] = esp_timer_get_time();

  if (first_button < 1)
  {
    first_button = 4;
  }
}

void button_config()
{
  gpio_install_isr_service(0);
  printf("configuring button\n");

  // reset pins
  gpio_reset_pin(BTN_RED);
  gpio_reset_pin(BTN_BLUE);
  gpio_reset_pin(BTN_GREEN);
  gpio_reset_pin(BTN_YELLOW);

  gpio_set_direction(BTN_RED, GPIO_MODE_INPUT);
  gpio_set_direction(BTN_BLUE, GPIO_MODE_INPUT);
  gpio_set_direction(BTN_GREEN, GPIO_MODE_INPUT);
  gpio_set_direction(BTN_YELLOW, GPIO_MODE_INPUT);

  gpio_pullup_en(BTN_RED);
  gpio_pullup_en(BTN_BLUE);
  gpio_pullup_en(BTN_GREEN);
  gpio_pullup_en(BTN_YELLOW);

  gpio_set_intr_type(BTN_RED, GPIO_INTR_POSEDGE);
  gpio_set_intr_type(BTN_BLUE, GPIO_INTR_POSEDGE);
  gpio_set_intr_type(BTN_GREEN, GPIO_INTR_POSEDGE);
  gpio_set_intr_type(BTN_YELLOW, GPIO_INTR_POSEDGE);

  // setup interrupts for each button
  gpio_isr_handler_add(BTN_RED, gpio_isr_handler_red, NULL);
  gpio_isr_handler_add(BTN_BLUE, gpio_isr_handler_blue, NULL);
  gpio_isr_handler_add(BTN_GREEN, gpio_isr_handler_green, NULL);
  gpio_isr_handler_add(BTN_YELLOW, gpio_isr_handler_yellow, NULL);

  printf("config complete\n");
}

void obstacle_config()
{
  // Configure GPIO for SR04 sensor
    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = 1ULL << TRIGGER_GPIO,
    };

    gpio_config(&io_conf);

    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = 1ULL << ECHO_GPIO;
    gpio_config(&io_conf);
}

void led_config()
{
  gpio_reset_pin(LED_1);
  gpio_reset_pin(LED_2);
  gpio_reset_pin(LED_3);
  gpio_reset_pin(LED_4);

  gpio_set_direction(LED_1, GPIO_MODE_OUTPUT);
  gpio_set_direction(LED_2, GPIO_MODE_OUTPUT);
  gpio_set_direction(LED_3, GPIO_MODE_OUTPUT);
  gpio_set_direction(LED_4, GPIO_MODE_OUTPUT);
}

static void turn_left()
{   
  gpio_set_level(LED_1, 1);
  gpio_set_level(LED_2, 0);
  gpio_set_level(LED_3, 0);
  gpio_set_level(LED_4, 1);
};

static void turn_right()
{   
  gpio_set_level(LED_1, 0);
  gpio_set_level(LED_2, 1);
  gpio_set_level(LED_3, 1);
  gpio_set_level(LED_4, 0);

};

static void forward()
{   
  gpio_set_level(LED_1, 1);
  gpio_set_level(LED_2, 0);
  gpio_set_level(LED_3, 1);
  gpio_set_level(LED_4, 0);
};

static void stop()
{   
  gpio_set_level(LED_1, 0);
  gpio_set_level(LED_2, 0);
  gpio_set_level(LED_3, 0);
  gpio_set_level(LED_4, 0);
};

int getDistance() 
{
  obstacle_config();

  gpio_set_level(TRIGGER_GPIO, 0);
  esp_rom_delay_us(5);
  gpio_set_level(TRIGGER_GPIO, 1);
  esp_rom_delay_us(10);
  gpio_set_level(TRIGGER_GPIO, 0);

  // Wait for echo start
  uint32_t start = esp_timer_get_time();
  while(gpio_get_level(ECHO_GPIO) == 0 && esp_timer_get_time() - start < TIMEOUT_US);

  // // Measure echo time
  start = esp_timer_get_time();
  while(gpio_get_level(ECHO_GPIO) == 1 && esp_timer_get_time() - start < TIMEOUT_US);

  // Calculate distance in cm
  uint32_t time = (esp_timer_get_time() - start);
  int distance = time/58;

  printf("Distance: %d cm\n", distance);
  
  return distance;
}

void app_main()
{   
  // I/O setup
  led_config();
  button_config();

  double Magnitude = 0;
  double Bearing = 0;

  while (1) 
  {
    int obstacle = 0;
    int test = getDistance();
    int turnDir = 0;

    if (test <= 40) 
    {
      obstacle = 1;
      turn_right()
      vTaskDelay(100 / portTICK_PERIOD_MS);
      stop()
      vTaskDelay(50 / portTICK_PERIOD_MS);

      if (test >= getDistance()) {turnDir = 1;}
    }

    while (getDistance() <= 40) 
    {
      if (turnDir) {turn_right();} elif (!turnDir) {turn_left();}

      vTaskDelay(50 / portTICK_PERIOD_MS);
      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);

    }

    if (obstacle == 1) 
    {
      obstacle = 0;

      if (turnDir) {turn_right();} elif (!turnDir) {turn_left();}

      vTaskDelay(500 / portTICK_PERIOD_MS);
      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);
      forward();

      for (int i = 0; i <= 100; i++) 
      {
        vTaskDelay(50 / portTICK_PERIOD_MS);

        if (getDistance() <= 40) {break;}
      }

      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);
    }

    int missing = 0;

    for (int i = 0; i < 4; i++)
    {
      if (timeDelay[i] == 0)
      {
        missing = 1;
      }
    }

    if (missing == 0)
    {
      lineFinder(timeDelay, &Magnitude, &Bearing);
      memset(timeDelay, 0, sizeof(timeDelay));
  
      switch (first_button)
      {
        case 2:
          Bearing += M_PI / 2;

          break;
        case 3:
          Bearing = 2 * M_PI - (Bearing + M_PI / 2);

          break;
        case 4:
          Bearing = 2 * M_PI - Bearing;

          break;
        default:
          break;
      }

      first_button = 0;

      printf("Magnitude: %f\nBearing: %f\n", Magnitude, Bearing * 180 / M_PI);

      gpio_intr_enable(BTN_RED);
      gpio_intr_enable(BTN_BLUE);
      gpio_intr_enable(BTN_GREEN);
      gpio_intr_enable(BTN_YELLOW);
    }

    if (Bearing != 0)
    {
      double Distance;

      if (Bearing >= M_PI)
      { 
        Bearing = 2*M_PI - Bearing;
        Distance = Bearing * WHEEL_DIST / 2;

        turn_left();
        double x = (Distance / MOTOR_SPEED) / portTICK_PERIOD_MS;
        printf("%f\n", x);
      }
      else if (Bearing < M_PI)
      {
        Distance = Bearing * WHEEL_DIST / 2;
        
        turn_right();
        double x = (Distance / MOTOR_SPEED) / portTICK_PERIOD_MS;
        printf("%f\n", x);
        
      }

      vTaskDelay((Distance / (MOTOR_SPEED)) / portTICK_PERIOD_MS);

      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);

      Bearing = 0;
    }

    if (Magnitude > 0)
    {
      forward();

      for (int i = 0; i <= 20; i++) 
      {
        vTaskDelay((Magnitude / MOTOR_SPEED) / portTICK_PERIOD_MS);

        if (getDistance() <= 40) {break;}
      }

      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);

      Magnitude = 0;
    }

    vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
  }
}