UART, DMA, and Ownership

use embassy_rp::uart::{self, Config as UartConfig, InterruptHandler as UartInterruptHandler};
use embassy_rp::bind_interrupts;
use embassy_rp::peripherals::UART0;

// Bind UART0 interrupt for async operation
bind_interrupts!(struct Irqs {
    UART0_IRQ => UartInterruptHandler<UART0>;
});

#[embassy_executor::main]
async fn main(_spawner: Spawner) {
    let p = embassy_rp::init(Default::default());

    // Configure UART0 at 9600 baud (GPS default)
    let mut uart_config = UartConfig::default();
    uart_config.baudrate = 9600;

    let uart = uart::Uart::new(
        p.UART0,
        p.PIN_0,  // TX
        p.PIN_1,  // RX
        Irqs,
        p.DMA_CH0, // DMA channel for TX
        p.DMA_CH1, // DMA channel for RX
        uart_config,
    );
}

let (tx, rx) = uart.split();

// Now tx and rx can be moved into separate tasks
spawner.spawn(gps_receiver_task(rx)).unwrap();
spawner.spawn(gps_sender_task(tx)).unwrap();

use embassy_rp::uart::UartRx;

#[embassy_executor::task]
async fn reader_task(mut rx: UartRx<'static, UART0, uart::Async>) {
    let mut buf = [0u8; 128];

    loop {
        // Read exactly 128 bytes. This awaits until the DMA
        // has filled the entire buffer.
        match rx.read(&mut buf).await {
            Ok(()) => {
                defmt::info!("Received {} bytes", buf.len());
            }
            Err(e) => {
                defmt::error!("UART read error: {:?}", e);
            }
        }
    }
}

/// Read UART bytes into a line buffer until '\n' is found.
/// Returns the number of bytes in the line (including '\n').
async fn read_line(
    rx: &mut UartRx<'static, UART0, uart::Async>,
    line_buf: &mut [u8],
) -> Result<usize, uart::Error> {
    let mut pos = 0usize;

    loop {
        let mut byte = [0u8; 1];
        rx.read(&mut byte).await?;

        if pos < line_buf.len() {
            line_buf[pos] = byte[0];
            pos += 1;
        }

        if byte[0] == b'\n' {
            return Ok(pos);
        }

        // Safety limit: if the line is too long, return what we have
        if pos >= line_buf.len() {
            return Ok(pos);
        }
    }
}

use embassy_rp::uart::UartTx;

#[embassy_executor::task]
async fn writer_task(mut tx: UartTx<'static, UART0, uart::Async>) {
    let message = b"Hello from Pico!\r\n";

    loop {
        // Write the entire message. The DMA handles the transfer.
        tx.write(message).await.unwrap();
        Timer::after_secs(1).await;
    }
}

// C: Start DMA reception into buffer
uint8_t rx_buffer[256];
dma_channel_configure(dma_chan, &cfg,
    rx_buffer,           // Destination: raw pointer
    &uart0_hw->dr,       // Source: UART data register
    256,                 // Transfer count
    true);               // Start immediately

// WARNING: rx_buffer is now being written by DMA hardware.
// Nothing in the C language prevents you from doing this:
rx_buffer[0] = 0xFF;    // DATA CORRUPTION! DMA is still writing!
memcpy(other_buf, rx_buffer, 256);  // READING PARTIAL DATA!
printf("%s", rx_buffer);  // UNDEFINED BEHAVIOR!

// Rust: Start DMA reception into buffer
let rx_buffer = [0u8; 256];

// The transfer TAKES OWNERSHIP of rx_buffer.
// After this line, rx_buffer no longer exists in this scope.
let transfer = rx.read(rx_buffer).await;

// This would NOT compile:
// rx_buffer[0] = 0xFF;
//   error: use of moved value: `rx_buffer`

// The buffer is returned when the transfer completes.
// Only then can you access the data.

let mut buf = [0u8; 256];

// During this .await, `buf` is exclusively borrowed by the UART driver.
// The compiler prevents any other access to `buf` in this scope.
rx.read(&mut buf).await.unwrap();

// Now the borrow is released. We can safely read the DMA'd data.
process_data(&buf);

// C double-buffer DMA: notoriously bug-prone
uint8_t buf_a[256];
uint8_t buf_b[256];
volatile int current_buf = 0;

void dma_irq_handler() {
    if (current_buf == 0) {
        // Switch DMA to buf_b
        dma_set_write_addr(dma_chan, buf_b);
        current_buf = 1;
        // Process buf_a... but is the main loop also touching buf_a?
    } else {
        dma_set_write_addr(dma_chan, buf_a);
        current_buf = 0;
    }
    dma_start(dma_chan);
}

// Main loop
while (1) {
    if (current_buf == 1) {
        process(buf_a);  // Is DMA really done with buf_a? Race condition!
    }
}

