#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"

#define LED_RED GPIO_NUM_18
#define LED_GREEN GPIO_NUM_4
#define LED_YELLOW GPIO_NUM_5
#define LED_BLUE GPIO_NUM_2

#define BTN_LEFT GPIO_NUM_13
#define BTN_RIGHT GPIO_NUM_12

#define DELAY_TIME 5

#define NB_Relais 4
#define MAX 10

#define MAX_TIME 20000

#define DELAY_SWITCH 10
#define DELAY_MAX 20

struct relais {

  int time_point;
  int on_off;

};


volatile bool button_pressed = false;

  int byte_out[MAX_TIME];

struct led_task_parameters_t
{
  gpio_num_t led_gpio;
  TickType_t blink_time;
};
static void gpio_isr_handler(void* arg)
{
    button_pressed = true;
}
 
static led_task_parameters_t red_led_gpio = {LED_RED, 2000};
static led_task_parameters_t yellow_led_gpio = {LED_YELLOW, 1500};
static led_task_parameters_t green_led_gpio = {LED_GREEN, 1000};
static led_task_parameters_t blue_led_gpio = {LED_BLUE, 500};

void button_config()
{
    printf("configuring button\n");
    gpio_reset_pin(BTN_LEFT);
    gpio_set_direction(BTN_LEFT, GPIO_MODE_INPUT);
    gpio_set_direction(BTN_RIGHT, GPIO_MODE_INPUT);
    gpio_pullup_en(BTN_LEFT);
    gpio_pullup_en(BTN_RIGHT);
    printf("config complete\n");

  gpio_reset_pin(LED_RED);
  gpio_set_direction(LED_RED, GPIO_MODE_OUTPUT);
  gpio_set_level(LED_RED, 0);

  gpio_reset_pin(LED_YELLOW);
  gpio_set_direction(LED_YELLOW, GPIO_MODE_OUTPUT);
  gpio_set_level(LED_YELLOW, 0);

  gpio_reset_pin(LED_GREEN);
  gpio_set_direction(LED_GREEN, GPIO_MODE_OUTPUT);
  gpio_set_level(LED_GREEN, 0);

  gpio_reset_pin(LED_BLUE);
  gpio_set_direction(LED_BLUE, GPIO_MODE_OUTPUT);
  gpio_set_level(LED_BLUE, 0);

}



void Task1code( void * parameter ) {
  const TickType_t taskPeriod = DELAY_TIME; // 20ms <--> 50Hz
  int Button_Counter_LEFT = 0;
  int Button_Counter_RIGHT = 0;
  int TIME_Counter = 0;
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;)  {

    //
    // Do Stuff (needs to take less than 20ms)
    //

    if ((gpio_get_level(BTN_LEFT)==0) && (Button_Counter_LEFT<DELAY_MAX))
    {
        Button_Counter_LEFT ++;
    }
    else if (Button_Counter_LEFT>0)
    {
        Button_Counter_LEFT --;
    }    

    if ((Button_Counter_LEFT)>DELAY_SWITCH)
    {
      if(TIME_Counter<0)
      {
          TIME_Counter = 0;
      }
      else if (TIME_Counter<MAX_TIME)
      {
        TIME_Counter = TIME_Counter + taskPeriod;
      }
    }

    if ((gpio_get_level(BTN_RIGHT)==0) && (Button_Counter_RIGHT<DELAY_MAX))
    {
        Button_Counter_RIGHT ++;
    }
    else if (Button_Counter_RIGHT>0)
    {
        Button_Counter_RIGHT --;
    }    

    if ((Button_Counter_RIGHT)>DELAY_SWITCH)
    {
      if (TIME_Counter>0)
      {
        TIME_Counter = TIME_Counter - taskPeriod;
      }
    }

    //printf("Button_Counter_RIGHT: %d",Button_Counter_RIGHT);  
    //printf("Button_Counter_LEFT: %d",Button_Counter_LEFT);  

    printf("Counter: %d  LEDs: %d \n",TIME_Counter, byte_out[TIME_Counter]);

    gpio_set_level(LED_RED, (byte_out[TIME_Counter] & 0x01));
    gpio_set_level(LED_YELLOW, (byte_out[TIME_Counter] & 0x02)>>1);
    gpio_set_level(LED_GREEN, (byte_out[TIME_Counter] & 0x04)>>2);
    gpio_set_level(LED_BLUE, (byte_out[TIME_Counter] & 0x08)>>3);

    //printf("LEDs: %d \n",(byte_out[TIME_Counter] & 0x02)>>1);

    vTaskDelayUntil(&xLastWakeTime, taskPeriod);
  }
}

