use crate::{
    encoders::{Encoders, Relative},
    tb6612fng::Tb6612fng,
};
use embassy_executor::Spawner;
use embassy_stm32::{peripherals::WWDG, time::hz};
use futures::FutureExt;
use heapless::Vec;
use i2c2_target::{I2C2, MESSAGE_SIZE};
use support::boot_control;

mod motor_control;
mod watchdog;

pub const I2C_ADDRESS: u8 = 0x57;

struct State {
    _i2c: I2C2,
    encoders: Encoders<'static>,
    tick_count: Relative,
}

impl State {
    fn new(i2c2: I2C2, encoders: Encoders<'static>) -> Self {
        let tick_count = Relative::new(&encoders);
        Self {
            _i2c: i2c2,
            encoders,
            tick_count,
        }
    }
}

#[allow(clippy::needless_pass_by_value, clippy::used_underscore_binding)] // Clippy bug
pub fn start_i2c_target(
    spawner: Spawner,
    i2c2: I2C2,
    motors: Tb6612fng<'static>,
    encoders: Encoders<'static>,
    _wwdg: WWDG,
) {
    spawner.spawn(i2c_engine(i2c2, encoders)).unwrap();
    spawner.spawn(watchdog::watchdog()).unwrap();
    spawner.spawn(motor_control::motor_control(motors)).unwrap();
}

#[embassy_executor::task]
async fn i2c_engine(i2c2: I2C2, encoders: Encoders<'static>) {
    let state = State::new(i2c2, encoders);
    I2C2::start(&i2c_callback, state).await;
}

fn i2c_callback(command: &[u8], response: &mut Vec<u8, MESSAGE_SIZE>, state: &mut State) {
    if process_command(state, command, response) {
        watchdog::ping();
    } else {
        // If this does not succeed, this will reset due to
        // a lack of petting the window watchdog.
        while motor_control::standby().now_or_never().is_none() {}
    }
}

const CMD_FIRMWARE_VERSION: u8 = 0x08;
const CMD_WHO_AM_I: u8 = 0x0f;
const CMD_PWM_FREQUENCY: u8 = 0x10;
const CMD_MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const CMD_MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
const CMD_MOTOR_SPEED: u8 = 0x30;
const CMD_ENCODER_TICKS: u8 = 0x32;
const CMD_STATUS: u8 = 0x36;
const CMD_RESET: u8 = 0xe0;
const CMD_RESET_BOOTLOADER: u8 = 0xe1;
const CMD_DEVICE_ID: u8 = 0xf0;

include!(concat!(env!("OUT_DIR"), "/version.rs"));

#[allow(clippy::cast_possible_truncation)]
fn process_command(
    state: &mut State,
    command: &[u8],
    response: &mut Vec<u8, MESSAGE_SIZE>,
) -> bool {
    defmt::trace!("Processing command {:?}", command);
    match command {
        [CMD_FIRMWARE_VERSION] => {
            for b in PKG_VERSION {
                response.push(b).unwrap();
            }
        }
        [CMD_WHO_AM_I] => {
            response.push(I2C_ADDRESS).unwrap();
        }
        &[CMD_PWM_FREQUENCY, a, b, c] => {
            let f = u32::from_le_bytes([a, b, c, 0]);
            if (1..=100_000).contains(&f) {
                if motor_control::set_pwm_frequency(hz(f))
                    .now_or_never()
                    .is_none()
                {
                    defmt::error!("unable to queue up motor frequency");
                    return false;
                }
            } else {
                defmt::warn!("incorrect PWM frequency {}", f);
                return false;
            }
        }
        [CMD_PWM_FREQUENCY] => {
            let freq = motor_control::get_pwm_frequency();
            for &b in &freq.0.to_le_bytes()[..3] {
                response.push(b).unwrap();
            }
        }
        &[CMD_MAX_MOTOR_PERCENTAGE, p] => {
            if (1..=100).contains(&p) {
                motor_control::set_max_motor_percentage(p);
            } else {
                defmt::warn!("incorrect max percentage {}", p);
                return false;
            }
        }
        [CMD_MAX_MOTOR_PERCENTAGE] => {
            response
                .push(motor_control::get_max_motor_percentage())
                .unwrap();
        }
        &[CMD_MOTOR_SPEED, left, right] => {
            if motor_control::set_speed(left, right)
                .now_or_never()
                .is_none()
            {
                defmt::error!("unable to queue up motor speeds");
                return false;
            }
        }
        [CMD_MOTOR_SPEED] => {
            let (left, right) = motor_control::get_speed();
            response.push(left).unwrap();
            response.push(right).unwrap();
        }
        &[CMD_MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
            if (1..=100).contains(&ticks) {
                watchdog::set_shutdown_timeout(ticks);
            } else {
                defmt::warn!("ticks out of range: {}", ticks);
                return false;
            }
        }
        [CMD_MOTOR_SHUTDOWN_TIMEOUT] => {
            response.push(watchdog::get_shutdown_timeout()).unwrap();
        }
        [CMD_ENCODER_TICKS] => {
            let (left, right) = state.tick_count.ticks(&state.encoders);
            let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
            response.push(left[0]).unwrap();
            response.push(left[1]).unwrap();
            response.push(right[0]).unwrap();
            response.push(right[1]).unwrap();
        }
        [CMD_STATUS] => {
            response.push(u8::from(motor_control::is_moving())).unwrap();
        }
        [CMD_RESET] => {
            defmt::info!("resetting device after receiving the RESET command");
            boot_control::reboot();
        }
        [CMD_RESET_BOOTLOADER] => {
            defmt::info!("resetting device into bootloader mode");
            boot_control::reboot_to_bootloader();
        }
        [CMD_DEVICE_ID] => {
            for &b in support::device_id() {
                response.push(b).unwrap();
            }
        }
        _ => {
            defmt::warn!("unknown command or args {:#04x}", command);
            return false;
        }
    }
    true
}