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

#define OFF 0
#define ON 1


#define N_SEGMENTS 7
#define N_DIGITS_IN_DISPLAY 4

int n;

gpio_num_t gpio_for_segment[N_SEGMENTS] = {
  GPIO_NUM_26,  // a
  GPIO_NUM_25,  // b
  GPIO_NUM_33,  // c
  GPIO_NUM_32,  // d
  GPIO_NUM_2,   // e
  GPIO_NUM_15,  // f
  GPIO_NUM_23   // g
};

int segment_level[][N_SEGMENTS] = {
  { OFF, ON,  ON,  ON,  ON,  ON,  ON }, 
  { OFF, OFF, OFF, OFF, ON,  ON,  OFF},
  { ON,  OFF, ON,  ON,  OFF, ON,  ON },
  { ON,  OFF, OFF, ON,  ON,  ON,  ON },
  { ON,  ON,  OFF, OFF, ON,  ON,  OFF},
  { ON,  ON,  OFF, ON,  ON,  OFF, ON },
  { ON,  ON,  ON,  ON,  ON,  OFF, ON },
  { OFF, OFF, OFF, OFF, ON,  ON,  ON },
  { ON,  ON,  ON,  ON,  ON,  ON,  ON },
  { ON,  ON,  OFF, OFF, ON,  ON,  ON }
};

gpio_num_t gpio_for_control[] = {
  GPIO_NUM_5,  // 1
  GPIO_NUM_21, // 2
  GPIO_NUM_18, // 3
  GPIO_NUM_27  // 4
};

void turnOffDisplay(){
  for (int i=0; 1 < N_DIGITS_IN_DISPLAY; i++){
    gpio_reset_pin(gpio_for_control[i]);
    gpio_set_direction(gpio_for_control[i], GPIO_MODE_OUTPUT);
    gpio_set_level(gpio_for_control[i], ON);
  }
}

void resetSegments(){
  for (int i=0; i < N_SEGMENTS; i++){
    gpio_reset_pin(gpio_for_segment[i]);
    gpio_set_direction(gpio_for_segment[i], GPIO_MODE_OUTPUT);
    gpio_set_level(gpio_for_segment[i], OFF);
  }
}

void initDisplay(){
  resetSegments();
  turnOffDisplay();

  gpio_reset_pin(GPIO_NUM_19);
  gpio_set_direction(GPIO_NUM_19, GPIO_MODE_INPUT);
}

#define DELAY_TIME 10
#define COUNTER_TIME 500

void display(void *arg){

  int number;
  int digit;

  while(1){
    number = n;
    for (int i = 0; i < N_DIGITS_IN_DISPLAY; i++){
      turnOffDisplay();
      digit = number % 10;
      number /= 10;
      for (int s = 0; s < N_SEGMENTS; s++){
        gpio_set_level(gpio_for_segment[s], segment_level[digit][s]);
      }
      gpio_set_level(gpio_for_control[i], OFF);
      vTaskDelay(DELAY_TIME / portTICK_PERIOD_MS);
    }
  }
}

void counter(void *arg){
  for (n = 0; n < 10000; n++){
    if (gpio_get_level(GPIO_NUM_19) == OFF) n = 0;
    vTaskDelay(COUNTER_TIME / portTICK_PERIOD_MS);
  }
}

extern "C" void app_main(){

  initDisplay();

  xTaskCreate(&display, "displayNumber", 2048,NULL,5,NULL);
  xTaskCreate(&counter, "counter", 2048,NULL,5,NULL);

}