extern "C" void app_main()
{
uint8_t led_value = 0;


  struct relais arr_relais[NB_Relais][MAX];
  int max_arr_relais[NB_Relais];

  
    for (int TIME_cnt = 0; TIME_cnt < MAX_TIME; TIME_cnt++)
  byte_out[TIME_cnt]=0;
  int i;

  max_arr_relais[0] = 4;

  arr_relais[0][0].time_point = 0;
  arr_relais[0][0].on_off = 0;

  arr_relais[0][1].time_point = 1000;
  arr_relais[0][1].on_off = 1;

  arr_relais[0][2].time_point = 2000;
  arr_relais[0][2].on_off = 0;

  arr_relais[0][3].time_point = 3000;
  arr_relais[0][3].on_off = 0;

  arr_relais[0][4].time_point = 4000;
  arr_relais[0][4].on_off = 0;

  max_arr_relais[1] = 4;

  arr_relais[1][0].time_point = 0;
  arr_relais[1][0].on_off = 0;

  arr_relais[1][1].time_point = 3000;
  arr_relais[1][1].on_off = 1;

  arr_relais[1][2].time_point = 4000;
  arr_relais[1][2].on_off = 0;

  arr_relais[1][3].time_point = 5000;
  arr_relais[1][3].on_off = 0;

  arr_relais[1][4].time_point = 6000;
  arr_relais[1][4].on_off = 0;


  max_arr_relais[2] = 4;

  arr_relais[2][0].time_point = 0;
  arr_relais[2][0].on_off = 0;

  arr_relais[2][1].time_point = 5000;
  arr_relais[2][1].on_off = 1;

  arr_relais[2][2].time_point = 6000;
  arr_relais[2][2].on_off = 0;

  arr_relais[2][3].time_point = 8000;
  arr_relais[2][3].on_off = 1;

  arr_relais[2][4].time_point = 9000;
  arr_relais[2][4].on_off = 0;

  max_arr_relais[3] = 4;

  arr_relais[3][0].time_point = 0;
  arr_relais[3][0].on_off = 0;

  arr_relais[3][1].time_point = 7000;
  arr_relais[3][1].on_off = 1;

  arr_relais[3][2].time_point = 8000;
  arr_relais[3][2].on_off = 0;

  arr_relais[3][3].time_point = 9000;
  arr_relais[3][3].on_off = 1;

  arr_relais[3][4].time_point = 10000;
  arr_relais[3][4].on_off = 0;


  for (int Relais = 0; Relais < NB_Relais; Relais++)

  {
    for (int nb = 0; nb < (max_arr_relais[Relais] + 1); nb++)
      printf("%d\t%d\n", arr_relais[Relais][nb].time_point,arr_relais[Relais][nb].on_off);
  }
  
  int nb = 0;
  
  for (int Relais = 0; Relais < NB_Relais; Relais++)
  {

    for (nb = 0; nb < (max_arr_relais[Relais]); nb++)
    {

      for (int tp_cnt = arr_relais[Relais][nb].time_point; tp_cnt < arr_relais[Relais][nb + 1].time_point; tp_cnt++)
      {

        //printf("%d\t%d\t%d\t%d\t%d\n",Relais, nb, tp_cnt, byte_out[tp_cnt],(arr_relais[Relais][nb].on_off << Relais));

        byte_out[tp_cnt] = byte_out[tp_cnt] + (arr_relais[Relais][nb].on_off << Relais);

      }
    }

    for (int tp_cnt = arr_relais[Relais][nb].time_point; tp_cnt < MAX_TIME; tp_cnt++)
    {

      //printf("%d\t%d\t%d\t%d\t%d\n",Relais, nb, tp_cnt, byte_out[tp_cnt],(arr_relais[Relais][nb].on_off << Relais));

      byte_out[tp_cnt] = byte_out[tp_cnt] + (arr_relais[Relais][nb].on_off << Relais);

    }

  }

  for (int TIME_cnt = 0; TIME_cnt < MAX_TIME; TIME_cnt++)

  {

    //printf("%d\t%d\n", TIME_cnt, byte_out[TIME_cnt]);

  }
  // signal to operating system program ran fine

  button_config();

  xTaskCreate(
    Task1code, // task function
    "Timer_task", // task name
    2048, // stack size in words
    NULL, // pointer to parameters
    5, // priority
    NULL); // out pointer to task handle

  while (1) {

        //vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
    }
}