//! Application Logic

#[cfg(feature = "spat")]
use alloc::rc::Rc;
#[cfg(feature = "spat")]
use alloc::vec::Vec;

#[cfg(feature = "spat")]
use log::{debug, error};
#[cfg(feature = "gnss")]
use log::{info, warn};

#[cfg(feature = "spat")]
use crate::{applogic::v2x::map, geo_alg};

pub mod v2x;

#[cfg(feature = "cam")]
pub mod cam;
#[cfg(feature = "denm")]
pub mod denm;
#[cfg(feature = "spat")]
pub mod tlc;

#[cfg(feature = "spat")]
const TLC_CACHE_LIFETIME: chrono::Duration = chrono::Duration::minutes(1);
#[cfg(feature = "spat")]
const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(5);

#[cfg(feature = "spat")]
const GLOSA_SEARCH_DIST_M: f32 = 400.;
#[cfg(feature = "spat")]
const GLOSA_SEARCH_ANGLE_PARAMS: [(f32, f32); 2] = [(400., 45.), (50., 180.)]; // far and near (distance, angle) tuple
#[cfg(feature = "spat")]
const GLOSA_LANE_HEADING_DIFF_DEG: f32 = 35.;
#[cfg(feature = "spat")]
const GLOSA_AT_LANE_OFFSET_M: f64 = 10.;
#[cfg(feature = "spat")]
const GLOSA_LANE_LEFT_OFFSET_M: f64 = 20.;
#[cfg(feature = "spat")]
const GLOSA_STOP_LINE_PASSED_M: f64 = 5.;

#[allow(unused)]
pub struct State {
    initialized: bool,
    time: chrono::NaiveDateTime,

    #[allow(unused)]
    position: geo_types::Point,
    #[allow(unused)]
    heading_deg: Option<f32>,
    #[allow(unused)]
    speed_mps: Option<f32>,

    #[cfg(feature = "spat")]
    last_prune: chrono::NaiveDateTime,
    #[cfg(feature = "spat")]
    tlc_cache: tlc::Data,

    // GLOSA
    #[cfg(feature = "spat")]
    #[allow(clippy::struct_field_names)]
    glosa_state: GlosaState,
}

#[cfg(feature = "spat")]
#[derive(Debug, Default)]
pub struct SignalGroupData {
    pub phase: SignalPhase,
    pub maneuver: map::Maneuvers,
    pub end_sec: Option<i16>,
}

#[cfg(feature = "spat")]
#[derive(Debug, Default)]
pub enum SignalPhase {
    #[default]
    Unknown,
    Red,
    RedYellow,
    Green,
    Yellow,
}

#[cfg(feature = "spat")]
impl core::fmt::Display for SignalPhase {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            SignalPhase::Unknown => write!(f, "__"),
            SignalPhase::Red => write!(f, "RE"),
            SignalPhase::RedYellow => write!(f, "RY"),
            SignalPhase::Green => write!(f, "GN"),
            SignalPhase::Yellow => write!(f, "YE"),
        }
    }
}

#[cfg(feature = "spat")]
impl From<c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState> for SignalPhase {
    fn from(value: c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState) -> Self {
        use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState;

        match value {
            MovementPhaseState::stop_And_Remain => SignalPhase::Red,
            MovementPhaseState::pre_Movement => SignalPhase::RedYellow,
            MovementPhaseState::permissive_Movement_Allowed
            | MovementPhaseState::protected_Movement_Allowed => SignalPhase::Green,
            MovementPhaseState::permissive_clearance | MovementPhaseState::protected_clearance => {
                SignalPhase::Yellow
            }
            _ => SignalPhase::Unknown,
        }
    }
}

#[cfg(feature = "spat")]
#[derive(Debug, Default)]
enum GlosaState {
    #[default]
    Idle,
    Locked(TlData),
}

