//Includes
#include "driver/gpio.h"
#include "soc/io_mux_reg.h"
#include "soc/gpio_reg.h"
#include "soc/gpio_periph.h"
#include "soc/timer_group_reg.h"
#include <Arduino.h>
#include <Wire.h>
// Ensure that the LiquidCrystal I2C library by Frank de Brabander is installed in your Arduino IDE Library Manager
#include <LiquidCrystal_I2C.h>
// Definitions
#define LED1 6
#define TIMER_INCREMENT_MODE (1<<30)
#define TIMER_ENABLE (1<<31)
#define TIMER_DEVIDE 8000
#define READY 0
#define RUNNING 1
#define DONE 2
#define WAITING 3
#define TASK1_INT 1000
#define TASK2_INT 500
#define TASK3_INT 500
#define TASK4_INT 500
#define NUM_TASKS 4
LiquidCrystal_I2C lcd(0x27, 16, 2); // Initialize the LCD
#define SDA_PIN 36
#define SCL_PIN 35
#define SENDDATA 0x0D
#define SENDCOMMAND 0x0C
#define mover 0x14
#define movel 0x10
#define clear 0x01
#define home 0x80
#define shiftR 0x1C
#define BUZZER_PIN 4
#define TONELIGHT_PIN 5
//========= Variables ========
uint32_t current_time = 0;
uint32_t last_t1 = 0;
uint32_t last_t2 = 0;
uint32_t last_t3 = 0;
uint32_t last_t4 = 0;

// Type definitions
typedef void (*funcptr)();
struct TCBstruct{
        funcptr pointer;
        unsigned short int state;
        unsigned int delayTime;
        unsigned short int priority;
        };
TCBstruct task_list[NUM_TASKS+1];

// Function setups
void task1();
void task2();
void task3();
void task4();
void executeTask(funcptr);
uint32_t my_time();
// ========== Functions ============
//Setup
void setup() {
  Serial.begin(9600);
  *((volatile uint32_t *)GPIO_ENABLE_REG) |= (1<<LED1); //Enable gpio pins
  *((volatile uint32_t *)GPIO_ENABLE_REG) |= (1<<TONELIGHT_PIN);
  *((volatile uint32_t *)GPIO_ENABLE_REG) |= (1<<BUZZER_PIN);
  PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[LED1], PIN_FUNC_GPIO); //Setup gpio pins
  PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[TONELIGHT_PIN], PIN_FUNC_GPIO);
  PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[BUZZER_PIN], PIN_FUNC_GPIO);
  setup_timer(); //run timer setup
  ledcAttach(BUZZER_PIN, 500, 12);
  ledcAttach(TONELIGHT_PIN, 500, 12);
  //Setup tasks
  task_list[0].pointer = &task1;
  task_list[0].state = READY;
  task_list[0].delayTime = 2;
  task_list[0].priority = 4;

  task_list[1].pointer = &task2;
  task_list[1].state = READY;
  task_list[1].delayTime = 10;
  task_list[1].priority = 3;

  task_list[2].pointer = &task3;
  task_list[2].state = READY;
  task_list[2].delayTime = 2;
  task_list[2].priority = 2;

  task_list[3].pointer = &task4;
  task_list[3].state = READY;
  task_list[3].delayTime = 2;
  task_list[3].priority = 1;

  task_list[NUM_TASKS] = {}; //Null task, used to designate end of list

  Wire.begin(SDA_PIN, SCL_PIN); // Initialize lcd communication
  lcd.init();
  delay(2);
  send_byte(home, true);
}

int j=0; //  current task the scheduler is looking at
unsigned short int current_priority = 4; // Current priority for task to run
void loop(){
  current_time = my_time(1000); // Current timer time
  scheduler();
}

