use crate::config::PWMStep;
use log::info;
use rppal::pwm::Pwm;
use std::thread::sleep;
use std::time::Duration;
use thiserror::Error;

#[derive(Error, Debug)]
pub enum Error {
    #[error("PWM error")]
    PWMError(#[from] rppal::pwm::Error),
}

pub struct RollBot {
    pub pwm: Pwm,
    pub steps: Vec<PWMStep>,
}

impl RollBot {
    pub fn set_pwm_output(&self, pulse: Duration) -> Result<(), Error> {
        info!("Going to {:?}", pulse);
        self.pwm.set_pulse_width(pulse)?;

        Ok(())
    }

    pub fn roll(&self, tumble_times: u32) -> Result<(), Error> {
        for tumble in 1..=tumble_times {
            info!("Doing Tumble {}", tumble);
            for step in &self.steps {
                self.set_pwm_output(Duration::from_micros(step.pulse_us))?;
                sleep(Duration::from_millis(step.delay_ms));
            }
        }

        info!("Roll complete!");

        Ok(())
    }
}