/// Where we are and pre-processed information about the current intersection
#[cfg(feature = "spat")]
#[derive(Debug)]
struct TlData {
    intersection_id: u16, // mostly for debug printing
    intersection: Rc<core::cell::RefCell<v2x::map::Intersection>>,

    lane_id: u8,
    approach_id: Option<u8>,

    signal_groups: Vec<u8>,
}

impl State {
    pub fn new() -> Self {
        Self {
            initialized: false,
            time: chrono::NaiveDateTime::default(),
            position: geo_types::Point::default(),
            heading_deg: None,
            speed_mps: None,
            #[cfg(feature = "spat")]
            last_prune: chrono::NaiveDateTime::default(),
            #[cfg(feature = "spat")]
            tlc_cache: tlc::Data::new(TLC_CACHE_LIFETIME),
            #[cfg(feature = "spat")]
            glosa_state: GlosaState::default(),
        }
    }

    #[cfg(all(feature = "esp", feature = "gnss"))]
    pub fn update_with_gpsfix(&mut self, fix: &embassy_gps::types::GpsFix) {
        self.position = geo_types::Point::new(fix.longitude_deg, fix.latitude_deg);

        if let Some(heading) = fix.true_course_deg {
            self.heading_deg = Some(heading);
        } else {
            warn!("State: Course missing from GpsFix");
        }

        if let Some(speed) = fix.speed_over_ground {
            self.speed_mps = Some(speed);
        } else {
            warn!("State: Speed missing from GpsFix");
        }

        if let Some(time) = fix.get_timestamp() {
            self.time = time;

            if !self.initialized {
                info!("GNSS fix: {}", self.print_fix());
            }
            self.initialized = true;
        }
    }

    #[cfg(feature = "gnss")]
    pub fn print_fix(&self) -> alloc::string::String {
        alloc::format!(
            "{:?}, {:.4} N, {:.4} E, {:.0}°, {:.2} m/s",
            self.time,
            self.position.y(),
            self.position.x(),
            self.heading_deg.unwrap_or_default(),
            self.speed_mps.unwrap_or_default(),
        )
    }

    pub fn prune(&mut self) {
        #[cfg(feature = "spat")]
        if self.last_prune + PRUNE_INTERVAL < self.time {
            self.tlc_cache.prune(self.time);

            info!("Known int.: {:?}", self.tlc_cache.known_intersections());

            self.last_prune = self.time;
        }
    }

    #[cfg(feature = "spat")]
    pub fn handle_mapem(&mut self, etsi: &tlc::MAPEM) {
        self.tlc_cache.handle_mapem(self.time, etsi);
    }

    #[cfg(feature = "spat")]
    pub fn handle_spatem(&mut self, etsi: &tlc::SPATEM) {
        // only update "our" intersection
        if let GlosaState::Locked(data) = &self.glosa_state
            && etsi
                .spat
                .intersections
                .0
                .iter()
                .find(|i| i.id.id.0 == data.intersection_id)
                .is_some()
        {
            self.tlc_cache.handle_spatem(self.time, etsi);
        }
    }

