#![warn(clippy::all, clippy::pedantic)]

use crate::comms::{CommsState, CommsTask};
use crate::hardware::Hardware;
use crate::hardware::initialize;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
use crate::hardware::pin::PinDevice;
use crate::rcs::RcsTask;
use crate::scheduler::Scheduler;
use crate::state_vector::StateVector;
use anyhow::Result;
use log::info;
use nautilus_common::add_ctrlc_handler_arc;
use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep;
use std::time::Duration;
use crc::Algorithm;
use crate::drive::Drive;
use crate::drive::task::DriveTask;

mod hardware;

mod commanded_state;
mod comms;
mod data;
mod rcs;
mod scheduler;
mod state_vector;
mod drive;
#[cfg(test)]
mod test_utils;

pub const CRC_8_CCITT: Algorithm<u8> = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF };

fn new_shutdown_handler(running: &Arc<AtomicBool>) -> impl Fn(()) {
    let running = running.clone();
    move |()| running.store(false, Ordering::Relaxed)
}

/// Run the flight software
///
/// # Errors
/// An unrecoverable error had occurred
pub fn run() -> Result<()> {
    info!(
        "Project Nautilus Flight Software {}",
        env!("CARGO_PKG_VERSION")
    );

    let running = Arc::new(AtomicBool::new(true));
    add_ctrlc_handler_arc(running.clone())?;

    let state_vector = StateVector::new();

    let mut hal = initialize()?;

    let a4963 = hal.new_a4963()?;
    let mut mcp23017_a = hal.new_mcp23017_a()?;
    let mut mcp23017_b = hal.new_mcp23017_b()?;
    let pwm0 = hal.new_pwm0()?;

    info!("Battery Voltage: {}", hal.get_battery_voltage()?);

    mcp23017_a.init()?;
    mcp23017_b.init()?;

    Scheduler::scope(running.clone(), |s| {
        let task_a = s.run_cyclic(
            "mcp23017-a-task",
            Mcp23017Task::new(mcp23017_a, &state_vector, true),
            10,
        )?;
        let a_id = task_a.get_id();

        let task_b = s.run_cyclic(
            "mcp23017-b-task",
            Mcp23017Task::new(mcp23017_b, &state_vector, false),
            10,
        )?;
        let b_id = task_b.get_id();

        let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?;

        let drive = s.run_cyclic(
            "drive-task",
            DriveTask::new(a4963, pwm0),
            10
        )?;

        let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?;
        comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?;
        comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
        comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
        comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?;
        comms.add_command_handler("/drive/throttle/set", drive.new_set_throttle_callback())?;
        let comms = s.run_cyclic("comms-task", comms, 10)?;
        let comms_id = *comms;

        let sv = &state_vector;
        s.run_cyclic(
            "telemetry-producer",
            move || {
                sv.access_section(a_id, |state: &Mcp23017State| {
                    comms.send(TelemetryMessage::SwitchState {
                        bank: SwitchBank::A,
                        switches: state.pins,
                    });
                });
                sv.access_section(b_id, |state: &Mcp23017State| {
                    comms.send(TelemetryMessage::SwitchState {
                        bank: SwitchBank::B,
                        switches: state.pins,
                    });
                });
                sv.access_section(comms_id, |state: &CommsState| {
                    comms.send(state.clone().into());
                });
            },
            1,
        )?;

        info!("Starting Main Loop");

        while running.load(Ordering::Relaxed) {
            sleep(Duration::from_millis(1000));
        }

        anyhow::Ok(())
    })?;

    info!("Shutting Down");

    // Explicitly drop these to allow the borrow checker to be sure that
    // dropping the hal is safe
    // drop(pwm0);
    // drop(mct8316);

    drop(hal);

    drop(state_vector);

    Ok(())
}
