//! Encoders and decoders for the FTMS **Fitness Machine Control Point**
//! (`0x2AD9`).
//!
//! The control point is a request/response characteristic: the central writes a
//! procedure (with response), and the fitness machine replies with an
//! **indication** of the form `[0x80, request_op_code, result_code, ...params]`.
//! Treating writes as fire-and-forget is a bug (FR-2.7) — a trainer that has
//! not granted control will silently ignore everything until `RequestControl`
//! succeeds, and there is no other way to find that out.
//!
//! The op-code table and the sint16 encodings for inclination, resistance and
//! target power were ported from `obostjancic/smart-trainer-control`
//! (`src/lib/bike/ftms-control.ts`), MIT licensed, Copyright (c) 2025 Ogi —
//! a working Van Rysel D100 client. See REQUIREMENTS.md §3.2.

/// FTMS Fitness Machine Control Point op codes.
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
#[repr(u8)]
pub enum OpCode {
    RequestControl = 0x00,
    Reset = 0x01,
    SetTargetSpeed = 0x02,
    SetTargetInclination = 0x03,
    SetTargetResistanceLevel = 0x04,
    SetTargetPower = 0x05,
    SetTargetHeartRate = 0x06,
    StartOrResume = 0x07,
    StopOrPause = 0x08,
    SetTargetedExpendedEnergy = 0x09,
    SetTargetedNumberOfSteps = 0x0A,
    SetTargetedNumberOfStrides = 0x0B,
    SetTargetedDistance = 0x0C,
    SetTargetedTrainingTime = 0x0D,
    SetTargetedTimeInTwoHeartRateZones = 0x0E,
    SetTargetedTimeInThreeHeartRateZones = 0x0F,
    SetTargetedTimeInFiveHeartRateZones = 0x10,
    SetIndoorBikeSimulationParameters = 0x11,
    SetWheelCircumference = 0x12,
    SetSpinDownControl = 0x13,
    SetTargetedCadence = 0x14,
}

impl OpCode {
    pub fn from_u8(v: u8) -> Option<Self> {
        use OpCode::*;
        Some(match v {
            0x00 => RequestControl,
            0x01 => Reset,
            0x02 => SetTargetSpeed,
            0x03 => SetTargetInclination,
            0x04 => SetTargetResistanceLevel,
            0x05 => SetTargetPower,
            0x06 => SetTargetHeartRate,
            0x07 => StartOrResume,
            0x08 => StopOrPause,
            0x09 => SetTargetedExpendedEnergy,
            0x0A => SetTargetedNumberOfSteps,
            0x0B => SetTargetedNumberOfStrides,
            0x0C => SetTargetedDistance,
            0x0D => SetTargetedTrainingTime,
            0x0E => SetTargetedTimeInTwoHeartRateZones,
            0x0F => SetTargetedTimeInThreeHeartRateZones,
            0x10 => SetTargetedTimeInFiveHeartRateZones,
            0x11 => SetIndoorBikeSimulationParameters,
            0x12 => SetWheelCircumference,
            0x13 => SetSpinDownControl,
            0x14 => SetTargetedCadence,
            _ => return None,
        })
    }

    pub fn as_u8(self) -> u8 {
        self as u8
    }
}

impl std::fmt::Display for OpCode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{self:?} (0x{:02x})", self.as_u8())
    }
}

/// The first byte of every control point indication.
pub const RESPONSE_CODE: u8 = 0x80;

/// Result codes carried in a control point indication.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ResultCode {
    Success,
    OpCodeNotSupported,
    InvalidParameter,
    OperationFailed,
    ControlNotPermitted,
    /// Reserved or vendor-specific.
    Unknown(u8),
}

impl ResultCode {
    pub fn from_u8(v: u8) -> Self {
        match v {
            0x01 => ResultCode::Success,
            0x02 => ResultCode::OpCodeNotSupported,
            0x03 => ResultCode::InvalidParameter,
            0x04 => ResultCode::OperationFailed,
            0x05 => ResultCode::ControlNotPermitted,
            other => ResultCode::Unknown(other),
        }
    }

    pub fn as_u8(self) -> u8 {
        match self {
            ResultCode::Success => 0x01,
            ResultCode::OpCodeNotSupported => 0x02,
            ResultCode::InvalidParameter => 0x03,
            ResultCode::OperationFailed => 0x04,
            ResultCode::ControlNotPermitted => 0x05,
            ResultCode::Unknown(v) => v,
        }
    }

    pub fn is_success(self) -> bool {
        matches!(self, ResultCode::Success)
    }
}