bool all_waiting = true;
void scheduler(){
  all_waiting = true;
  for (int i=0; i<NUM_TASKS; i++){
    if (task_list[i].state!=WAITING){ 
      all_waiting=false; // If all tasks are waiting, stays true
    }
  }
  if (all_waiting){ // If all tasks are waiting, set them to ready
    for (int i=0; i<NUM_TASKS; i++){
      task_list[i].state=READY;
      task_list[i].priority = task_list[i].priority%4+1;
    }
    current_priority = 4;
  }
  if ((task_list[j].pointer==nullptr) && (j==0)){ // If there are no tasks
    Serial.println("No Tasks :("); //No tasks
    delay(4);
  }
  if (task_list[j].state==RUNNING){ // If the task was started and isn't finished
    executeTask(task_list[j].pointer);
  }
  else if ((task_list[j].state==READY)&&(task_list[j].priority==current_priority)){
    task_list[j].state=RUNNING; // Set task to in progress
    executeTask(task_list[j].pointer);
  }
  else{
    if (task_list[j].state==DONE){ // If the task just finished
      delay(task_list[j].delayTime);
      printf("Task %u complete, priority level %u\n", j, task_list[j].priority);
      task_list[j].state=WAITING; // Task is now waiting
    }
    j++;
    if ((task_list[j].pointer==nullptr && j!=0)){// If at the end of the list, return to the start
      j=0; 
      current_priority--;
      if (current_priority==0){
        current_priority=4;
      }
    }
  }
}

uint8_t current_count = 0; // Current count of led toggles
// Toggles led based on current time and interval
void task1(){
  if ((current_time-last_t1)>=TASK1_INT){
    last_t1 = current_time;
    toggleLed(LED1);
    current_count++;
    if (current_count==16){
      current_count=0;
      task_list[0].state=DONE;
    }
  }
}

uint8_t current_int = 0x30; // Current integer as a char
// Count up to 10
void task2(){
  if ((current_time-last_t2)>=TASK2_INT){
    if (current_int==0x3B){ // If current int is 10
      current_int = 0x30;
      send_byte(clear, true);
      task_list[1].state = DONE;
    }
    else {
      last_t2 = current_time;
      if (current_int == 0x3A){ // send 1 and 0 if ten
        send_byte(0x31, false);
        send_byte(0x30, false);
      }
      else{send_byte(current_int, false);}
      current_int++;
    }
  }
}
uint8_t tone_incr = 1;
// Task 3, play 10 notes
void task3(){
  if ((current_time-last_t3)>=TASK3_INT){
    last_t3 = current_time;
    ledcWriteTone(BUZZER_PIN, 200*tone_incr); // Buzzer freq
    ledcWrite(TONELIGHT_PIN, 50*tone_incr); // LED brightness
    tone_incr++;
    if (tone_incr>=11){ // If 10 notes were played
      ledcWriteTone(BUZZER_PIN, 0);
      tone_incr = 1;
      task_list[2].state=DONE;
    }
  }
}


uint8_t current_char= 0x41; // Capital A
void task4(){
  if ((current_time-last_t4)>=TASK4_INT){
    last_t4 = current_time;
    Serial.println((char)current_char);
    current_char++;
    if (current_char>0x5A){ // If char is now past Z
      task_list[3].state = DONE;
      current_char=0x41;
    }
  }
}

// Toggles given led
void toggleLed(int pin) {
  *((volatile uint32_t *)GPIO_OUT_REG) ^= (1<<pin);
}

// Execute a task given a function pointer
void executeTask(funcptr functionPTR){
  functionPTR();
}

// Setup the timer
void setup_timer(){
    // Configure timer
    uint32_t timer_config = 0;
    // Optionally apply a clock divider
    timer_config |= TIMER_DEVIDE<<13;
    // Set increment mode and enable timer
    timer_config |= TIMER_INCREMENT_MODE;
    timer_config |= TIMER_ENABLE;
    // Write config to timer register
    *((volatile uint32_t *)TIMG_T0CONFIG_REG(0)) = timer_config;
    // Trigger a timer update to load settings
    *((volatile uint32_t *)TIMG_T0UPDATE_REG(0)) = 1;
}

// Read from timer and return value
uint32_t my_time(int freq){
  *((volatile uint32_t *)TIMG_T0UPDATE_REG(0)) = 1; // Update timer
  uint32_t value = *((volatile uint32_t *)TIMG_T0LO_REG(0));
  return value*freq/(10000);// Grab timer time
}


// === Data to send over wire. passing true for command send the data as a 
// command. Sends the last 4 bits of the data
void send(uint8_t data, bool command){
  if (command){
    Wire.write((data<<4)|SENDCOMMAND);
  } else {
    Wire.write((data<<4)|SENDDATA);
  }
  Wire.write(0x08);
}

// Using the previous method, sends a byte of data. Passing true for command
// sends the data to the LCD as a command. Otherwise it is treated as data
void send_byte(uint8_t data, bool command){
  Wire.beginTransmission(0x27);
  send(data>>4, command);
  send(data,command);
  Wire.endTransmission();
}