use std::sync::{Arc, Mutex};

use serialport::SerialPort;

mod lidar;

// TODO: Should be returning results in these traits
pub trait Servo {
    fn get_value(&self) -> f64;

    fn set_value(&mut self, value: f64);

    fn min(&mut self) {
        self.set_value(-1.);
    }
    fn mid(&mut self) {
        self.set_value(0.);
    }
    fn max(&mut self) {
        self.set_value(1.);
    }
}

pub trait Vehicle {
    fn get_throttle(&self) -> f64;
    fn set_throttle(&mut self, throttle: f64);
    fn get_steering(&self) -> f64;
    fn set_steering(&mut self, steering: f64);
}
#[cfg(feature = "rppal")]
pub mod rppal {
    use rppal::pwm::{Channel, Pwm};

    pub struct RpiPwmServo {
        pwm: Pwm,
        min_duty_cycle: f64,
        duty_cycle_range: f64,
        value: f64,
        frame_width: f64,
    }

    impl RpiPwmServo {
        pub fn new(pwm: Pwm) -> RpiPwmServo {
            RpiPwmServo::new(pwm, 1000000)
        }

        pub fn new(pwm: Pwm, min_pulse_width: f64) -> RpiPwmServo {}

        pub fn new(
            pwm: Pwm,
            min_pulse_width: f64,
            max_pulse_width: f64,
            frame_width: f64,
        ) -> RpiPwmServo {
            RpiPwmServo {
                pwm,
                min_duty_cycle: min_pulse_width / frame_width,
                duty_cycle_range: (max_pulse_width - min_pulse_width) / frame_width,
                frame_width,
            }
        }
    }

    impl Default for RpiPwmServo {
        fn default() -> Self {
            Self {
                pwm: Pwm::new(Channel::Pwm0).expect("Failed to initialise Pwm servo"),
            }
        }
    }

    impl Servo for RpiPwmServo {
        fn get_duty_cycle(&self) -> f64 {
            self.pwm.duty_cycle().unwrap_or(0.)
        }

        fn set_duty_cycle(&self, pwm: f64) {
            self.pwm
                .set_duty_cycle(pwm)
                .expect("Failed to write duty cycle");
        }

        fn get_frequency(&self) -> f64 {
            self.pwm.duty_cycle().unwrap_or(0.0)
        }

        fn set_frequency(&self, frequency: f64) {
            self.pwm
                .set_frequency(frequency, self.get_duty_cycle())
                .expect("Failed to set Frequency")
        }
    }
}

pub struct Esp32SerialPwmServo<T: SerialPort> {
    serial_port: T,
    value: f64,
    channel: u8,
    pin: u8,
}

impl<T: SerialPort> Esp32SerialPwmServo<T> {
    pub fn new(serial_port: T, channel: u8, pin: u8) -> Esp32SerialPwmServo<T> {
        let mut servo = Esp32SerialPwmServo {
            serial_port,
            value: 0.,
            channel,
            pin,
        };
        servo.init_pwm();
        servo
    }
}

impl<T: SerialPort> Esp32SerialPwmServo<T> {
    fn init_pwm(&mut self) {
        let bytes_written = self.serial_port.write(&[0, 1, self.channel, self.pin]);
        // TODO: Better error handling (even anyhow would be better)
        match bytes_written {
            Ok(size) => println!("{}", size),
            Err(err) => eprintln!("{}", err),
        }
    }
}

impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
    fn get_value(&self) -> f64 {
        self.value
    }

    fn set_value(&mut self, value: f64) {
        let mut temp_value = value;
        if temp_value < -1. {
            temp_value = -1.;
        } else if temp_value > 1. {
            temp_value = 1.;
        }
        self.value = temp_value;
        let bytes_written = self
            .serial_port
            .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]);
        // TODO: Better error handling
        match bytes_written {
            Ok(size) => println!("{}", size),
            Err(err) => eprintln!("{}", err),
        }
    }
}

pub struct ServoVehicle<T: Servo> {
    steering_servo: T,
    throttle_servo: T,
}

impl<T: Servo> ServoVehicle<T> {
    pub fn new(steering_servo: T, throttle_servo: T) -> ServoVehicle<T> {
        ServoVehicle {
            steering_servo,
            throttle_servo,
        }
    }
}

impl<T: Servo> Vehicle for ServoVehicle<T> {
    fn get_throttle(&self) -> f64 {
        self.throttle_servo.get_value()
    }

    fn set_throttle(&mut self, throttle: f64) {
        self.throttle_servo.set_value(throttle);
    }

    fn get_steering(&self) -> f64 {
        self.steering_servo.get_value()
    }

    fn set_steering(&mut self, steering: f64) {
        self.steering_servo.set_value(steering);
    }
}

#[derive(Default, Debug)]
pub struct PrintVehicle {
    throttle: f64,
    steering: f64,
}

impl Vehicle for PrintVehicle {
    fn get_throttle(&self) -> f64 {
        self.throttle
    }

    fn set_throttle(&mut self, throttle: f64) {
        println!("New Throttle: {}", throttle);
        self.throttle = throttle;
    }

    fn get_steering(&self) -> f64 {
        self.steering
    }

    fn set_steering(&mut self, steering: f64) {
        println!("New Steering: {}", steering);
        self.steering = steering;
    }
}