impl std::fmt::Display for ResultCode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            ResultCode::Success => write!(f, "Success"),
            ResultCode::OpCodeNotSupported => write!(f, "Op Code not supported"),
            ResultCode::InvalidParameter => write!(f, "Invalid parameter"),
            ResultCode::OperationFailed => write!(f, "Operation failed"),
            ResultCode::ControlNotPermitted => {
                write!(f, "Control not permitted (RequestControl first)")
            }
            ResultCode::Unknown(v) => write!(f, "Unknown result code 0x{v:02x}"),
        }
    }
}

/// A decoded control point indication.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct ControlPointResponse {
    /// The op code this response refers to, if it is one we know.
    pub request_op_code: Option<OpCode>,
    /// The raw op code byte, even when unrecognised.
    pub raw_request_op_code: u8,
    pub result: ResultCode,
    /// Response parameters, present only for some procedures.
    pub parameters: Vec<u8>,
}

impl ControlPointResponse {
    pub fn is_success(&self) -> bool {
        self.result.is_success()
    }
}

/// Why a control point indication could not be decoded.
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
pub enum ResponseError {
    #[error("control point indication is {len} bytes; at least 3 are required")]
    TooShort { len: usize },
    #[error("control point indication started with 0x{first:02x}, expected 0x80")]
    NotAResponse { first: u8 },
}

/// Decode a control point indication. Pure function; no hardware needed.
pub fn decode_response(data: &[u8]) -> Result<ControlPointResponse, ResponseError> {
    if data.len() < 3 {
        return Err(ResponseError::TooShort { len: data.len() });
    }
    if data[0] != RESPONSE_CODE {
        return Err(ResponseError::NotAResponse { first: data[0] });
    }
    Ok(ControlPointResponse {
        request_op_code: OpCode::from_u8(data[1]),
        raw_request_op_code: data[1],
        result: ResultCode::from_u8(data[2]),
        parameters: data[3..].to_vec(),
    })
}

// ---------------------------------------------------------------------------
// Request encoders
// ---------------------------------------------------------------------------

/// `StopOrPause` (`0x08`) takes a one-byte control parameter.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum StopOrPause {
    Stop = 0x01,
    Pause = 0x02,
}

/// Parameters for `SetIndoorBikeSimulationParameters` (`0x11`).
///
/// Whether the D100 accepts this at all is open question **A-1** — the MIT
/// reference drives grade with `SetTargetInclination` (`0x03`) instead. The
/// `probe set` subcommand exists to resolve it against real hardware.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SimulationParameters {
    /// Wind speed, m/s. Encoded as sint16 with 0.001 resolution.
    pub wind_speed_mps: f32,
    /// Grade, percent. Encoded as sint16 with 0.01 resolution.
    pub grade_percent: f32,
    /// Coefficient of rolling resistance. Encoded as uint8 with 0.0001
    /// resolution (so the representable range is 0.0000 – 0.0255).
    pub crr: f32,
    /// Wind resistance coefficient, kg/m. Encoded as uint8 with 0.01
    /// resolution (representable range 0.00 – 2.55).
    pub wind_resistance_coefficient: f32,
}

impl Default for SimulationParameters {
    fn default() -> Self {
        Self {
            wind_speed_mps: 0.0,
            grade_percent: 0.0,
            crr: 0.004,
            wind_resistance_coefficient: 0.51,
        }
    }
}

/// `[0x00]` — take control of the machine. Must succeed before any target
/// setting procedure is accepted (FR-2.1).
pub fn request_control() -> Vec<u8> {
    vec![OpCode::RequestControl.as_u8()]
}

/// `[0x01]` — reset the machine to its default state. Part of the shutdown
/// safety sequence (SAF-2).
pub fn reset() -> Vec<u8> {
    vec![OpCode::Reset.as_u8()]
}

/// `[0x07]` — start or resume.
pub fn start_or_resume() -> Vec<u8> {
    vec![OpCode::StartOrResume.as_u8()]
}

/// `[0x08, param]` — stop or pause.
pub fn stop_or_pause(what: StopOrPause) -> Vec<u8> {
    vec![OpCode::StopOrPause.as_u8(), what as u8]
}

/// `[0x03, sint16 LE]` — target inclination in percent, 0.1 resolution.
///
/// Values outside the sint16 range after scaling are saturated rather than
/// wrapped: wrapping a +400% mistake into a large negative grade would be a
/// safety hazard.
pub fn set_target_inclination(percent: f32) -> Vec<u8> {
    let raw = scale_to_i16(percent, 10.0);
    let mut v = vec![OpCode::SetTargetInclination.as_u8()];
    v.extend_from_slice(&raw.to_le_bytes());
    v
}

