# Space Mariner - Mission 01 Cadet flight computer
# Board:   Adafruit Feather RP2040 Adalogger (CircuitPython 9.x)
# Sensors: MS8607 (STEMMA QT), u-blox MAX-M10S GPS (STEMMA QT, 0x42),
#          DS18B20 waterproof probe on D5 with 4.7k pull-up to 3V
# Logs one CSV line per second to /sd/FLIGHTnnn.csv
# Required in CIRCUITPY/lib: adafruit_ms8607.mpy, adafruit_gps.mpy,
#   adafruit_onewire/ (folder), adafruit_ds18x20.mpy, adafruit_bus_device/ (folder)

import os
import time
import board
import busio
import sdcardio
import storage
import adafruit_ms8607
import adafruit_gps
from adafruit_bus_device.i2c_device import I2CDevice
from adafruit_onewire.bus import OneWireBus
import adafruit_ds18x20

LOG_INTERVAL = 1.0      # seconds between lines
PROBE_PIN = board.D5    # DS18B20 data wire (see wiring card)

# ---- microSD (built-in slot on the Feather RP2040 Adalogger) ----
def mount_sd():
    try:
        spi_sd = busio.SPI(board.SD_CLK, board.SD_MOSI, board.SD_MISO)
        card = sdcardio.SDCard(spi_sd, board.SD_CS)
        vfs = storage.VfsFat(card)
        storage.mount(vfs, "/sd")
        return True
    except Exception as e:  # no card, or card not formatted FAT32
        print("microSD not found:", e)
        return False


def new_log_path(prefix):
    # Never overwrite an old flight: pick the first unused number.
    n = 0
    while True:
        path = "/sd/%s%03d.csv" % (prefix, n)
        try:
            os.stat(path)
            n += 1
        except OSError:
            return path

# ---- u-blox MAX-M10S GPS over STEMMA QT (I2C address 0x42) ----
GPS_ADDR = 0x42


def ubx_packet(cls, msg_id, payload):
    body = bytes([cls, msg_id, len(payload) & 0xFF, len(payload) >> 8]) + payload
    ck_a = ck_b = 0
    for b in body:
        ck_a = (ck_a + b) & 0xFF
        ck_b = (ck_b + ck_a) & 0xFF
    return b"\xb5\x62" + body + bytes([ck_a, ck_b])


def set_airborne_mode(i2c):
    # UBX-CFG-VALSET, RAM + BBR layers, key CFG-NAVSPG-DYNMODEL (0x20110021)
    # value 6 = "Airborne <1g". Without this the GPS stops reporting above ~18 km.
    payload = bytes([0x00, 0x03, 0x00, 0x00,
                     0x21, 0x00, 0x11, 0x20,
                     0x06])
    pkt = ubx_packet(0x06, 0x8A, payload)
    while not i2c.try_lock():
        pass
    try:
        i2c.writeto(GPS_ADDR, pkt)
    finally:
        i2c.unlock()
    print("GPS airborne mode requested")


class UbloxI2C(adafruit_gps.GPS):
    """NMEA reader for u-blox modules over I2C (DDC). The u-blox reports how
    many bytes are waiting in registers 0xFD/0xFE and pads idle reads with
    0xFF, which Adafruit's GPS_GtopI2C class does not filter, so we feed the
    library's NMEA parser ourselves."""

    def __init__(self, i2c, address=GPS_ADDR):
        self._dev = I2CDevice(i2c, address)
        self._buf = bytearray()
        self._reg = bytearray(2)
        self._chunk = bytearray(64)
        super().__init__(None)

    def _fill(self):
        try:
            with self._dev as d:
                d.write_then_readinto(b"\xfd", self._reg)
                avail = (self._reg[0] << 8) | self._reg[1]
                if avail == 0 or avail == 0xFFFF:
                    return 0
                n = min(avail, len(self._chunk))
                d.readinto(self._chunk, end=n)
        except OSError:
            return 0
        for c in self._chunk[:n]:
            if c != 0xFF:
                self._buf.append(c)
        if len(self._buf) > 512:  # lost sync; drop old data
            self._buf = self._buf[-256:]
        return n

    @property
    def in_waiting(self):
        self._fill()
        return len(self._buf)

    def read(self, num_bytes=1):
        out = self._buf[:num_bytes]
        self._buf = self._buf[num_bytes:]
        return out

    def readline(self):
        while True:
            for i, c in enumerate(self._buf):
                if c == 0x0A:
                    line = bytes(self._buf[:i + 1])
                    self._buf = self._buf[i + 1:]
                    return line
            if self._fill() == 0:
                return b""

    def write(self, bytestr):
        with self._dev as d:
            d.write(bytestr)


def safe(fn, fmt="%.2f"):
    try:
        v = fn()
        return "" if v is None else fmt % v
    except Exception:
        return ""


i2c = board.STEMMA_I2C()
ms = adafruit_ms8607.MS8607(i2c)

set_airborne_mode(i2c)
time.sleep(0.2)
gps = UbloxI2C(i2c)

probe = None
try:
    ow = OneWireBus(PROBE_PIN)
    devs = ow.scan()
    if devs:
        probe = adafruit_ds18x20.DS18X20(ow, devs[0])
        probe.resolution = 11
    else:
        print("DS18B20 probe not found - check wires and 4.7k resistor")
except Exception as e:
    print("Probe error:", e)

have_sd = mount_sd()
path = new_log_path("FLIGHT") if have_sd else None
HEADER = ("seconds,gps_time,lat,lon,gps_alt_m,sats,fix,"
          "pressure_hpa,inside_temp_c,inside_rh,outside_temp_c")
if path:
    with open(path, "w") as f:
        f.write(HEADER + "\n")
    print("Logging to", path)
print(HEADER)

start = time.monotonic()
next_t = start
while True:
    # Feed the GPS parser often so sentences are not lost.
    for _ in range(4):  # drain several NMEA sentences per pass
        try:
            if not gps.update():
                break
        except Exception:
            break
    now = time.monotonic()
    if now < next_t:
        time.sleep(0.02)
        continue
    next_t += LOG_INTERVAL

    fix = gps.has_fix
    if fix and gps.timestamp_utc:
        t = gps.timestamp_utc
        gtime = "%02d:%02d:%02d" % (t.tm_hour, t.tm_min, t.tm_sec)
    else:
        gtime = ""
    row = [
        "%.1f" % (now - start),
        gtime,
        ("%.6f" % gps.latitude) if fix and gps.latitude is not None else "",
        ("%.6f" % gps.longitude) if fix and gps.longitude is not None else "",
        ("%.1f" % gps.altitude_m) if fix and gps.altitude_m is not None else "",
        str(gps.satellites or 0),
        "1" if fix else "0",
        safe(lambda: ms.pressure),
        safe(lambda: ms.temperature),
        safe(lambda: ms.relative_humidity, "%.1f"),
        safe(lambda: probe.temperature) if probe else "",
    ]

    line = ",".join(row)
    print(line)
    if path:
        try:
            with open(path, "a") as f:
                f.write(line + "\n")
        except OSError as e:
            print("SD write failed:", e)