$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
       |      | |         | |         | |     |     |      |     |
       |      | |         | |         | |     |     |      |     Checksum
       |      | |         | |         | |     |     |      Magnetic var
       |      | |         | |         | |     |     Date (DDMMYY)
       |      | |         | |         | |     Course (degrees)
       |      | |         | |         | Speed (knots)
       |      | |         | Longitude
       |      | Latitude
       |      Status (A=active, V=void)
       Time (HHMMSS)

/// Parsed GPS position data
#[derive(Clone, Copy, defmt::Format)]
struct GpsPosition {
    latitude: f32,       // Decimal degrees (positive = North)
    longitude: f32,      // Decimal degrees (positive = East)
    altitude_m: f32,     // Meters above sea level
    speed_knots: f32,    // Speed over ground
    satellites: u8,      // Number of satellites used
    fix_valid: bool,     // True if GPS has a valid fix
    hours: u8,
    minutes: u8,
    seconds: u8,
}

impl GpsPosition {
    const fn empty() -> Self {
        Self {
            latitude: 0.0,
            longitude: 0.0,
            altitude_m: 0.0,
            speed_knots: 0.0,
            satellites: 0,
            fix_valid: false,
            hours: 0,
            minutes: 0,
            seconds: 0,
        }
    }
}

/// Parse an NMEA sentence and update the position if relevant.
/// Returns true if the position was updated.
fn parse_nmea(line: &[u8], pos: &mut GpsPosition) -> bool {
    // Validate minimum length and start character
    if line.len() < 10 || line[0] != b'$' {
        return false;
    }

    // Verify checksum
    if !verify_checksum(line) {
        return false;
    }

    // Extract the sentence type (e.g., "GPRMC", "GPGGA")
    let sentence_type = &line[1..6];

    if sentence_type == b"GPRMC" {
        parse_gprmc(line, pos)
    } else if sentence_type == b"GPGGA" {
        parse_gpgga(line, pos)
    } else {
        false
    }
}

/// Verify the NMEA checksum.
/// The checksum is the XOR of all bytes between '$' and '*'.
fn verify_checksum(line: &[u8]) -> bool {
    let mut checksum: u8 = 0;
    let mut found_star = false;
    let mut expected: u8 = 0;

    for (i, &byte) in line.iter().enumerate() {
        if byte == b'$' {
            continue;
        }
        if byte == b'*' {
            found_star = true;
            // Next two bytes are the hex checksum
            if i + 2 < line.len() {
                expected = hex_to_u8(line[i + 1], line[i + 2]);
            }
            break;
        }
        checksum ^= byte;
    }

    found_star && checksum == expected
}

fn hex_to_u8(high: u8, low: u8) -> u8 {
    let h = match high {
        b'0'..=b'9' => high - b'0',
        b'A'..=b'F' => high - b'A' + 10,
        b'a'..=b'f' => high - b'a' + 10,
        _ => 0,
    };
    let l = match low {
        b'0'..=b'9' => low - b'0',
        b'A'..=b'F' => low - b'A' + 10,
        b'a'..=b'f' => low - b'a' + 10,
        _ => 0,
    };
    (h << 4) | l
}

/// Split a byte slice by commas, returning an array of field slices.
/// Returns the number of fields found.
fn split_fields<'a>(
    data: &'a [u8],
    fields: &mut [&'a [u8]; 20],
) -> usize {
    let mut count = 0usize;
    let mut start = 0usize;

    for i in 0..data.len() {
        if data[i] == b',' || data[i] == b'*' {
            if count < 20 {
                fields[count] = &data[start..i];
                count += 1;
            }
            start = i + 1;
            if data[i] == b'*' {
                break;
            }
        }
    }

    count
}