/// `[0x04, sint16 LE]` — target resistance level, in the trainer's own units.
///
/// Note: the FTMS specification defines this parameter as a *uint8* with 0.1
/// resolution, but the working D100 reference implementation (§3.2) sends a
/// sint16 and the trainer accepts it. We follow the reference, since it is the
/// only behaviour confirmed on this hardware. **Needs hardware confirmation**
/// if a different trainer is ever targeted.
pub fn set_target_resistance(level: i16) -> Vec<u8> {
    let mut v = vec![OpCode::SetTargetResistanceLevel.as_u8()];
    v.extend_from_slice(&level.to_le_bytes());
    v
}

/// `[0x05, sint16 LE]` — target power in watts.
pub fn set_target_power(watts: i16) -> Vec<u8> {
    let mut v = vec![OpCode::SetTargetPower.as_u8()];
    v.extend_from_slice(&watts.to_le_bytes());
    v
}

/// `[0x11, sint16 wind, sint16 grade, uint8 crr, uint8 cw]` — indoor bike
/// simulation parameters. See [`SimulationParameters`] and open question A-1.
pub fn set_simulation_parameters(p: SimulationParameters) -> Vec<u8> {
    let wind = scale_to_i16(p.wind_speed_mps, 1000.0);
    let grade = scale_to_i16(p.grade_percent, 100.0);
    let crr = scale_to_u8(p.crr, 10_000.0);
    let cw = scale_to_u8(p.wind_resistance_coefficient, 100.0);

    let mut v = vec![OpCode::SetIndoorBikeSimulationParameters.as_u8()];
    v.extend_from_slice(&wind.to_le_bytes());
    v.extend_from_slice(&grade.to_le_bytes());
    v.push(crr);
    v.push(cw);
    v
}

/// Scale a physical value by `factor` and saturate into sint16.
fn scale_to_i16(value: f32, factor: f32) -> i16 {
    if !value.is_finite() {
        return 0;
    }
    let scaled = (value * factor).round();
    scaled.clamp(i16::MIN as f32, i16::MAX as f32) as i16
}

