#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 <esp_adc_cal.h>

#include "driver/gpio.h"
#include "driver/adc.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};

void adc_config()
{
  esp_adc_cal_characteristics_t adc1_chars;
  esp_adc_cal_characterize(ADC_UNIT_1, ADC_ATTEN_DB_11, ADC_WIDTH_BIT_DEFAULT, 0, &adc1_chars);
  
  // MIC 1 Config
  adc1_config_channel_atten(ADC1_CHANNEL_4, ADC_ATTEN_DB_11);


  // MIC 2 Config
  adc1_config_channel_atten(ADC1_CHANNEL_5, ADC_ATTEN_DB_11);

  // MIC 3 CONFIG
  adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11);

  // MIC 4 CONFIG
  adc1_config_channel_atten(ADC1_CHANNEL_7, ADC_ATTEN_DB_11);

  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();
  adc_config();

  double Magnitude = 0;
  double Bearing = 0;

  while (1) 
  {
    adc1_config_width(ADC_WIDTH_BIT_12);
    int r = adc1_get_raw(ADC1_CHANNEL_4);
    
    printf("Voltage: %d\n", r); 

    int obstacle = 0;

    if (getDistance() <= 40) {obstacle = 1;}

    while (getDistance() <= 40) 
    {
      turn_right();
      vTaskDelay(50 / portTICK_PERIOD_MS);
      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);

    }

    if (obstacle == 1) 
    {
      //printf("%d\n", obstacle);
      obstacle = 0;

      turn_right();
      vTaskDelay(500 / portTICK_PERIOD_MS);
      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);
    }

    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();
      vTaskDelay((Magnitude / MOTOR_SPEED) / portTICK_PERIOD_MS);
      
      stop();
      vTaskDelay(50 / portTICK_PERIOD_MS);

      Magnitude = 0;
    }

    vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
  }
}