    #[cfg(feature = "spat")]
    pub fn run_glosa(&mut self) -> Vec<SignalGroupData> {
        let Some(own_heading) = self.heading_deg else {
            return alloc::vec![];
        };

        let (state_update, sig_grps) = match &self.glosa_state {
            // search for upcoming intersection
            GlosaState::Idle => {
                debug!(
                    "GLOSA: Searching with {} known intersections",
                    self.tlc_cache.known_intersections().len()
                );

                if let Some(search_result) =
                    Self::find_intersection(&self.position, own_heading, &self.tlc_cache)
                {
                    info!(
                        "GLOSA: Approach has signal groups: {:?}",
                        search_result.signal_groups
                    );

                    (Some(GlosaState::Locked(search_result)), None)
                } else {
                    (None, None)
                }
            }
            GlosaState::Locked(info) => {
                debug!(
                    "GLOSA: Re-evaluating intersection {:4} (at lane {:3})",
                    info.intersection_id, info.lane_id
                );

                // determine if intersection is left
                let intersection = info.intersection.borrow();
                if Self::is_intersection_left(
                    &intersection,
                    info.lane_id,
                    info.approach_id,
                    &self.position,
                ) {
                    info!("GLOSA: Intersection was left -> back to idle");
                    (Some(GlosaState::Idle), None)
                } else {
                    // get signal groups which are in our approach
                    let mut approach_sig_grps = intersection
                        .signal_groups()
                        .iter()
                        .filter(|i| {
                            info.signal_groups
                                .iter()
                                .find(|sg_id| **sg_id == i.id())
                                .is_some()
                        })
                        .collect::<Vec<_>>();

                    // sort signal groups by maneuver
                    approach_sig_grps.sort_by_key(|v| v.maneuvers());

                    // display signal phases
                    let sig_grps = approach_sig_grps
                        .iter()
                        .filter_map(|sig| {
                            if let Some(manv) = sig.maneuvers() {
                                use core::ops::Sub;

                                use num_traits::Float;

                                let phase = sig
                                    .phase()
                                    .map(core::convert::Into::into)
                                    .unwrap_or_default();

                                let end_sec =
                                    sig.likely_time()
                                        .or_else(|| sig.min_end_time())
                                        .map(|time| {
                                            let dur =
                                                time.sub(self.time.and_utc()).as_seconds_f32();

                                            #[allow(clippy::cast_possible_truncation)]
                                            let end_sec = dur.round() as i16;

                                            end_sec
                                        });

                                Some(SignalGroupData {
                                    phase,
                                    maneuver: manv,
                                    end_sec,
                                })
                            } else {
                                None
                            }
                        })
                        .collect::<Vec<_>>();

                    (None, Some(sig_grps)) // keep state, return signal groups
                }
            }
        };

        // can't update the state from within the match statement, so do it here
        if let Some(new) = state_update {
            self.glosa_state = new;
        }

        sig_grps.unwrap_or_default()
    }

