from machine import I2C, Pin
import time
import ustruct
import math
import os

# --- I2C BUS CONFIGURATION ---
# I2C0: MPU6050 and BMP180 (GP0/GP1)
i2c0 = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
# I2C1: DS1307 (GP2/GP3) - Isolated to prevent address 0x68 conflict
i2c1 = I2C(1, sda=Pin(2), scl=Pin(3), freq=400000)

class DS1307:
    def __init__(self, i2c):
        self.i2c = i2c
        self.addr = 0x68
    def _bcd_to_dec(self, bcd):
        return ((bcd >> 4) * 10) + (bcd & 0x0F)
    def get_time(self):
        try:
            d = self.i2c.readfrom_mem(self.addr, 0x00, 7)
            s, m, h = self._bcd_to_dec(d[0] & 0x7F), self._bcd_to_dec(d[1]), self._bcd_to_dec(d[2] & 0x3F)
            day, month, year = self._bcd_to_dec(d[4]), self._bcd_to_dec(d[5]), self._bcd_to_dec(d[6]) + 2000
            return f"{year}-{month:02d}-{day:02d} {h:02d}:{m:02d}:{s:02d}"
        except: return "0000-00-00 00:00:00"

class BMP180:
    def __init__(self, i2c):
        self.i2c, self.addr, self.b5 = i2c, 0x77, 0
        try:
            c = self.i2c.readfrom_mem(self.addr, 0xAA, 22)
            # Unpack with correct signed/unsigned types
            # h=signed 16-bit, H=unsigned 16-bit
            self.AC1, self.AC2, self.AC3, self.AC4, self.AC5, self.AC6, \
            self.B1, self.B2, self.MB, self.MC, self.MD = ustruct.unpack('>hhhhhhHHHHH', c)
            
            # Manual signed correction for MC/MD if needed (Wokwi stability fix)
            if self.MC > 32767: self.MC -= 65536
            if self.MD > 32767: self.MD -= 65536
            print("BMP180: Calibration Handshake OK")
        except: print("BMP180: Init Failed")

    def get_temp(self):
        try:
            self.i2c.writeto_mem(self.addr, 0xF4, b'\x2E')
            time.sleep(0.02)
            raw = self.i2c.readfrom_mem(self.addr, 0xF6, 2)
            UT = (raw[0] << 8) + raw[1]
            x1 = (UT - self.AC6) * self.AC5 // 32768
            x2 = (self.MC * 2048) // (x1 + self.MD)
            self.b5 = x1 + x2
            return ((self.b5 + 8) // 16) / 10.0
        except: return -99.9

class MPU6050:
    def __init__(self, i2c):
        self.i2c = i2c
        self.addr = 0x68
        try: self.i2c.writeto_mem(self.addr, 0x6B, b'\x00')
        except: print("MPU6050: Init Failed")
    def get_accel(self):
        try:
            v = ustruct.unpack('>hhh', self.i2c.readfrom_mem(self.addr, 0x3B, 6))
            return (v[0]/16384.0, v[1]/16384.0, v[2]/16384.0)
        except: return (0, 0, 0)

# --- INITIALIZE DEVICES ---
rtc, imu, bmp = DS1307(i2c1), MPU6050(i2c0), BMP180(i2c0)

def log_data(ts, roll, pitch, temp):
    entry = f"{ts},{roll:.2f},{pitch:.2f},{temp:.2f}\n"
    try:
        with open("data.csv", "a") as f: f.write(entry)
        print(f"Logged: {entry.strip()}")
    except: print("SD/Flash Write Error")

# Header for CSV
if "data.csv" not in os.listdir():
    with open("data.csv", "w") as f: f.write("timestamp,roll,pitch,temp_c\n")

print("--- SMART BUOY SYSTEM ONLINE ---")

while True:
    try:
        t_stamp = rtc.get_time()
        temp_c = bmp.get_temp()
        ax, ay, az = imu.get_accel()
        
        # Calculate Tilt
        roll = math.atan2(ay, az) * 57.3
        pitch = math.atan2(-ax, math.sqrt(ay*ay + az*az)) * 57.3
        
        log_data(t_stamp, roll, pitch, temp_c)
    except Exception as e:
        print("Cycle Error:", e)
    
    time.sleep(5)