use crate::virtual_controller::*;
use crate::{DualsenseStatesBuffer, Tilt, TiltEstimator, state::DualsenseState};

/// A controller that represents all the available inputs of a Dualsense device
pub struct VirtualController<const N: usize> {
    states_buffer: DualsenseStatesBuffer<N>,
    tilt_estimator: TiltEstimator<N>,
    tilt: Tilt,
    tilt_enabled: bool,
}

impl<const N: usize> VirtualController<N> {
    /// Creates a new controller using the provided estimator for mapping roll/pitch
    pub fn new(tilt_estimator: TiltEstimator<N>) -> VirtualController<N> {
        VirtualController {
            states_buffer: DualsenseStatesBuffer::default(),
            tilt_estimator,
            tilt: Tilt::default(),
            tilt_enabled: true,
        }
    }

    /// Enables/disables tilt tracking
    pub fn toggle_tilt_enabled(&mut self) {
        self.tilt_enabled = !self.tilt_enabled
    }

    /// Updates the virtual controller state using the given Dualsense readings
    pub fn handle_dualsense(&mut self, ds_state: DualsenseState) -> VirtualControllerState {
        let state_event = self.states_buffer.push(ds_state);

        let throttle: u8 = (((ds_state.axes.rz.as_u8() as i16) - (ds_state.axes.z.as_u8() as i16)
            + (u8::MAX as i16))
            / 2) as u8;

        let pitch;
        let roll;
        if self.tilt_enabled {
            self.tilt = self
                .tilt_estimator
                .next_estimate(state_event)
                .accel_corrected_gyro;

            pitch = self.tilt.get_pitch_radians().into();
            roll = self.tilt.get_roll_radians().into();
        } else {
            pitch = AxisValue::default();
            roll = AxisValue::default();
        }

        VirtualControllerState {
            axes: Axes {
                x: ds_state.axes.x.into(),
                y: ds_state.axes.y.into(),
                z: ds_state.axes.z.into(),
                rx: ds_state.axes.rx.into(),
                ry: ds_state.axes.ry.into(),
                rz: ds_state.axes.rz.into(),
                throttle: throttle.into(),
                pitch,
                roll,
            },
            hat: ds_state.hat.into(),
            buttons: Buttons {
                square: ds_state.square,
                triangle: ds_state.triangle,
                circle: ds_state.circle,
                cross: ds_state.cross,
                l1: ds_state.l1,
                r1: ds_state.r1,
                l2: ds_state.l2,
                r2: ds_state.r2,
                l3: ds_state.l3,
                r3: ds_state.r3,
                option: ds_state.option,
                share: ds_state.share,
                touch_click: ds_state.touch_click,
                ps: ds_state.ps,
                mic: ds_state.mic,
            },
            is_tilt_enabled: self.tilt_enabled,
        }
    }
}
