//! # Pimoroni Servo2040 PWM Micro Servo Example
//!
//! Moves the micro servo on a Servo2040 board using the PWM peripheral.
//!
//! This will move in different positions the motor attached to GP0.
#![no_std]
#![no_main]

// GPIO traits
use embedded_hal::delay::DelayNs;
use embedded_hal::pwm::SetDutyCycle;

// Ensure we halt the program on panic (if we don't mention this crate it won't
// be linked)
use panic_halt as _;

// A shorter alias for the Peripheral Access Crate, which provides low-level
// register access
use pimoroni_servo2040::hal::pac;

// A shorter alias for the Hardware Abstraction Layer, which provides
// higher-level drivers.
use pimoroni_servo2040::hal;

/// Number of microseconds for the pwm signal period.
const PERIOD_US: u32 = 20_000;
/// Max resolution for the pwm signal.
const TOP: u16 = u16::MAX;

#[pimoroni_servo2040::entry]
fn main() -> ! {
    let mut pac = pac::Peripherals::take().unwrap();
    let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);

    let sio = hal::Sio::new(pac.SIO);

    let clocks = hal::clocks::init_clocks_and_plls(
        pimoroni_servo2040::XOSC_CRYSTAL_FREQ,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    // Configure the Timer peripheral in count-down mode
    let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS, &clocks);

    let pins = pimoroni_servo2040::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let pwm_slices = hal::pwm::Slices::new(pac.PWM, &mut pac.RESETS);

    const MIN_PULSE: u16 = 1000;
    const MID_PULSE: u16 = 1500;
    const MAX_PULSE: u16 = 2000;

    let mut pwm: hal::pwm::Slice<_, _> = pwm_slices.pwm0;

    // 50Hz desired frequency
    // Rp2040 clock = 125MHz
    // Top = 65_535, resolution for counter (maximum possible u16 value)
    // Wrap = Top+1 (number of possible values)
    // Phase correction multiplier = 2 if phase correction enabled, else 1

    // Divider = rp2040_clock / (Wrap * phase_correction_multiplier * desired_frequency)
    // Divider = 125,000,000/(65_536 * 1 * 50)
    // Divider = 38.14639
    // Divider int = 38
    // Divider frac = 3 (3/16 = 0.1875, smallest frac greater than desired clock divider).
    pwm.set_div_int(38);
    pwm.set_div_frac(3);

    // If phase correction enabled, then values would be:
    // Divider = rp2040_clock / (Wrap * phase_correction_multiplier * desired_frequency)
    // Divider = 125,000,000/(65_536 * 2 * 50)
    // Divider = 19.073195
    // Divider int = 19
    // Divider frac = 2 (2/16 = .1250, smallest frac greater than desired clock divider).

    // pwm.set_ph_correct();
    // pwm.set_div_int(19);
    // pwm.set_div_frac(2);
    pwm.set_top(TOP);
    pwm.enable();

    // Output channel A on PWM0 to the GPIO0/servo1 pin
    let mut channel_a = pwm.channel_a;
    let _channel_a_pin = channel_a.output_to(pins.servo1);
    let movement_delay_ms = 400;

    // Infinite loop, moving micro servo from one position to another.
    // You may need to adjust the pulse width since several servos from
    // different manufacturers respond differently.
    loop {
        // move to 0°
        let _ = channel_a.set_duty_cycle(us_to_duty(MID_PULSE));
        timer.delay_ms(movement_delay_ms);

        // 0° to 90°
        let _ = channel_a.set_duty_cycle(us_to_duty(MAX_PULSE));
        timer.delay_ms(movement_delay_ms);

        // 90° to 0°
        let _ = channel_a.set_duty_cycle(us_to_duty(MID_PULSE));
        timer.delay_ms(movement_delay_ms);

        // 0° to -90°
        let _ = channel_a.set_duty_cycle(us_to_duty(MIN_PULSE));
        timer.delay_ms(movement_delay_ms);
    }
}

/// Convert microseconds to duty value.
///
/// This function uses the constants TOP and PERIOD_US defined at the top of the file.
fn us_to_duty(us: u16) -> u16 {
    // Do math in u32 so we maintain higher precision. If we do math in u16, we need to divide first
    // and lose some precision when truncating the remainder.
    (TOP as u32 * us as u32 / PERIOD_US) as u16
}