#![no_std]
#![no_main]

use core::sync::atomic::{AtomicBool, AtomicU32, Ordering::Relaxed};
use defmt::unwrap;
use embassy_executor::Spawner;
use embassy_stm32::{
    adc::{Adc, SampleTime},
    gpio::OutputType::PushPull,
    peripherals::{ADC1, PA0, TIM3},
    time::Hertz,
    timer::{
        low_level::CountingMode::EdgeAlignedUp,
        simple_pwm::{PwmPin, SimplePwm},
    },
    Config, Peri,
};
use embassy_time::{Duration, Timer};
use {defmt_rtt as _, panic_probe as _};

static SERVO_HZ: Hertz = Hertz::hz(60);
static SERVO_MIN_USECS: f32 = 490.0;
static SERVO_MAX_USECS: f32 = 2000.0;
static SERVO_MIN_DEG: f32 = -90.0;
static SERVO_MAX_DEG: f32 = 90.0;
static SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG);

static SENSOR_VALUE: AtomicU32 = AtomicU32::new(4095);
static SHOULD_EXIT: AtomicBool = AtomicBool::new(false);

fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 {
    let degrees = degrees.clamp(SERVO_MIN_DEG, SERVO_MAX_DEG);
    let micros = 0.5 + SERVO_MIN_USECS + SERVO_SLOPE * (degrees - SERVO_MIN_DEG);
    ((micros / max_dur.as_micros() as f32) * max_duty as f32) as u32
}

fn sensor2duty(sensor: u32, max_dur: Duration, max_duty: u32) -> u32 {
    let slope = 180.0 / (4095.0 - 300.0);
    let degrees = SERVO_MIN_DEG + slope * (sensor as f32 - 300.00);
    defmt::debug!("degrees {}", degrees);
    degrees2duty(degrees, max_dur, max_duty)
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let config = config();

    let peripherals = embassy_stm32::init(config);

    let pwm_pin = PwmPin::new(peripherals.PC7, PushPull);
    let pwm = SimplePwm::new(
        peripherals.TIM3,
        None,
        Some(pwm_pin),
        None,
        None,
        SERVO_HZ,
        EdgeAlignedUp,
    );

    let adc1 = Adc::new(peripherals.ADC1, Default::default());

    let sensor_input = peripherals.PA0.into();
    spawner.spawn(unwrap!(read_adc1_pa0(adc1, sensor_input)));

    spawner.spawn(unwrap!(servo_loop(pwm)));

    // calls the global exit after 30 seconds
    spawner.spawn(unwrap!(exit()));
}

#[embassy_executor::task]
async fn servo_loop(mut pwm: SimplePwm<'static, TIM3>) {
    let mut pwm_channel = pwm.ch2();
    pwm_channel.enable();

    let max_duty = pwm_channel.max_duty_cycle();
    let max_dur = Duration::from_hz(SERVO_HZ.0 as u64);

    let zero = degrees2duty(0.0, max_dur, max_duty);
    pwm_channel.set_duty_cycle(zero);
    Timer::after_millis(500).await;

    defmt::info!(
        "max duty: {}",
        degrees2duty(SERVO_MAX_DEG, max_dur, max_duty)
    );
    let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty);
    defmt::info!("min duty: {}", min_duty);

    //
    loop {
        if SHOULD_EXIT.load(Relaxed) {
            break;
        }
        let sensor = SENSOR_VALUE.load(Relaxed);
        let duty = sensor2duty(sensor, max_dur, max_duty);
        defmt::info!("duty: {}", duty);
        pwm_channel.set_duty_cycle(duty);
        Timer::after_millis(100).await;
    }

    pwm_channel.set_duty_cycle(degrees2duty(0.0, max_dur, max_duty));
    Timer::after_millis(600).await;

    treatbot_5k::exit();
}

#[embassy_executor::task]
async fn exit() {
    Timer::after_secs(30).await;
    SHOULD_EXIT.store(true, Relaxed);
}

#[embassy_executor::task]
async fn read_adc1_pa0(mut adc1: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) {
    loop {
        let val = adc1.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES640_5);
        defmt::info!("sensor val: {}", val);
        SENSOR_VALUE.store(val as u32, Relaxed);
        Timer::after_millis(100).await;
    }
}

fn config() -> Config {
    use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk};

    let mut config = Config::default();
    config.rcc.pll = Some(Pll {
        source: PllSource::HSI,
        prediv: PllPreDiv::DIV4,
        mul: PllMul::MUL85,
        divp: None,
        divq: None,
        // Main system clock at 170 MHz
        divr: Some(PllRDiv::DIV2),
    });
    config.rcc.mux.adc12sel = mux::Adcsel::SYS;
    config.rcc.sys = Sysclk::PLL1_R;

    config
}