// Use an interrupt to light up LED with a button and print messages in a separate task.

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050_light.h>

#define button_pin GPIO_NUM_4
#define led_pin GPIO_NUM_27
#define ON HIGH
#define OFF LOW

#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels

// create an OLED display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
MPU6050 mpu(Wire);

bool button_pressed = false;


void task_PrintMessage(void* param){
  /**while(1){
    printf("Message being printed by a task.\n");
    vTaskDelay(2000/portTICK_PERIOD_MS);
  }*/
}

// Interrrupt to change value of pushbutton when pressed
static void button_isr_handler(void* args){
  button_pressed = true;
}

void init_button(){
  gpio_install_isr_service(0);
  gpio_reset_pin(button_pin);
  gpio_set_direction(button_pin, GPIO_MODE_INPUT);
  gpio_pullup_en(button_pin);
  gpio_set_intr_type(button_pin, GPIO_INTR_NEGEDGE);
  gpio_isr_handler_add(button_pin, button_isr_handler, NULL);
}

/*void init_led(){
  gpio_reset_pin(led_pin);
  gpio_set_direction(led_pin, GPIO_MODE_OUTPUT);
}*/

void init_oled(){ // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
  }
  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(1);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("test"); // set text
  oled.display();              // display on OLED
  oled.println("asdasd"); // set text
}

void task_button(void* param){
  int led_state = OFF;
  while(1){
    if(button_pressed == true){
      printf("Button was pressed.\n");
      button_pressed = false;
      led_state = !led_state;
      gpio_set_level(led_pin,led_state);
    }
    vTaskDelay(1000/portTICK_PERIOD_MS);
  }
}

void timer_callback1(void *args){
  printf("Message 1KHz\n");
}

void timer_callback2(void *args){
  printf("Message 500Hz\n");
}
void timer_callback3(void *args){
  printf("Message 10Hz\n");
}
void timer_callback4(void *args){
  printf("Message 5Hz\n");
}
void setup() {
  init_button();
  init_oled();
  //init_led();
  const esp_timer_create_args_t esp_timer1_create_args = {.callback = timer_callback1, .name = "ADC Channels"};
  const esp_timer_create_args_t esp_timer2_create_args = {.callback = timer_callback2, .name = "IMU 9-AXIS"  };
  const esp_timer_create_args_t esp_timer3_create_args = {.callback = timer_callback3, .name = "Temperature" };
  const esp_timer_create_args_t esp_timer4_create_args = {.callback = timer_callback4, .name = "Current Mon" };
  
  esp_timer_handle_t esp_timer_handle1, esp_timer_handle2, esp_timer_handle3, esp_timer_handle4; //handler
  esp_timer_create(&esp_timer1_create_args, &esp_timer_handle1);
  esp_timer_create(&esp_timer2_create_args, &esp_timer_handle2);
  esp_timer_create(&esp_timer2_create_args, &esp_timer_handle3);
  esp_timer_create(&esp_timer2_create_args, &esp_timer_handle4);

  esp_timer_start_periodic(esp_timer_handle1, 1e6 / 1000); // microseconds
  esp_timer_start_periodic(esp_timer_handle2, 1e6 / 500); // microseconds
  esp_timer_start_periodic(esp_timer_handle3, 1e6 / 10); // microseconds
  esp_timer_start_periodic(esp_timer_handle4, 1e6 / 5); // microseconds

  //xTaskCreate(&task_PrintMessage, "Task for printing messages", 2048, NULL, 5, NULL);
  //xTaskCreate(&task_button, "Task for button", 2048, NULL, 5, NULL);

}

void loop() {
  // put your main code here, to run repeatedly:

}