import time
from machine import Pin, I2C, PWM
from mpu6050 import MPU6050  # Import MPU6050 driver
from bmp180 import BMP180  # Ensure you have this library

# BMP180 constants
BMP180_ADDR = 0x77  # Default I2C address for BMP180 sensor
CONTROL_REG = 0xF4
RESULT_REG = 0xF6

# BMP180 Commands
TEMPERATURE_CMD = 0x2E
PRESSURE_CMD = 0x34

# Set up I2C
i2c = I2C(0, scl=Pin(16), sda=Pin(17))  # Adjust the pins for your ESP32 setup

# Initialize BMP180 sensor
bmp = BMP180(i2c)

class BMP180:
    def __init__(self, i2c):
        self.i2c = i2c
        self.addr = BMP180_ADDR

        # Read calibration data from the sensor
        self.ac1 = self._read_signed_int16(0xAA)
        self.ac2 = self._read_signed_int16(0xAC)
        self.ac3 = self._read_signed_int16(0xAE)
        self.ac4 = self._read_unsigned_int16(0xB0)
        self.ac5 = self._read_unsigned_int16(0xB2)
        self.ac6 = self._read_unsigned_int16(0xB4)
        self.b1 = self._read_signed_int16(0xB6)
        self.b2 = self._read_signed_int16(0xB8)
        self.mb = self._read_signed_int16(0xBA)
        self.mc = self._read_signed_int16(0xBC)
        self.md = self._read_signed_int16(0xBE)

    def _read_signed_int16(self, reg):
        data = self.i2c.readfrom_mem(self.addr, reg, 2)
        return (data[0] << 8) + data[1] if data[0] >= 0x80 else (data[0] << 8) + data[1]

    def _read_unsigned_int16(self, reg):
        data = self.i2c.readfrom_mem(self.addr, reg, 2)
        return (data[0] << 8) + data[1]

    def _write(self, reg, value):
        self.i2c.writeto_mem(self.addr, reg, bytearray([value]))

    def _read_temperature(self):
        self._write(CONTROL_REG, TEMPERATURE_CMD)
        time.sleep(0.005)  # Wait for conversion
        msb, lsb = self.i2c.readfrom_mem(self.addr, RESULT_REG, 2)
        ut = (msb << 8) + lsb
        x1 = ((ut - self.ac6) * self.ac5) >> 15
        x2 = (self.mc << 11) // (x1 + self.md)
        b5 = x1 + x2
        temperature = (b5 + 8) >> 4
        return temperature

    def _read_pressure(self):
        self._write(CONTROL_REG, PRESSURE_CMD)
        time.sleep(0.026)  # Wait for conversion
        msb, lsb, xlsb = self.i2c.readfrom_mem(self.addr, RESULT_REG, 3)
        up = (msb << 16) + (lsb << 8) + xlsb
        up >>= 8  # Adjust for MSB
        return up

    def get_temperature(self):
        temperature = self._read_temperature()
        return temperature / 10.0  # Convert to Celsius

    def get_pressure(self):
        pressure = self._read_pressure()
        return pressure  # Return pressure in Pa

    def get_altitude(self, pressure):
        # Assume sea-level pressure of 1013.25 hPa
        return 44330 * (1 - (pressure / 101325) ** 0.1903)
       
def get_calibrated_altitude():
    global ground_altitude
    temperature = bmp180.temperature  # Use 'temperature' property
    pressure = bmp180.pressure
    raw_altitude = bmp180.altitude  # Uncalibrated altitude

    # Set ground level at startup
    if ground_altitude is None:
        ground_altitude = raw_altitude
        print(f"Calibrating... Ground Altitude Set to {ground_altitude:.2f} m")

    # Compute relative altitude
    calibrated_altitude = raw_altitude - ground_altitude

    print(f"Temperature: {temperature:.2f}°C | Pressure: {pressure} Pa | Altitude: {calibrated_altitude:.2f} m")
    
    return calibrated_altitude  # This return should be at the same level as the rest of the code




# Initialize I2C for MPU6050
i2c_mpu = I2C(0, scl=Pin(1), sda=Pin(2))  # Adjust pins as per your ESP32-S3 setup
mpu = MPU6050(i2c_mpu)

# Buzzer for startup tone
buzzer = PWM(Pin(19))

def startup_tone():
    melody = [(1000, 0.1), (1200, 0.1), (1500, 0.1)]  # Simple tone sequence
    for freq, duration in melody:
        buzzer.freq(freq)
        buzzer.duty(512)
        time.sleep(duration)
    buzzer.duty(0)  # Turn off buzzer

