#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <dht11.h>
#include <BH1750.h>
#include <Servo.h>

#define DHT11_PIN 7
#define photoL1 A0
#define photoL2 A1
#define photoL3 A2
#define photoL4 A3
#define batteryPin A6
#define button 3
#define buzzer 6
#define circleServoPin 10
#define tiltServoPin 9
#define LED 4 //define the pin of LED as D10

#define TILT_ANGLE_FLAT 0
#define TILT_ANGLE_MAX  170

#define TURN_L1 0
#define TURN_L2 45
#define TURN_L3 180
#define TURN_L4 90

#define SERVO_STEP  5

// Battery monitoring constants
#define BATTERY_MIN_VOLTAGE 10.5  // Minimum safe voltage for 12V battery (1.75V per cell)
#define BATTERY_MAX_VOLTAGE 14.4  // Maximum charging voltage for 12V battery (2.4V per cell)
#define BATTERY_FULL_VOLTAGE 12.6 // Full charge voltage for 12V battery (2.1V per cell)

byte m_speed = 10;
BH1750 lightMeter;
dht11 DHT;
volatile int buttonState;
volatile int lightReadL1 = 0; // EAST FACING
volatile int lightReadL2 = 0; // NORTH FACING
volatile int lightReadL3 = 0; // WEST FACING
volatile int lightReadL4 = 0; // SOUTH FACING
float batteryVoltage = 0.0;
int batteryPercentage = 0;
int value;
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo circleServo;
Servo tiltServo;
int tiltPos;
int circlePos;

// Charging states
enum ChargingState {
  NOT_CHARGING,
  CHARGING,
  FULLY_CHARGED,
  ERROR
};

ChargingState chargingState = NOT_CHARGING;

