import time
from machine import Pin, I2C, PWM
from mpu6050 import MPU6050  # Import MPU6050 driver

# BMP180 Constants and Initialization
BMP180_ADDR = 0x77  # Default I2C address for BMP180 sensor
CONTROL_REG = 0xF4
RESULT_REG = 0xF6
TEMPERATURE_CMD = 0x2E
PRESSURE_CMD = 0x34

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)

# Initialize I2C for BMP180
i2c_bmp = I2C(0, scl=Pin(16), sda=Pin(17))  # Adjust the pins for your ESP32 setup

# Initialize BMP180 sensor
bmp = BMP180(i2c_bmp)

# Initialize I2C for MPU6050
i2c_mpu = I2C(0, scl=Pin(22), sda=Pin(21))  # Pins for MPU6050
mpu = MPU6050(i2c_mpu)

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

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

# Simulated 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

# Simulated Drone Lamps as Motors
class DroneLamps:
    def __init__(self):
        self.lamp_pins = [Pin(5, Pin.OUT), Pin(18, Pin.OUT), Pin(19, Pin.OUT), Pin(23, Pin.OUT)]
        self.brightness = [0, 0, 0, 0]  # Individual brightness levels for each lamp

    def adjust_lamps(self, corrections):
        # Lamp control logic - print debugging the corrections applied
        print(f"Adjusting lamps with corrections: {corrections}")
        for i, correction in enumerate(corrections):
            self.brightness[i] = 1 if correction > 0 else 0  # Simulating ON/OFF behavior
            self.lamp_pins[i].value(self.brightness[i])
        print(f"Lamp States: {[ 'ON' if b else 'OFF' for b in self.brightness ]}")

# 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 manual values
def test_pid(ax, ay):
    # Test PID logic by giving a fixed setpoint (e.g., 0 for pitch and roll)
    setpoint_pitch = 0  # Target for pitch (e.g., 0 degrees - level position)
    setpoint_roll = 0   # Target for roll (e.g., 0 degrees - level position)
    
    correction_pitch = pid_pitch.compute(setpoint_pitch, ax)  # Use ax as pitch
    correction_roll = pid_roll.compute(setpoint_roll, ay)     # Use ay as roll
    
    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 Lamps and BMP180 Sensor...")

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
    
    # Read BMP180 temperature and pressure
    temp = bmp.get_temperature()
    pressure = bmp.get_pressure()
    altitude = bmp.get_altitude(pressure)
    print(f"Temperature: {temp:.2f} C, Pressure: {pressure:.2f} Pa, Altitude: {altitude:.2f} meters")
    
    # Time delay to prevent excessive data printing
    time.sleep(2)  # Wait 2 seconds before the next iteration