// src/mpu/task.rs
//! MPU6050 sampling task.
//!
//! This task:
//! 1. Initializes the MPU6050 sensor
//! 2. Continuously reads sensor data at a fixed rate
//! 3. Publishes readings to the IMU channel

use embassy_time::{Duration, Instant, Timer};
use log::{error, info, warn};

use crate::bus::I2cDevice;
use crate::contracts::ImuReading;
use crate::mpu::api::sender;
use crate::mpu::driver::Mpu6050;

/// Sampling interval in milliseconds.
/// 50ms = 20Hz
const SAMPLE_INTERVAL_MS: u64 = 50;

/// MPU6050 I2C address (0x68 with AD0 low, 0x69 with AD0 high)
const MPU_ADDR: u8 = 0x68;

/// The MPU6050 sampling task.
///
/// # Arguments
/// * `i2c` - An I2C device handle from the shared bus
///
/// # Panics
/// Does not panic; logs errors and retries on failure.
#[embassy_executor::task]
pub async fn mpu_task(i2c: I2cDevice) {
    info!("MPU task starting...");

    let mut mpu = Mpu6050::new(i2c, MPU_ADDR);

    // Initialize with retries
    let mut init_attempts = 0;
    loop {
        init_attempts += 1;

        // Verify device is present
        match mpu.verify() {
            Ok(true) => {
                info!("MPU6050 detected at 0x{:02X}", MPU_ADDR);
            }
            Ok(false) => {
                warn!(
                    "Device at 0x{:02X} is not MPU6050 (attempt {})",
                    MPU_ADDR, init_attempts
                );
                Timer::after(Duration::from_secs(2)).await;
                continue;
            }
            Err(_) => {
                warn!(
                    "I2C error verifying MPU6050 (attempt {})",
                    init_attempts
                );
                Timer::after(Duration::from_secs(2)).await;
                continue;
            }
        }

        // Initialize sensor
        match mpu.init() {
            Ok(()) => {
                info!("MPU6050 initialized successfully");
                break;
            }
            Err(_) => {
                error!("MPU6050 init failed (attempt {})", init_attempts);
                Timer::after(Duration::from_secs(2)).await;
                continue;
            }
        }
    }

    // Allow sensor to stabilize after wake-up
    Timer::after(Duration::from_millis(100)).await;
    info!(
        "MPU task entering sampling loop ({}ms interval)",
        SAMPLE_INTERVAL_MS
    );

    let tx = sender();
    let mut consecutive_errors = 0u32;
    let mut sent_count: u32 = 0;

    loop {
        let start = Instant::now();

        match mpu.read() {
            Ok((accel_g, gyro_dps, temp_c)) => {
                consecutive_errors = 0;

                let reading = ImuReading {
                    accel_g,
                    gyro_dps,
                    temp_c,
                    timestamp_ms: start.as_millis(),
                };

                sent_count = sent_count.wrapping_add(1);
                if tx.try_send(reading).is_ok() {
                    if sent_count % 20 == 0 {
                        info!(
                            "IMU send#{} ax:{:.2} ay:{:.2} az:{:.2} t:{:.1}",
                            sent_count, accel_g[0], accel_g[1], accel_g[2], temp_c
                        );
                    }
                } else if sent_count % 20 == 0 {
                    info!("IMU drop: channel full ({} sent)", sent_count);
                }
            }

            Err(_) => {
                consecutive_errors += 1;
                if consecutive_errors == 1 || consecutive_errors % 10 == 0 {
                    warn!("MPU read error (consecutive: {})", consecutive_errors);
                }

                // If too many errors, try to reinitialize
                if consecutive_errors >= 50 {
                    error!("Too many MPU errors, attempting reinit...");
                    if mpu.init().is_ok() {
                        info!("MPU reinit successful");
                        consecutive_errors = 0;
                        Timer::after(Duration::from_millis(100)).await;
                    }
                }
            }
        }

        // Maintain a steady period
        let elapsed = start.elapsed();
        let target = Duration::from_millis(SAMPLE_INTERVAL_MS);
        if elapsed < target {
            Timer::after(target - elapsed).await;
        }
    }
}