    #[cfg(feature = "spat")]
    #[allow(clippy::too_many_lines)]
    fn find_intersection(
        own_pos: &geo_types::Point,
        own_heading: f32,
        tlc_cache: &tlc::Data,
    ) -> Option<TlData> {
        // filter known intersections by distance (and bearing)
        let upcoming_intersections = tlc_cache.intersections_matching(|i| {
            let int_pos = i.center();

            #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
            let distance = geo_alg::haversine_dist(own_pos, &int_pos) as f32;
            let bearing = geo_alg::haversine_bearing(own_pos, &int_pos);
            let bearing_diff = geo_alg::deg_180(own_heading - bearing);

            distance < GLOSA_SEARCH_DIST_M && bearing_diff.abs() < Self::get_search_angle(distance)
        });

        info!(
            "GLOSA: upcoming int. count: {}",
            upcoming_intersections.len()
        );

        // determine relevant approach
        for int in upcoming_intersections {
            let intersection = int.borrow();
            let lanes = intersection.ingress_lanes();

            let own_pos_cart = geo_alg::lonlat_to_dcart(&intersection.ref_point(), own_pos);

            // find lanes with matching heading
            let relevant_lanes = lanes
                .iter()
                .filter(|i| {
                    let lane_heading = i.get_heading();
                    let heading_diff = geo_alg::deg_180(own_heading - lane_heading);

                    debug!(
                        " - Lane {:4}/{:03} has heading {lane_heading:4.0} -> diff {heading_diff:4.0}°",
                        intersection.id(),
                        i.id()
                    );

                    heading_diff.abs() < GLOSA_LANE_HEADING_DIFF_DEG
                })
                .collect::<Vec<_>>();

            info!(
                "GLOSA: Int {:4} has {} relevant lanes",
                intersection.id(),
                relevant_lanes.len()
            );
            for lane in &relevant_lanes {
                info!(
                    " - lane {:03} from appr. {:?}, usages {:?}",
                    lane.id(),
                    lane.approach_id(),
                    lane.usages()
                );
            }

            // determine in which lane/ approach we are
            let current_lane = relevant_lanes.iter().find(|lane| {
                #[cfg(feature = "spat_debug")]
                info!("GLOSA: Searching for current lane");
                match geo_alg::LinePosition::for_ingress_lane(own_pos_cart, lane.nodes()) {
                    Ok(pos) => {
                        #[cfg(feature = "spat_debug")]
                        info!(
                            " - Lane {:4}/{:03} {:?} with {:.0} m offset",
                            intersection.id(),
                            lane.id(),
                            pos.semantic,
                            pos.perpendicular_dist,
                        );

                        if pos.perpendicular_dist < GLOSA_AT_LANE_OFFSET_M
                            && pos.semantic == geo_alg::LinePositionType::InBetween
                        {
                            info!(
                                "GLOSA: At lane {:4}/{:03} -> locking intersection",
                                intersection.id(),
                                lane.id()
                            );

                            true
                        } else {
                            false
                        }
                    }
                    Err(err) => {
                        warn!("Lane {:04}: Failed to determine position: {err}", lane.id());
                        false
                    }
                }
            });

            if let Some(lane) = current_lane {
                // we found some lane which we are "inside" -> lane search can be aborted here
                // but we should extract some information which is needed for future processing first

                // collect all signal groups from the current approach
                let mut signal_groups = if let Some(own_approach) = lane.approach_id() {
                    let approach_lanes = relevant_lanes.iter().filter(|lane| {
                        if let Some(approach) = lane.approach_id() {
                            approach == own_approach
                        } else {
                            false
                        }
                    });

                    approach_lanes
                        .flat_map(|lane| {
                            lane.connections()
                                .iter()
                                .filter_map(v2x::map::Connection::signal_group)
                        })
                        .collect::<Vec<_>>()
                } else {
                    error!(
                        "GLOSA: Lane has no approach ID, can't determine all possibly relevant signal groups"
                    );
                    // just fall back to only the current lane's signal groups
                    lane.connections()
                        .iter()
                        .filter_map(v2x::map::Connection::signal_group)
                        .collect::<Vec<_>>()
                };

                signal_groups.sort_unstable();
                signal_groups.dedup();

                return Some(TlData {
                    intersection_id: intersection.id(),
                    intersection: int.clone(),
                    lane_id: lane.id(),
                    approach_id: lane.approach_id(),
                    signal_groups,
                });
            }
        }

        None
    }

    #[cfg(feature = "spat")]
    fn get_search_angle(distance: f32) -> f32 {
        let (dist_far, angle_far) = GLOSA_SEARCH_ANGLE_PARAMS[0];
        let (dist_near, angle_near) = GLOSA_SEARCH_ANGLE_PARAMS[1];

        if distance > dist_far {
            angle_far
        } else if distance < dist_near {
            angle_near
        } else {
            // interpolate angle from near to far
            let diff_dist = dist_far - dist_near;
            let diff_angle = angle_far - angle_near;

            angle_near + (diff_angle / diff_dist) * (distance - dist_near)
        }
    }