/// Scale a physical value by `factor` and saturate into uint8.
fn scale_to_u8(value: f32, factor: f32) -> u8 {
    if !value.is_finite() {
        return 0;
    }
    let scaled = (value * factor).round();
    scaled.clamp(0.0, u8::MAX as f32) as u8
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn simple_procedures_are_single_bytes() {
        assert_eq!(request_control(), vec![0x00]);
        assert_eq!(reset(), vec![0x01]);
        assert_eq!(start_or_resume(), vec![0x07]);
        assert_eq!(stop_or_pause(StopOrPause::Stop), vec![0x08, 0x01]);
        assert_eq!(stop_or_pause(StopOrPause::Pause), vec![0x08, 0x02]);
    }

    #[test]
    fn inclination_is_sint16_tenths_of_a_percent_little_endian() {
        // +5.0% -> 50 -> 0x0032
        assert_eq!(set_target_inclination(5.0), vec![0x03, 0x32, 0x00]);
        // 0% -> 0
        assert_eq!(set_target_inclination(0.0), vec![0x03, 0x00, 0x00]);
        // -7.5% -> -75 -> 0xFFB5
        assert_eq!(set_target_inclination(-7.5), vec![0x03, 0xb5, 0xff]);
        // Rounding, not truncation.
        assert_eq!(set_target_inclination(1.26), vec![0x03, 0x0d, 0x00]);
    }

    #[test]
    fn inclination_saturates_instead_of_wrapping() {
        // 5000% * 10 = 50000, past i16::MAX. Must clamp to +32767, not wrap to
        // a large negative grade.
        assert_eq!(set_target_inclination(5000.0), vec![0x03, 0xff, 0x7f]);
        assert_eq!(set_target_inclination(-5000.0), vec![0x03, 0x00, 0x80]);
        assert_eq!(set_target_inclination(f32::NAN), vec![0x03, 0x00, 0x00]);
        assert_eq!(set_target_inclination(f32::INFINITY), vec![0x03, 0x00, 0x00]);
    }

    #[test]
    fn resistance_is_sint16_little_endian() {
        assert_eq!(set_target_resistance(0), vec![0x04, 0x00, 0x00]);
        assert_eq!(set_target_resistance(50), vec![0x04, 0x32, 0x00]);
        assert_eq!(set_target_resistance(100), vec![0x04, 0x64, 0x00]);
        assert_eq!(set_target_resistance(-1), vec![0x04, 0xff, 0xff]);
    }

    #[test]
    fn power_is_sint16_watts_little_endian() {
        assert_eq!(set_target_power(200), vec![0x05, 0xc8, 0x00]);
        assert_eq!(set_target_power(600), vec![0x05, 0x58, 0x02]);
        // Negative target power is nonsense but must still encode as sint16
        // rather than panic; the safety gate is what prevents it being sent.
        assert_eq!(set_target_power(-10), vec![0x05, 0xf6, 0xff]);
    }

    #[test]
    fn simulation_parameters_layout() {
        let p = SimulationParameters {
            wind_speed_mps: 0.0,
            grade_percent: 4.5,   // * 100 = 450 = 0x01C2
            crr: 0.004,           // * 10000 = 40 = 0x28
            wind_resistance_coefficient: 0.51, // * 100 = 51 = 0x33
        };
        assert_eq!(
            set_simulation_parameters(p),
            vec![0x11, 0x00, 0x00, 0xc2, 0x01, 0x28, 0x33]
        );
        assert_eq!(set_simulation_parameters(p).len(), 7);
    }

    #[test]
    fn simulation_parameters_negative_grade_and_wind() {
        let p = SimulationParameters {
            wind_speed_mps: -1.5, // * 1000 = -1500 = 0xFA24
            grade_percent: -8.0,  // * 100 = -800 = 0xFCE0
            crr: 0.0,
            wind_resistance_coefficient: 0.0,
        };
        assert_eq!(
            set_simulation_parameters(p),
            vec![0x11, 0x24, 0xfa, 0xe0, 0xfc, 0x00, 0x00]
        );
    }

    #[test]
    fn simulation_parameter_bytes_saturate() {
        let p = SimulationParameters {
            wind_speed_mps: 0.0,
            grade_percent: 0.0,
            crr: 1.0,                          // * 10000 = 10000, way past u8
            wind_resistance_coefficient: -5.0, // negative, clamps to 0
        };
        let b = set_simulation_parameters(p);
        assert_eq!(b[5], 0xff);
        assert_eq!(b[6], 0x00);
    }

    #[test]
    fn response_decoding() {
        let r = decode_response(&[0x80, 0x05, 0x01]).unwrap();
        assert_eq!(r.request_op_code, Some(OpCode::SetTargetPower));
        assert_eq!(r.result, ResultCode::Success);
        assert!(r.is_success());
        assert!(r.parameters.is_empty());
    }

    #[test]
    fn response_decoding_reports_every_error_code() {
        for (byte, expect) in [
            (0x02u8, ResultCode::OpCodeNotSupported),
            (0x03, ResultCode::InvalidParameter),
            (0x04, ResultCode::OperationFailed),
            (0x05, ResultCode::ControlNotPermitted),
            (0x77, ResultCode::Unknown(0x77)),
        ] {
            let r = decode_response(&[0x80, 0x11, byte]).unwrap();
            assert_eq!(r.result, expect);
            assert!(!r.is_success());
        }
    }

    /// A-1: this is exactly the byte sequence that tells us the D100 rejects
    /// simulation mode.
    #[test]
    fn sim_mode_rejection_is_recognisable() {
        let r = decode_response(&[0x80, 0x11, 0x02]).unwrap();
        assert_eq!(
            r.request_op_code,
            Some(OpCode::SetIndoorBikeSimulationParameters)
        );
        assert_eq!(r.result, ResultCode::OpCodeNotSupported);
    }

    #[test]
    fn response_with_parameters() {
        let r = decode_response(&[0x80, 0x13, 0x01, 0xaa, 0xbb]).unwrap();
        assert_eq!(r.parameters, vec![0xaa, 0xbb]);
    }

    #[test]
    fn unknown_op_code_in_response_is_preserved_not_dropped() {
        let r = decode_response(&[0x80, 0xfe, 0x02]).unwrap();
        assert_eq!(r.request_op_code, None);
        assert_eq!(r.raw_request_op_code, 0xfe);
    }

    #[test]
    fn malformed_responses_are_errors() {
        assert_eq!(
            decode_response(&[]).unwrap_err(),
            ResponseError::TooShort { len: 0 }
        );
        assert_eq!(
            decode_response(&[0x80, 0x05]).unwrap_err(),
            ResponseError::TooShort { len: 2 }
        );
        assert_eq!(
            decode_response(&[0x01, 0x05, 0x01]).unwrap_err(),
            ResponseError::NotAResponse { first: 0x01 }
        );
    }

    #[test]
    fn op_code_round_trips() {
        for v in 0x00u8..=0x14 {
            let op = OpCode::from_u8(v).expect("all of 0x00..=0x14 are defined");
            assert_eq!(op.as_u8(), v);
        }
        assert_eq!(OpCode::from_u8(0x15), None);
        assert_eq!(OpCode::from_u8(0xff), None);
    }

    #[test]
    fn result_code_round_trips() {
        for v in 0x00u8..=0xff {
            assert_eq!(ResultCode::from_u8(v).as_u8(), v);
        }
    }
}
