#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Arduino_FreeRTOS.h>

#define SERVO_PIN 9
#define POTENTIOMETER_PIN A1

#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 16
#define LCD_ROWS 2

const float GAMMA = 0.7;
const float RL10 = 50;

Servo myservo;
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);
TaskHandle_t xHandle_LCD;

volatile int potValue = 0;
volatile int servoPos = 0;
volatile bool blinkingState = false;
volatile float lux = 0;

void TaskReadPotentiometer(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    potValue = analogRead(POTENTIOMETER_PIN);
    servoPos = map(potValue, 0, 1023, 0, LCD_COLUMNS); 
    
    vTaskDelay(10 / portTICK_PERIOD_MS);
  }
}

void TaskControlServo(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    int targetPosition = map(potValue, 0, 1023, 0, 180); 
    myservo.write(targetPosition);
    vTaskDelay(20 / portTICK_PERIOD_MS); 
  }
}

void TaskUpdateLCD(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    blinkingState = !blinkingState;
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void TaskDisplayLCD(void *pvParameters) {
  (void)pvParameters;

  lcd.init();
  lcd.backlight();
  
  byte squareChar[8] = {
    B11111,
    B11111,
    B11111,
    B11111,
    B11111,
    B11111,
    B11111,
    B11111
  };
  lcd.createChar(0, squareChar);
  

  bool displayBlink = false;

  for (;;) {
    lcd.setCursor(0, 0);
    lcd.print("Angle: ");
    lcd.print(map(servoPos, 0, LCD_COLUMNS, 0, 180));

    lcd.setCursor(0, 1);
    lcd.print("Lypkan/Rachkovsky");
    
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    lcd.clear();
    vTaskDelay(500 / portTICK_PERIOD_MS);
    lcd.setCursor(0, 1);
    lcd.print("Lux: ");
    lcd.print(lux);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    lcd.clear();
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void TaskReadLux(void *pvParameters) {
  (void)pvParameters;

  for (;;) {
    int analogValue = analogRead(A0);
    float voltage = analogValue / 1024. * 5;
    float resistance = 2000 * voltage / (1 - voltage / 5);
    lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));

    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void setup() {
  myservo.attach(SERVO_PIN);

  xTaskCreate(TaskReadPotentiometer, "ReadPot", 128, NULL, 1, NULL);
  xTaskCreate(TaskUpdateLCD, "UpdateLCD", 128, NULL, 2, NULL);
  xTaskCreate(TaskDisplayLCD, "DisplayLCD", 128, NULL, 3, &xHandle_LCD);
  xTaskCreate(TaskControlServo, "ControlServo", 128, NULL, 4, NULL);
  xTaskCreate(TaskReadLux, "ReadLux", 128, NULL, 5, NULL);

  vTaskStartScheduler();
}

void loop() {
 
}