/*
  Project: Intelligent Connected Dual-Axis Solar Tracker
  Client: abdelfetah123
  Milestone: Day 2 - Drivers & Sensor Integration
  Platform: ESP32
  
  Description:
  - Initializes LDRs, Servos, INA219, and BH1750.
  - Reads sensor data and prints to Serial Monitor for verification.
  - Implements basic math for Power and Irradiance.
*/

#include <Wire.h>
#include <ESP32Servo.h>
#include <Adafruit_INA219.h>
#include <BH1750.h>

// --- Pin Definitions (Matches Wiring_And_Pinout.md) ---
// LDRs (Analog Inputs)
#define LDR_TOP_LEFT      34
#define LDR_TOP_RIGHT     35
#define LDR_BOT_LEFT      32
#define LDR_BOT_RIGHT     33

// Servos (PWM Outputs)
#define SERVO_AZ_PIN      18 // Horizontal
#define SERVO_EL_PIN      19 // Vertical

// I2C Pins (Default for ESP32)
#define SDA_PIN           21
#define SCL_PIN           22

// --- Constants & Globals ---
Servo servoAzimuth;
Servo servoElevation;

Adafruit_INA219 ina219;
BH1750 lightMeter;

// Solar Panel Physics Constants (Example values, tunable)
const float PANEL_AREA = 0.01; // m^2 (Example small panel)
const float EFFICIENCY = 0.18; // 18% efficiency

// Variables for Data
int tl, tr, bl, br;    // LDR values
float busVoltage = 0;  // V
float current_mA = 0;  // mA
float power_mW = 0;    // mW
float lux = 0;         // Lux
float irradiance = 0;  // W/m^2
int azPos = 90;        // Start position
int elPos = 90;        // Start position

void setup() {
  Serial.begin(115200);
  Serial.println("--- Starting Solar Tracker System (Day 2 Test) ---");

  // 1. Setup Servos
  servoAzimuth.attach(SERVO_AZ_PIN);
  servoElevation.attach(SERVO_EL_PIN);
  
  // Move to Home Position
  servoAzimuth.write(azPos);
  servoElevation.write(elPos);
  Serial.println("[Init] Servos homed to 90 degrees.");

  // 2. Setup I2C Sensors
  Wire.begin(SDA_PIN, SCL_PIN);
  
  // Initialize INA219 (Power)
  if (!ina219.begin()) {
    Serial.println("[Error] Failed to find INA219 chip");
  } else {
    Serial.println("[Init] INA219 Power Sensor Connected.");
  }

  // Initialize BH1750 (Lux)
  if (lightMeter.begin(BH1750::CONTINUOUS_HIGH_RES_MODE)) {
    Serial.println("[Init] BH1750 Lux Sensor Connected.");
  } else {
    Serial.println("[Error] Error initialising BH1750");
  }

  // 3. Setup LDR Pins
  pinMode(LDR_TOP_LEFT, INPUT);
  pinMode(LDR_TOP_RIGHT, INPUT);
  pinMode(LDR_BOT_LEFT, INPUT);
  pinMode(LDR_BOT_RIGHT, INPUT);
  
  Serial.println("--- Setup Complete ---");
}

void loop() {
  readSensors();
  calculatePhysics();
  printDebugData();
  
  // Simple test: wiggle servos slightly to prove they work
  testServos();

  delay(2000); // Wait 2 seconds between readings
}

void readSensors() {
  // Read LDRs (ESP32 ADC is 12-bit, 0-4095)
  tl = analogRead(LDR_TOP_LEFT);
  tr = analogRead(LDR_TOP_RIGHT);
  bl = analogRead(LDR_BOT_LEFT);
  br = analogRead(LDR_BOT_RIGHT);

  // Read Power Data
  busVoltage = ina219.getBusVoltage_V();
  current_mA = ina219.getCurrent_mA();
  power_mW = ina219.getPower_mW();

  // Read Lux
  lux = lightMeter.readLightLevel();
}

void calculatePhysics() {
  // 1. Calculate Irradiance (G)
  // Formula: G = Power(W) / (Efficiency * Area(m^2))
  // Power from sensor is mW, convert to W (/1000)
  float power_W = power_mW / 1000.0;
  
  if (power_W > 0) {
    irradiance = power_W / (EFFICIENCY * PANEL_AREA);
  } else {
    irradiance = 0;
  }
}

void printDebugData() {
  Serial.println("\n--- Sensor Readings ---");
  Serial.printf("LDRs  | TL:%d  TR:%d  BL:%d  BR:%d\n", tl, tr, bl, br);
  Serial.printf("Power | %.2f V  %.2f mA  %.2f mW\n", busVoltage, current_mA, power_mW);
  Serial.printf("Light | %.2f Lux  | Est. Irradiance: %.2f W/m^2\n", lux, irradiance);
}

void testServos() {
  // A gentle "heartbeat" movement to show they are active
  static bool toggle = false;
  if(toggle) {
    servoAzimuth.write(90 + 10);
    servoElevation.write(90 + 10);
  } else {
    servoAzimuth.write(90 - 10);
    servoElevation.write(90 - 10);
  }
  toggle = !toggle;
}