    /// Determines if intersection was left
    ///
    /// TODO: return if left via stop line or otherwise
    #[cfg(feature = "spat")]
    fn is_intersection_left(
        intersection: &v2x::map::Intersection,
        lane_id: u8,
        approach_id: Option<u8>,
        own_pos: &geo_types::Point,
    ) -> bool {
        // get all lanes from the current approach
        let lanes = if let Some(approach) = approach_id {
            intersection.approach_lanes(approach)
        } else if let Some(lane) = intersection.lane(lane_id) {
            alloc::vec![lane]
        } else {
            alloc::vec![]
        };

        // find minimal distance to any lane where we are "inside"
        let own_pos_cart = geo_alg::lonlat_to_dcart(&intersection.ref_point(), own_pos);
        let min_perp_dist = lanes.iter().fold(f64::MAX, |min_dist, i| {
            match geo_alg::LinePosition::for_ingress_lane(own_pos_cart, i.nodes()) {
                Ok(pos) => {
                    // check if we left the lane via the stop line, but only count stop line as passed after X meters
                    if pos.semantic == geo_alg::LinePositionType::InBetween
                        || (pos.semantic == geo_alg::LinePositionType::BeforeStart
                            && pos.parallel_dist.abs() < GLOSA_STOP_LINE_PASSED_M)
                    {
                        // still at the lane -> update min_dist
                        pos.perpendicular_dist
                    } else {
                        // not at the lane -> take old min_dist
                        min_dist
                    }
                }
                Err(err) => {
                    warn!("Lane {:04}: Failed to determine position: {err}", i.id());
                    min_dist
                }
            }
        });

        min_perp_dist > GLOSA_LANE_LEFT_OFFSET_M
    }

    // -- getters --

    #[allow(unused)]
    pub fn initialized(&self) -> bool {
        self.initialized
    }

    #[allow(unused)]
    pub fn time(&self) -> chrono::prelude::NaiveDateTime {
        self.time
    }
}

#[cfg(test)]
mod tests {

    use super::*;
    use crate::init_test_env_logger;
    use crate::testdata::{MAPEM_HH_560, MAPEM_HH_1328};

    #[cfg(feature = "spat")]
    #[test]
    fn glosa() {
        init_test_env_logger();

        // prime TLC cache
        let mut state = State::new();
        let time_now = chrono::NaiveDateTime::default();

        let etsi = if let c_its_parser::ItsMessage::Mapem {
            geonetworking: _,
            transport: _,
            etsi,
        } =
            c_its_parser::de::decode(MAPEM_HH_1328, c_its_parser::Headers::None).unwrap()
        {
            etsi
        } else {
            panic!("Unexpected C-ITS message")
        };
        state.tlc_cache.handle_mapem(time_now, &etsi);

        let etsi = if let c_its_parser::ItsMessage::Mapem {
            geonetworking: _,
            transport: _,
            etsi,
        } =
            c_its_parser::de::decode(MAPEM_HH_560, c_its_parser::Headers::None).unwrap()
        {
            etsi
        } else {
            panic!("Unexpected C-ITS message")
        };
        state.tlc_cache.handle_mapem(time_now, &etsi);

        // test GLOSA
        let own_pos_infront = geo_types::Point::new(9.9752864, 53.5564028);
        let own_pos_inside = geo_types::Point::new(9.9760764, 53.5563621);
        let own_pos_passed = geo_types::Point::new(9.977217, 53.5560602);
        let own_heading = 90.;

        state.heading_deg = Some(own_heading);

        state.position = own_pos_infront;
        state.run_glosa();
        assert!(matches!(state.glosa_state, GlosaState::Idle));

        state.position = own_pos_inside;
        state.run_glosa();
        assert!(matches!(state.glosa_state, GlosaState::Locked(_)));
        if let GlosaState::Locked(info) = &state.glosa_state {
            assert_eq!(1328, info.intersection_id);
            assert!(info.lane_id == 1 || info.lane_id == 2);
        }

        state.position = own_pos_inside;
        state.run_glosa();
        assert!(matches!(state.glosa_state, GlosaState::Locked(_)));
        if let GlosaState::Locked(info) = &state.glosa_state {
            assert_eq!(1328, info.intersection_id);
            assert!(info.lane_id == 1 || info.lane_id == 2);
        }

        state.position = own_pos_passed;
        state.run_glosa();
        assert!(matches!(state.glosa_state, GlosaState::Idle));
    }
}