use crate::nmea::{self, Gga, GgaQuality, NmeaTalker, NmeaTime};
use crate::{Error, Result, Wgs84Geodetic};

#[derive(Clone, Debug, PartialEq)]
pub struct GgaPosition {
    pub lat_deg: f64,
    pub lon_deg: f64,
    pub height_m: f64,
    pub fix_quality: u8,
    pub num_satellites: u8,
    pub hdop: f64,
}

impl Default for GgaPosition {
    fn default() -> Self {
        Self {
            lat_deg: 0.0,
            lon_deg: 0.0,
            height_m: 0.0,
            fix_quality: 1,
            num_satellites: 10,
            hdop: 1.0,
        }
    }
}

pub fn format_gga(position: &GgaPosition, utc_seconds_of_day: f64) -> Result<Vec<u8>> {
    validate(position, utc_seconds_of_day)?;
    let geodetic = Wgs84Geodetic::new(
        position.lat_deg.to_radians(),
        position.lon_deg.to_radians(),
        position.height_m,
    )
    .map_err(|err| Error::InvalidInput(err.to_string()))?;
    let mut gga = Gga::vrs_position(
        geodetic,
        format_time(utc_seconds_of_day)?,
        quality(position.fix_quality),
        position.num_satellites,
        position.hdop,
        7,
    )
    .map_err(|err| Error::InvalidInput(err.to_string()))?;
    gga.geoid_separation_m = None;
    nmea::write_gga(NmeaTalker::System(crate::GnssSystem::Gps), &gga)
        .map(|sentence| sentence.into_bytes())
        .map_err(|err| Error::InvalidInput(err.to_string()))
}

fn validate(position: &GgaPosition, utc_seconds_of_day: f64) -> Result<()> {
    if !position.lat_deg.is_finite()
        || !position.lon_deg.is_finite()
        || !position.height_m.is_finite()
        || !position.hdop.is_finite()
        || !utc_seconds_of_day.is_finite()
    {
        return Err(Error::InvalidInput("GGA inputs must be finite".into()));
    }
    if !(-90.0..=90.0).contains(&position.lat_deg) {
        return Err(Error::InvalidInput("GGA latitude outside [-90, 90]".into()));
    }
    if !(-180.0..=180.0).contains(&position.lon_deg) {
        return Err(Error::InvalidInput(
            "GGA longitude outside [-180, 180]".into(),
        ));
    }
    if position.hdop < 0.0 {
        return Err(Error::InvalidInput("GGA HDOP must be non-negative".into()));
    }
    if !(0.0..86400.0).contains(&utc_seconds_of_day) {
        return Err(Error::InvalidInput("GGA time must be in [0, 86400)".into()));
    }
    Ok(())
}

fn format_time(seconds: f64) -> Result<NmeaTime> {
    NmeaTime::from_seconds_of_day_floor_centis(seconds)
        .map_err(|err| Error::InvalidInput(err.to_string()))
}

fn quality(value: u8) -> GgaQuality {
    match value {
        0 => GgaQuality::Invalid,
        1 => GgaQuality::GpsSps,
        2 => GgaQuality::Differential,
        3 => GgaQuality::Pps,
        4 => GgaQuality::RtkFixed,
        5 => GgaQuality::RtkFloat,
        6 => GgaQuality::Estimated,
        7 => GgaQuality::Manual,
        8 => GgaQuality::Simulator,
        other => GgaQuality::Other(other),
    }
}