/// Parse a GPRMC sentence for position, speed, and time.
fn parse_gprmc(line: &[u8], pos: &mut GpsPosition) -> bool {
    let mut fields = [&[] as &[u8]; 20];
    let count = split_fields(line, &mut fields);

    if count < 10 {
        return false;
    }

    // Field 0: sentence type ($GPRMC)
    // Field 1: time (HHMMSS.SS)
    // Field 2: status (A=active, V=void)
    // Field 3: latitude (DDMM.MMMM)
    // Field 4: N/S
    // Field 5: longitude (DDDMM.MMMM)
    // Field 6: E/W
    // Field 7: speed in knots
    // Field 8: course
    // Field 9: date (DDMMYY)

    // Check fix status
    if fields[2].is_empty() || fields[2][0] != b'A' {
        pos.fix_valid = false;
        return false;
    }

    pos.fix_valid = true;

    // Parse time
    if fields[1].len() >= 6 {
        pos.hours = parse_u8_2digit(fields[1][0], fields[1][1]);
        pos.minutes = parse_u8_2digit(fields[1][2], fields[1][3]);
        pos.seconds = parse_u8_2digit(fields[1][4], fields[1][5]);
    }

    // Parse latitude (DDMM.MMMM format)
    if fields[3].len() >= 4 {
        let degrees = parse_u8_2digit(fields[3][0], fields[3][1]) as f32;
        let minutes = parse_nmea_float(fields[3]);
        let minutes_val = minutes - (degrees * 100.0);
        pos.latitude = degrees + minutes_val / 60.0;

        if !fields[4].is_empty() && fields[4][0] == b'S' {
            pos.latitude = -pos.latitude;
        }
    }

    // Parse longitude (DDDMM.MMMM format)
    if fields[5].len() >= 5 {
        let degrees_str = &fields[5][..3];
        let degrees = parse_u8_3digit(degrees_str) as f32;
        let minutes = parse_nmea_float(fields[5]);
        let minutes_val = minutes - (degrees * 100.0);
        pos.longitude = degrees + minutes_val / 60.0;

        if !fields[6].is_empty() && fields[6][0] == b'W' {
            pos.longitude = -pos.longitude;
        }
    }

    // Parse speed in knots
    if !fields[7].is_empty() {
        pos.speed_knots = parse_nmea_float(fields[7]);
    }

    true
}

/// Parse a GPGGA sentence for altitude and satellite count.
fn parse_gpgga(line: &[u8], pos: &mut GpsPosition) -> bool {
    let mut fields = [&[] as &[u8]; 20];
    let count = split_fields(line, &mut fields);

    if count < 10 {
        return false;
    }

    // Field 0: sentence type ($GPGGA)
    // Field 6: fix quality (0=invalid, 1=GPS fix, 2=DGPS fix)
    // Field 7: number of satellites
    // Field 9: altitude in meters

    // Fix quality
    if fields[6].is_empty() || fields[6][0] == b'0' {
        return false;
    }

    // Number of satellites
    if !fields[7].is_empty() {
        pos.satellites = parse_u8_field(fields[7]);
    }

    // Altitude
    if !fields[9].is_empty() {
        pos.altitude_m = parse_nmea_float(fields[9]);
    }

    true
}

// -----------------------------------------------------------
// Numeric parsing helpers (no_std, no alloc)
// -----------------------------------------------------------

fn parse_u8_2digit(high: u8, low: u8) -> u8 {
    (high.wrapping_sub(b'0')) * 10 + (low.wrapping_sub(b'0'))
}

fn parse_u8_3digit(s: &[u8]) -> u16 {
    if s.len() < 3 {
        return 0;
    }
    let h = s[0].wrapping_sub(b'0') as u16;
    let m = s[1].wrapping_sub(b'0') as u16;
    let l = s[2].wrapping_sub(b'0') as u16;
    h * 100 + m * 10 + l
}

fn parse_u8_field(s: &[u8]) -> u8 {
    let mut val = 0u8;
    for &b in s {
        if b >= b'0' && b <= b'9' {
            val = val.wrapping_mul(10).wrapping_add(b - b'0');
        } else {
            break;
        }
    }
    val
}

/// Parse a float from a byte slice (e.g., "4807.038" or "22.4").
/// Simple implementation for NMEA fields; handles integer and one decimal part.
fn parse_nmea_float(s: &[u8]) -> f32 {
    let mut integer_part: i32 = 0;
    let mut decimal_part: i32 = 0;
    let mut decimal_divisor: f32 = 1.0;
    let mut in_decimal = false;
    let mut negative = false;

    for &b in s {
        if b == b'-' {
            negative = true;
        } else if b == b'.' {
            in_decimal = true;
        } else if b >= b'0' && b <= b'9' {
            if in_decimal {
                decimal_part = decimal_part * 10 + (b - b'0') as i32;
                decimal_divisor *= 10.0;
            } else {
                integer_part = integer_part * 10 + (b - b'0') as i32;
            }
        }
    }

    let val = integer_part as f32 + decimal_part as f32 / decimal_divisor;
    if negative { -val } else { val }
}

[package]
name = "gps-parser"
version = "0.1.0"
edition = "2021"

[dependencies]
embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread"] }
embassy-rp = { version = "0.3", features = ["time-driver", "rp2040"] }
embassy-time = { version = "0.4", features = ["generic-queue-8"] }
embassy-sync = "0.6"

cortex-m = { version = "0.7", features = ["inline-asm"] }
cortex-m-rt = "0.7"
panic-halt = "1.0"
defmt = "0.3"
defmt-rtt = "0.4"

portable-atomic = { version = "1.10", features = ["critical-section"] }
static_cell = "2.1"

[profile.release]
opt-level = "s"
debug = true
lto = true
codegen-units = 1