#include <stdio.h>
#include <stdbool.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_err.h>

#include "ultrasonic.h"

//PROTÓTIPO DE FUNÇÕES===================================================
void app_main(void);
void init_leds();
void task_ultrasonic_test(void *pvParameters);
void task_control_leds(void *pvParameters);

//DEFINIÇÕES E CONSTATNTES===============================================
#define delay(value) vTaskDelay(value/portTICK_PERIOD_MS)
#define ECHO_GPIO 12
#define TRIGGER_GPIO 13
#define MAX_DISTANCE_CM 500 // Maximum of 5 meters
#define LED_VERD GPIO_NUM_21
#define LED_VERD_1 GPIO_NUM_22
#define LED_VERD_2 GPIO_NUM_23
#define LED_AMAR GPIO_NUM_19
#define LED_VERM GPIO_NUM_15
#define LED_VERM_1 GPIO_NUM_2
#define LED_VERM_2 GPIO_NUM_4
enum estados_led {VERM, AMAR, VERD};

//Máscara para Saídas===================================================
#define GPIO_OUTPUT_PIN_SEL ((1ULL<<LED_VERD) | (1ULL<<LED_VERD_1) | (1ULL<<LED_VERD_2) | (1ULL<<LED_AMAR) | (1ULL<<LED_VERM) | (1ULL<<LED_VERM_1) | (1ULL<<LED_VERM_2)) 

//Variáveis=============================================================
static TaskHandle_t xtask_handle_leds = NULL;
enum estados_led estado;
uint8_t parada_emergencia = 0;

//CONFIGURANDO SAÍDAS===================================================
void init_leds(){  
	gpio_config_t output_config = {};//variável descritora
	output_config.intr_type = GPIO_INTR_DISABLE;//desabilita interrupção externa
	output_config.mode = GPIO_MODE_OUTPUT;//configura GPIO como saídas
	output_config.pin_bit_mask = GPIO_OUTPUT_PIN_SEL;//carrega GPIOs configuradas
	output_config.pull_up_en = 0;//desativa pull_up
	output_config.pull_down_en = 0;//desativa pull_down 
	gpio_config(&output_config);//configura GPIO conforme descritor

  estado = VERM;
}

void task_control_leds(void *pvParameters){
  while(true){
    printf("Estado autal do semáforo: %d\n", estado);
    if(estado == VERM) {
      gpio_set_level(LED_VERM_2, 1);
      gpio_set_level(LED_VERM_1, 1);
      gpio_set_level(LED_VERM, 1);      
      gpio_set_level(LED_AMAR, 0);
      gpio_set_level(LED_VERD, 0);    
      estado = VERD;
      delay(1500);
      gpio_set_level(LED_VERM_2, 0);
      delay(1500);
      gpio_set_level(LED_VERM_1, 0);
      delay(3000);
      gpio_set_level(LED_VERM, 0);     
    } 
  else if(estado == VERD) {
      gpio_set_level(LED_VERM, 0);
      gpio_set_level(LED_AMAR, 0);
      gpio_set_level(LED_VERD_2, 1);
      gpio_set_level(LED_VERD_1, 1);
      gpio_set_level(LED_VERD, 1);    
      estado = AMAR;
      delay(4000);
      gpio_set_level(LED_VERD_2, 0);
      delay(3000);
      gpio_set_level(LED_VERD_1, 0);
      delay(3000);
      gpio_set_level(LED_VERD, 0);
    }
  else {
      gpio_set_level(LED_VERM, 0);
      gpio_set_level(LED_AMAR, 1);
      gpio_set_level(LED_VERD, 0);    
      estado = VERM;
      delay(1000);    
    }         
  }  
}


void task_ultrasonic_test(void *pvParameters)
{
    float distance;

    ultrasonic_sensor_t sensor = {
        .trigger_pin = TRIGGER_GPIO,
        .echo_pin = ECHO_GPIO
    };

    ultrasonic_init(&sensor);

    while (true) {
        esp_err_t res = ultrasonic_measure(&sensor, MAX_DISTANCE_CM, &distance);

        if (res == ESP_OK) {
            printf("Distance: %0.04f m\n", distance);
            if(distance <= 1){
              if(parada_emergencia == 0){
                vTaskSuspend(xtask_handle_leds);
                gpio_set_level(LED_VERM_2, 1);
                gpio_set_level(LED_VERM_1, 1);
                gpio_set_level(LED_VERM, 1);  
                gpio_set_level(LED_AMAR, 0);
                gpio_set_level(LED_VERD, 0);
                gpio_set_level(LED_VERD_1, 0);
                gpio_set_level(LED_VERD_2, 0);
                estado = VERM;
                parada_emergencia = 1;
              }              
            }
            else{
              parada_emergencia = 0;
              vTaskResume(xtask_handle_leds);              
            }
        } // Print error
        else {
            printf("Error %d: ", res);
            switch (res) {
                case ESP_ERR_ULTRASONIC_PING:
                    printf("Cannot ping (device is in invalid state)\n");
                    break;
                case ESP_ERR_ULTRASONIC_PING_TIMEOUT:
                    printf("Ping timeout (no device found)\n");
                    break;
                case ESP_ERR_ULTRASONIC_ECHO_TIMEOUT:
                    printf("Echo timeout (i.e. distance too big)\n");
                    break;
                default:
                    printf("%s\n", esp_err_to_name(res));
            }
        }        
        delay(500);
    }
}

void app_main(){
  init_leds();
  xTaskCreate(task_ultrasonic_test, "ultrasonic_test", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
  xTaskCreate(task_control_leds, "LEDs", 2048, NULL, 5, &xtask_handle_leds);
}