use std::{thread, time::Duration};

use crate::DriveBase;
use ev3dev_lang_rust::{
    Ev3Error,
    sensors::{ColorSensor, Sensor},
};

#[expect(missing_docs, clippy::missing_errors_doc)]
pub trait Linefollower {
    const A1: f64;
    const A2: f64;
    const KP_MAX: f64;
    const KD_MAX: f64;
    const KORREKTUR_V: f64;
    const MAX_SPEED: f64;

    fn lf(
        &self,
        v_max_input: impl Into<f64>,
        v_end: impl Into<f64>,
        distance: impl Into<f64>,
    ) -> Result<&Self, Ev3Error>;
}

impl Linefollower for DriveBase {
    // Associate constants for acceleration and PD gains (as per original C code)
    const A1: f64 = 8.0; // acceleration (deg/s^2) during speeding up
    const A2: f64 = -7.0; // deceleration (deg/s^2, negative)
    const KP_MAX: f64 = 0.02; // max proportional gain
    const KD_MAX: f64 = 0.5; // max derivative gain
    const KORREKTUR_V: f64 = 1.0; // correction factor for differential drive
    const MAX_SPEED: f64 = 1560.;

    /// Follow a line: drive with PD control for given distance (in mm) at given speed.
    #[expect(
        clippy::while_float,
        clippy::unwrap_used,
        clippy::cast_possible_truncation
    )]
    fn lf(
        &self,
        v_max_input: impl Into<f64>,
        v_end: impl Into<f64>,
        distance: impl Into<f64>,
    ) -> Result<&Self, Ev3Error> {
        let v_max_input = v_max_input.into();
        let v_end = v_end.into();
        let distance = distance.into();
        let s = 360. / self.circumference * distance.abs();
        let mut v_max = ((2. * Self::A1 * Self::A2 * s).mul_add(
            1.,
            (f64::from(self.current_speed))
                .powi(2)
                .mul_add(Self::A2, -(v_end.powi(2) * Self::A1)),
        ) / (Self::A2 - Self::A1))
            .sqrt();
        v_max = v_max.clamp(-(v_max_input.abs()), v_max_input.abs());
        v_max = v_max.clamp(-Self::MAX_SPEED, Self::MAX_SPEED);

        let s1 = v_max.mul_add(v_max, -(f64::from(self.current_speed).powi(2))) / (2. * Self::A1);
        let s2 = v_end.mul_add(v_end, -(v_max.powi(2))) / (2. * Self::A2);

        let mut v = 0_f64;
        let (mut v_a, mut v_b) = (0_f64, 0_f64);
        let (mut k_p, mut k_d) = (0_f64, 0_f64);
        let (mut err, mut prev_err, mut diff) = (0_f64, 0_f64, 0_f64);
        let left_sensor = self.left_sensor.as_ref().unwrap();
        let right_sensor = self.right_sensor.as_ref().unwrap();

        self.reset()?;
        self.run_forever()?;

        while s1 > f64::from(self.left.get_position()?) {
            v = (f64::from(self.left.get_position()?)
                .mul_add(2. * Self::A1, f64::from(self.current_speed).powi(2)))
            .sqrt();
            k_p = Self::KP_MAX * v / Self::MAX_SPEED;
            k_d = Self::KD_MAX * v / Self::MAX_SPEED;

            err = f64::from(left_sensor.get_value0()?) - f64::from(right_sensor.get_value0()?);

            diff = err - prev_err;

            v_a = (-err).mul_add(k_p, (-diff).mul_add(k_d, v));
            v_b = err.mul_add(k_p, diff.mul_add(k_d, v));

            self.left.set_speed_sp(
                (-v_a)
                    .round()
                    .clamp(f64::from(i32::MIN), f64::from(i32::MAX)) as i32,
            )?;
            self.right.set_speed_sp(
                (v_b * Self::KORREKTUR_V)
                    .round()
                    .clamp(f64::from(i32::MIN), f64::from(i32::MAX)) as i32,
            )?;

            self.run_forever()?;

            prev_err = err;

            thread::sleep(Duration::from_millis(1));
        }

        Ok(self)
    }
}