# PID Controller for stabilization
class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0

    def compute(self, setpoint, measured_value):
        error = setpoint - measured_value
        self.integral += error
        derivative = error - self.prev_error
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

# Drone Lamp Control using L298N Motor Driver
class DroneLamps:
    def __init__(self):
        # L298N Motor Driver Pins
        self.in1 = Pin(10, Pin.OUT)
        self.in2 = Pin(11, Pin.OUT)
        self.in3 = Pin(12, Pin.OUT)
        self.in4 = Pin(13, Pin.OUT)
        
        # PWM Pins for Speed Control
        self.ena = PWM(Pin(9), freq=1000)   # Speed control for Lamp 1 & 2
        self.enb = PWM(Pin(14), freq=1000)  # Speed control for Lamp 3 & 4

    def control_motor(self, motor, direction, speed):
        """Controls a motor (1-4) with a given direction and speed."""
        if motor == 1:
            self.in1.value(direction)
            self.in2.value(not direction)
            self.ena.duty(speed)
        elif motor == 2:
            self.in3.value(direction)
            self.in4.value(not direction)
            self.enb.duty(speed)

    def adjust_lamps(self, corrections):
        """Apply PID corrections to lamps (simulating motor thrust)."""
        speeds = [max(0, min(1023, int(abs(correction) * 500))) for correction in corrections]
        directions = [1 if correction > 0 else 0 for correction in corrections]

        # Control each motor
        self.control_motor(1, directions[0], speeds[0])  # Front-left
        self.control_motor(2, directions[1], speeds[1])  # Front-right
        self.control_motor(3, directions[2], speeds[2])  # Rear-left
        self.control_motor(4, directions[3], speeds[3])  # Rear-right

        print(f"Lamp States: {directions} | Speeds: {speeds}")

# Initialize PID and Lamps
pid_pitch = PIDController(1.2, 0.1, 0.05)
pid_roll = PIDController(1.2, 0.1, 0.05)
lamps = DroneLamps()

# Play startup tone
startup_tone()

# Test MPU6050 Data Retrieval
def test_accelerometer():
    ax, ay, az = mpu.get_accel()
    print(f"Raw Accelerometer Data - ax: {ax}, ay: {ay}, az: {az}")

    # Normalize accelerometer data (assume ax, ay values are in the range of -1 to 1)
    ax_normalized = ax / 16384.0  # Normalize to G (±1g = ±16384 counts)
    ay_normalized = ay / 16384.0  # Normalize to G (±1g = ±16384 counts)

    print(f"Normalized Accelerometer Data - ax: {ax_normalized}, ay: {ay_normalized}")
    return ax_normalized, ay_normalized

# Test PID with real-time values
def test_pid(ax, ay):
    # Target for pitch and roll (stable hover at 0°)
    setpoint_pitch = 0
    setpoint_roll = 0
    
    correction_pitch = pid_pitch.compute(setpoint_pitch, ax)
    correction_roll = pid_roll.compute(setpoint_roll, ay)

    print(f"PID Pitch Correction: {correction_pitch}, Roll Correction: {correction_roll}")

    # Distribute corrections to lamps
    lamp_corrections = [
        correction_pitch + correction_roll,  # Front-left lamp (Motor 1)
        correction_pitch - correction_roll,  # Front-right lamp (Motor 2)
        -correction_pitch + correction_roll, # Rear-left lamp (Motor 3)
        -correction_pitch - correction_roll  # Rear-right lamp (Motor 4)
    ]

    # Apply corrections to lamps
    lamps.adjust_lamps(lamp_corrections)

# Main loop
print("Starting Drone Flight Simulation with L298N-controlled Lamps...")

while True:
    # Test MPU6050 Data
    ax, ay = test_accelerometer()  # Get accelerometer data
    
    # Test PID with real-time accelerometer values
    test_pid(ax, ay)  # Test PID with actual accelerometer data and lamp control
    
    # Time delay to prevent excessive data printing
    time.sleep(2)  # Wait 2 seconds before next iteration

    # Read temperature and pressure
    temp = bmp180.temperature()
    pressure = bmp.get_pressure()
    altitude = bmp.get_altitude(pressure)

    # Print results
    print("Temperature: {:.2f} C".format(temp))
    print("Pressure: {:.2f} Pa".format(pressure))
    print("Altitude: {:.2f} meters".format(altitude))

    time.sleep(2)  # Wait 2 seconds before the next reading