void setup()
{
  Serial.begin(9600);
  Serial.println("");
  Serial.print("setting up!\n");

  Serial.print("init lcd\n");
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.printstr("setting up!");
  //lcd.setCursor(0,1);
  //lcd.printstr("hey,human!");

  pinMode(LED, OUTPUT); //initialize digital pin LED as an output.
  pinMode(button, INPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(batteryPin, INPUT);

  // Initialize servos with error checking
  if (!circleServo.attach(circleServoPin)) {
    Serial.println("Error: Failed to attach circle servo");
  }
  circleServo.write(TURN_L4);
  Serial.print("circle pos: ");
  Serial.print(TURN_L4);
  Serial.println("");
  delay(1000);
  
  if (!tiltServo.attach(tiltServoPin)) {
    Serial.println("Error: Failed to attach tilt servo");
  }
  tiltServo.write(0);
  Serial.print("tilt pos: ");
  Serial.print(TILT_ANGLE_FLAT);
  Serial.println("");
  delay(1000);

  // Test servo movement
  if (false) {
  circleServo.write(TURN_L3);
  delay(1000);
  circleServo.write(TURN_L1);
  delay(1000);
  circleServo.write(TURN_L4);
  delay(1000);
  
  for (tiltPos = TILT_ANGLE_FLAT; tiltPos < TILT_ANGLE_MAX; tiltPos += SERVO_STEP) {
    tiltServo.write(tiltPos);
    Serial.print("tilt pos: ");
    Serial.print(tiltPos);
    Serial.println("");
    delay(m_speed);
  }
  }

  Serial.print("done setting up!\n");
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.printstr("done");

  //Initialize the I2C bus
  Wire.begin();
  //On esp8266 you can select SCL and SDA pins using Wire.begin
  //For Wemos /Lolin D1 Mini Pro and the Ambient Light shield use
  if (!lightMeter.begin()) {
    Serial.println("Error: Failed to initialize BH1750 sensor");
    lcd.setCursor(0,1);
    lcd.printstr("Light ERR");
  } else {
    Serial.println(F("BH1750 Test begin"));
  }
}

long long count = 0;

// Function to find the direction with maximum light
int findBrightestDirection() {
  int maxLight = 0;
  int direction = 0;
  
  if (lightReadL1 > maxLight) {
    maxLight = lightReadL1;
    direction = 1; // East
  }
  if (lightReadL2 > maxLight) {
    maxLight = lightReadL2;
    direction = 2; // North
  }
  if (lightReadL3 > maxLight) {
    maxLight = lightReadL3;
    direction = 3; // West
  }
  if (lightReadL4 > maxLight) {
    maxLight = lightReadL4;
    direction = 4; // South
  }
  
  return direction;
}

// More sophisticated solar tracking algorithm
void adjustPanelPosition() {
  int direction = findBrightestDirection();
  
  // Current positions
  int currentCirclePos = circleServo.read();
  int currentTiltPos = tiltServo.read();
  
  // Determine target positions
  int targetCirclePos = currentCirclePos;
  int targetTiltPos = currentTiltPos;
  
  switch (direction) {
    case 1: // East
      targetCirclePos = TURN_L1;
      break;
    case 2: // North
      targetCirclePos = TURN_L2;
      break;
    case 3: // West
      targetCirclePos = TURN_L3;
      break;
    case 4: // South
      targetCirclePos = TURN_L4;
      break;
    default:
      // Stay in current position
      break;
  }
  
  // Adjust tilt based on light intensity
  // Simple algorithm: more light = more tilt up to max
  int avgLight = (lightReadL1 + lightReadL2 + lightReadL3 + lightReadL4) / 4;
  targetTiltPos = map(avgLight, 0, 1023, 0, TILT_ANGLE_MAX);
  targetTiltPos = constrain(targetTiltPos, 0, TILT_ANGLE_MAX);
  
  // Move servos gradually to target positions to avoid jerky movement
  // Only move if there's a significant difference
  if (abs(targetCirclePos - currentCirclePos) > SERVO_STEP) {
    int newPos = currentCirclePos;
    if (targetCirclePos > currentCirclePos) {
      newPos = currentCirclePos + SERVO_STEP;
    } else {
      newPos = currentCirclePos - SERVO_STEP;
    }
    circleServo.write(newPos);
    Serial.print("Adjusted circle servo to: ");
    Serial.println(newPos);
  }
  
  if (abs(targetTiltPos - currentTiltPos) > SERVO_STEP) {
    int newPos = currentTiltPos;
    if (targetTiltPos > currentTiltPos) {
      newPos = currentTiltPos + SERVO_STEP;
    } else {
      newPos = currentTiltPos - SERVO_STEP;
    }
    tiltServo.write(newPos);
    Serial.print("Adjusted tilt servo to: ");
    Serial.println(newPos);
  }
}

// Function to implement charging safety measures
void chargingSafetyCheck() {
  // If battery is fully charged, stop charging
  if (chargingState == FULLY_CHARGED) {
    // In a real implementation, this would control a charging circuit
    // For now, we'll just indicate it with the LED
    digitalWrite(LED, HIGH);
    if (count % 15 == 0) { // Every 30 seconds
      tone(buzzer, 1500, 300); // Higher pitch short beep
    }
  }
  
  // If battery voltage is too high or too low, indicate error
  if (chargingState == ERROR) {
    // Flash LED rapidly to indicate error
    for (int i = 0; i < 5; i++) {
      digitalWrite(LED, HIGH);
      delay(200);
      digitalWrite(LED, LOW);
      delay(200);
    }
    // Continuous beep for error
    if (count % 10 == 0) { // Every 20 seconds
      tone(buzzer, 300, 1000);
    }
  }
}

// Function to check for extreme environmental conditions
bool isEnvironmentSafe() {
  // Check for extreme temperatures
  if (CtoF(DHT.temperature) > 104 || CtoF(DHT.temperature) < 32) {
    // Temperature outside safe charging range (32°F to 104°F)
    return false;
  }
  
  // Check for extremely high humidity which might affect electronics
  if (DHT.humidity > 85) {
    return false;
  }
  
  return true;
}

void loop()
{
  char buf[40];
  
  // Read sensor values with error checking
  int chk = DHT.read(DHT11_PIN);
  switch (chk)
  {
    case DHTLIB_OK:
      // All good, continue
      break;
    case DHTLIB_ERROR_CHECKSUM:
      Serial.println("[ERR] dht11 checksum");
      lcd.setCursor(0,0);
      lcd.printstr("DHT11 ERR");
      break;
    case DHTLIB_ERROR_TIMEOUT:
      Serial.println("[ERR] dht11 timeout");
      lcd.setCursor(0,0);
      lcd.printstr("DHT11 TO");
      break;
    default:
      Serial.println("[ERR] dht11 unknown");
      lcd.setCursor(0,0);
      lcd.printstr("DHT11 UNK");
      break;
  }
  
  float lux = lightMeter.readLightLevel();
  if (lux == -1) {
    Serial.println("[ERR] BH1750 read error");
    lcd.setCursor(0,0);
    lcd.printstr("Light ERR");
  }
  
  lightReadL1 = analogRead(photoL1);
  lightReadL2 = analogRead(photoL2);
  lightReadL3 = analogRead(photoL3);
  lightReadL4 = analogRead(photoL4);
  
  // Read battery status
  batteryVoltage = readBatteryVoltage();
  batteryPercentage = calculateBatteryPercentage(batteryVoltage);
  chargingState = getChargingState(batteryVoltage);
  
  // Display sensor readings
  char lStr[6];
  dtostrf(lux, 4, 2, lStr);
  sprintf(buf, "h:%d,t:%dF,l:%s", DHT.humidity, CtoF(DHT.temperature), lStr);
  Serial.print(buf);
  
  sprintf(buf, " L1:%d,L2:%d", lightReadL1, lightReadL2);
  Serial.println(buf);
  
  sprintf(buf, " L3:%d,L4:%d", lightReadL3, lightReadL4);
  Serial.println(buf);
  
  char vStr[6];
  dtostrf(batteryVoltage, 4, 2, vStr);
  sprintf(buf, " Batt:%sV %d%%", vStr, batteryPercentage);
  Serial.println(buf);
  
  // Update LCD display
  lcd.clear();
  lcd.setCursor(0,0);
  sprintf(buf, "h:%d,t:%dF,l:%s", DHT.humidity, CtoF(DHT.temperature), lStr);
  lcd.printstr(buf);
  
  // Display battery status on second line
  displayBatteryStatus();
  
  // Adjust solar panel position only if environment is safe
  if (isEnvironmentSafe()) {
    adjustPanelPosition();
  } else {
    // Display environmental warning
    lcd.setCursor(0,1);
    lcd.printstr("ENV ERROR");
  }
  
  // Perform charging safety checks
  chargingSafetyCheck();
  
  // Check button state
  buttonState = digitalRead(button);
  if(buttonState == 0) {
    analogWrite(LED, HIGH);
    tone(buzzer, 1000, 100); // Short beep
    delay(500); // Debounce
  } else {
    // Only turn off LED if not in error or full charge state
    if (chargingState != ERROR && chargingState != FULLY_CHARGED) {
      digitalWrite(LED, LOW);
    }
  }
  
  // Safety check for battery voltage - beep every 10 seconds if there's an issue
  if ((batteryVoltage < BATTERY_MIN_VOLTAGE || batteryVoltage > BATTERY_MAX_VOLTAGE) && count % 5 == 0) {
    // Alert user to battery problem (every 10 seconds with 2 second delay)
    tone(buzzer, 500, 500);
    digitalWrite(LED, HIGH);
    delay(500);
    if (chargingState != FULLY_CHARGED) { // Don't turn off if fully charged
      digitalWrite(LED, LOW);
    }
  }
  
  count++;
  delay(2000); // Update every 2 seconds
}



int CtoF(int temp)
{
  return temp * 1.8 + 32;
}

int FtoC(int temp)
{
  return (temp - 32) * 0.55555;
}

void old_loop()
{
  int chk;
  chk = DHT.read(DHT11_PIN);
  switch (chk)
  {
    case DHTLIB_OK:
      break;
    case DHTLIB_ERROR_CHECKSUM:
      Serial.println("[ERR] dht11 checksum");
      break;
    case DHTLIB_ERROR_TIMEOUT:
      Serial.println("[ERR] dht11 timeout");
      break;
    default:
      break;
  }

  float lux = lightMeter.readLightLevel();
  lightReadL1 = analogRead(photoL1);
  lightReadL2 = analogRead(photoL2);
  lightReadL3 = analogRead(photoL3);
  lightReadL4 = analogRead(photoL4);
  {
    char buf[40];
    char lStr[6];
    dtostrf(lux, 4, 2, lStr);  // https://forum.arduino.cc/t/sprintf-with-float-values/937562
    sprintf(buf, "h: %d%%, t: %dF, l: %s, ", DHT.humidity, CtoF(DHT.temperature), lStr);
    Serial.print(buf);

    sprintf(buf, "L1: %d, L2: %d, L3: %d, L4: %d", lightReadL1, lightReadL2, lightReadL3, lightReadL4);
    Serial.println(buf);

    lcd.clear();
    // the LCD has two rows and 17 columns ...
    sprintf(buf, "h:%d,t:%d,l:%s", DHT.humidity, CtoF(DHT.temperature), lStr);
    lcd.setCursor(0,0);
    lcd.printstr(buf);

    sprintf(buf, "%d,%d,%d,%d", lightReadL1, lightReadL2, lightReadL3, lightReadL4);
    lcd.setCursor(0,1);
    lcd.printstr(buf);
  }
  delay(2000);

  buttonState = digitalRead(button);
  if(buttonState == 0) {
      analogWrite (LED, HIGH);
  } else {
    digitalWrite(LED, LOW);
  }
}

// Function to read battery voltage
float readBatteryVoltage() {
  int sensorValue = analogRead(batteryPin);
  // Convert the analog reading to voltage
  // Assuming a voltage divider circuit that scales 12V to 5V range
  float voltage = sensorValue * (12.0 / 1023.0);
  return voltage;
}

// Function to calculate battery percentage
int calculateBatteryPercentage(float voltage) {
  // Simple linear mapping between min and max voltages
  int percentage = map(voltage * 100, BATTERY_MIN_VOLTAGE * 100, BATTERY_FULL_VOLTAGE * 100, 0, 100);
  return constrain(percentage, 0, 100);
}

// Function to determine charging state
ChargingState getChargingState(float voltage) {
  if (voltage < BATTERY_MIN_VOLTAGE) {
    return ERROR;
  } else if (voltage >= BATTERY_FULL_VOLTAGE) {
    return FULLY_CHARGED;
  } else if (voltage > BATTERY_MIN_VOLTAGE && voltage < BATTERY_FULL_VOLTAGE) {
    return CHARGING;
  } else {
    return NOT_CHARGING;
  }
}

// Function to display battery status on LCD
void displayBatteryStatus() {
  char buf[40];
  char vStr[6];
  dtostrf(batteryVoltage, 4, 2, vStr);
  
  lcd.setCursor(0,1);
  
  // Display different messages based on charging state
  switch (chargingState) {
    case NOT_CHARGING:
      sprintf(buf, "B:%sV %d%% NC", vStr, batteryPercentage);
      break;
    case CHARGING:
      sprintf(buf, "B:%sV %d%% CG", vStr, batteryPercentage);
      break;
    case FULLY_CHARGED:
      sprintf(buf, "B:%sV %d%% FC", vStr, batteryPercentage);
      break;
    case ERROR:
      sprintf(buf, "B:%sV %d%% ER", vStr, batteryPercentage);
      break;
    default:
      sprintf(buf, "B:%sV %d%%", vStr, batteryPercentage);
      break;
  }
  lcd.printstr(buf);
}

void blink()
{
  digitalWrite(LED,HIGH);  //turn the LED on (HIGH is the voltage level
  delay(1000);  //wait for a second
  digitalWrite(LED,LOW);  //turn the LED off by making the voltage LOW
  delay(1000);  //wait for a second
}