//! Types for V2X data handling

use alloc::string::{String, ToString};
use alloc::vec::Vec;
use alloc::{format, vec};
use core::ops::Div;

use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc;
use log::warn;

use crate::cache::Cachable;
use crate::geo_alg;

#[derive(Debug, Clone)]
#[allow(unused)]
pub struct Intersection {
    id: u16,
    /// reference point as lon/lat
    ref_point: geo_types::Point,
    /// guessed center point as lon/lat
    center: geo_types::Point,
    lanes: Vec<Lane>,
    signal_groups: Vec<SignalGroup>,

    // internal state
    num_layers: u8,
    layers_present: Vec<u8>,
    is_complete: bool,
    last_spat_millis: u16,
}

impl Cachable<u16> for Intersection {
    fn key(&self) -> u16 {
        self.id
    }
}

impl core::fmt::Display for Intersection {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let lanes = self.lanes.iter().fold(String::new(), |mut acc, i| {
            use core::fmt::Write;
            let _ = writeln!(acc, "  {i}");
            acc
        });

        write!(
            f,
            "Intersection {} {{ ref: {:?}, \n{lanes}}}",
            self.id, self.ref_point
        )
    }
}

#[allow(unused)]
impl Intersection {
    pub fn new(layer_id: Option<u8>, value: &etsi_its_dsrc::IntersectionGeometry) -> Self {
        let id = value.id.id.0;
        let ref_point = value.ref_point.clone().into();
        let lanes = value
            .lane_set
            .0
            .iter()
            .filter_map(|v| match Lane::new(v, &ref_point) {
                Ok(lane) => Some(lane),
                Err(err) => {
                    warn!("{err}");
                    None
                }
            })
            .collect::<Vec<_>>();

        let (num_layers, layers_present, is_complete) = if let Some(layer_id) = layer_id {
            let (num_layers, layer_num) = Self::layer_num_and_count(layer_id);
            let complete = num_layers == 1; // just to be save from stupid input data

            (num_layers, alloc::vec![layer_num], complete)
        } else {
            (1, alloc::vec![1], true)
        };

        let signal_groups = if is_complete {
            Self::collect_signal_groups(&lanes)
        } else {
            Vec::default()
        };

        let center = if is_complete {
            Self::calculate_center(&ref_point, &lanes)
        } else {
            geo_types::Point::default()
        };

        Self {
            id,
            ref_point,
            center,
            lanes,
            signal_groups,
            num_layers,
            layers_present,
            is_complete,
            last_spat_millis: 0,
        }
    }

    pub fn add_layer(&mut self, layer_id: Option<u8>, value: &etsi_its_dsrc::IntersectionGeometry) {
        if self.is_complete {
            return;
        }
        let Some(layer_id) = layer_id else {
            return;
        };
        let (_, layer_num) = Self::layer_num_and_count(layer_id);

        // determine if we already have this layer
        if self.layers_present.contains(&layer_num) {
            return;
        }

        // add lanes
        for lane in &value.lane_set.0 {
            let lane_id = lane.lane_id.0;

            if self.lanes.iter().find(|i| i.id == lane_id).is_none() {
                match Lane::new(lane, &self.ref_point) {
                    Ok(lane) => self.lanes.push(lane),
                    Err(err) => {
                        warn!("{err}");
                    }
                }
            }
        }

        self.layers_present.push(layer_num);

        // we can just count the present layers since we made sure to not add already known ones beforehand
        if self.layers_present.len() == self.num_layers as usize {
            self.signal_groups = Self::collect_signal_groups(&self.lanes);
            self.center = Self::calculate_center(&self.ref_point, &self.lanes);
            self.is_complete = true;
        }
    }

    pub fn add_spat(&mut self, value: &etsi_its_dsrc::IntersectionState, year: i32) {
        if let Some(millis) = &value.time_stamp {
            if self.last_spat_millis == millis.0 {
                // skip this SPATEM since we already processed one with the same timestamp
                return;
            }
            self.last_spat_millis = millis.0;
        }

        if let Some(moy) = &value.moy {
            for state in &value.states.0 {
                let sig_grp = state.signal_group.0;

                if let Some(event) = state.state_time_speed.0.first() {
                    // get write access to SignalGroup item
                    if let Some(idx) = self.signal_groups.iter().position(|i| i.id == sig_grp)
                        && let Some(sig_grp) = self.signal_groups.get_mut(idx)
                    {
                        sig_grp.phase = Some(event.event_state);
                        if let Some(timing) = &event.timing {
                            if !timing.min_end_time.is_out_of_range()
                                && !timing.min_end_time.is_unknown()
                            {
                                sig_grp.min_end_time =
                                    Some(timing.min_end_time.to_datetime_from_moy(moy, year));
                            }
                            if let Some(likely) = &timing.likely_time
                                && !likely.is_out_of_range()
                                && !likely.is_unknown()
                            {
                                sig_grp.likely_time = Some(likely.to_datetime_from_moy(moy, year));
                            }
                            if let Some(max) = &timing.max_end_time
                                && !max.is_out_of_range()
                                && !max.is_unknown()
                            {
                                sig_grp.max_end_time = Some(max.to_datetime_from_moy(moy, year));
                            }
                        }
                    }
                } else {
                    // warn!("SPAT: SG {sig_grp:03} has no timing in first event");
                }
            }
        }
    }

    fn collect_signal_groups(lanes: &[Lane]) -> Vec<SignalGroup> {
        let mut connections = lanes
            .iter()
            .flat_map(ConnectionExt::from_lane)
            .filter(|i| i.conn.signal_group.is_some())
            .collect::<Vec<_>>();

        connections.sort_by_key(|i| i.conn.signal_group.unwrap_or_default());

        let chunks = connections.chunk_by(|a, b| a.conn.signal_group == b.conn.signal_group);
        chunks
            .map(|chunk| {
                let (sig_grp_id, maneuvers, mut target_lanes, mut source_lane_usages) =
                    chunk.iter().fold(
                        (None, Maneuvers::default(), vec![], vec![]),
                        |(sig_grp_id, maneuvers, mut lanes, mut usages), i| {
                            let new_maneuvers = i
                                .conn
                                .maneuvers
                                .map(|v| core::ops::Add::add(maneuvers, v))
                                .unwrap_or_default();
                            lanes.push(i.conn.target_lane_id);

                            let lane_usages = i.source_lane_usages.clone();
                            usages.extend_from_slice(&lane_usages);

                            usages.sort_unstable();
                            usages.dedup(); // TODO: only do this after all lanes were added?

                            (i.conn.signal_group, new_maneuvers, lanes, usages)
                        },
                    );

                // unwrap is fine since we removed all connections without signal group beforehand
                let id = sig_grp_id.unwrap();
                let maneuvers = if maneuvers.is_emtpy() {
                    None
                } else {
                    Some(maneuvers)
                };
                target_lanes.sort_unstable();
                // TODO: dedup as well!?

                SignalGroup {
                    id,
                    maneuvers,
                    source_lane_usages,
                    target_lanes,
                    phase: None,
                    min_end_time: None,
                    max_end_time: None,
                    likely_time: None,
                }
            })
            .collect()
    }

    fn calculate_center(ref_pos: &geo_types::Point, lanes: &[Lane]) -> geo_types::Point {
        let (sum, count) = lanes.iter().fold(
            (geo_types::Point::default(), 0),
            |(center_sum, count), lane| {
                // only use vehicle lanes
                if lane.usages.contains(&UsageType::Vehicle)
                    && let Some(stop_line) = lane.get_stop_line()
                {
                    (center_sum + stop_line.into(), count + 1)
                } else {
                    (center_sum, count)
                }
            },
        );

        // convert from X/Y to geo coordinate
        let center_xy = sum.div(f64::from(count));

        geo_alg::dcart_to_lonlat(ref_pos, &center_xy)
    }

    fn layer_num_and_count(layer_id: u8) -> (u8, u8) {
        let layer_num = layer_id % 10;
        let num_layers = (layer_id - layer_num) / 10;

        (num_layers, layer_num)
    }

    pub fn is_complete(&self) -> bool {
        self.is_complete
    }

    pub fn id(&self) -> u16 {
        self.id
    }
    pub fn ref_point(&self) -> geo_types::Point {
        self.ref_point
    }
    pub fn center(&self) -> geo_types::Point {
        self.center
    }
    pub fn lanes(&self) -> &[Lane] {
        &self.lanes
    }
    pub fn signal_groups(&self) -> &[SignalGroup] {
        &self.signal_groups
    }

    pub fn lane(&self, lane_id: u8) -> Option<&Lane> {
        self.lanes.iter().find(|i| i.id == lane_id)
    }

    pub fn ingress_lanes(&self) -> Vec<&Lane> {
        self.lanes.iter().filter(|i| i.is_ingress).collect()
    }

    pub fn bikeable_ingress_lanes(&self) -> Vec<&Lane> {
        self.lanes
            .iter()
            .filter(|i| i.is_ingress && i.usages.contains(&UsageType::Bike))
            .collect()
    }

    pub fn approach_lanes(&self, approach_id: u8) -> Vec<&Lane> {
        self.lanes
            .iter()
            .filter(|i| i.approach_id.is_some_and(|i| i == approach_id))
            .collect()
    }
}

#[derive(Debug, Clone)]
#[allow(unused)]
pub struct Lane {
    id: u8,
    approach_id: Option<u8>,
    /// lane nodes as local X/Y coordinates, X to the North, Y to the East
    nodes: geo_types::LineString,
    is_ingress: bool,
    is_egress: bool,
    usages: Vec<UsageType>,
    connections: Vec<Connection>,
    maneuvers: Vec<Maneuvers>,
}

impl core::fmt::Display for Lane {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let type_str = match (self.is_ingress, self.is_egress) {
            (true, true) => "IE",
            (true, false) => "I",
            (false, true) => "E",
            (false, false) => "",
        };
        let appr_str = if let Some(appr) = self.approach_id {
            format!("{appr}")
        } else {
            String::new()
        };
        let conns_str: String = self.connections.iter().fold(String::new(), |mut acc, i| {
            use core::fmt::Write;
            let _ = write!(acc, "{i}");
            acc
        });

        write!(
            f,
            "Lane {} ({type_str}-{appr_str}, {:?}) -> [{conns_str}]",
            self.id, self.usages
        )
    }
}

#[derive(Debug, Copy, Clone)]
#[allow(unused)]
pub struct Connection {
    id: Option<u8>,
    target_lane_id: u8,
    maneuvers: Option<Maneuvers>,
    signal_group: Option<u8>,
}

impl core::fmt::Display for Connection {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let id = self.id.unwrap_or_default();
        let sig_str = match self.signal_group {
            Some(sig) => format!("sig. {sig}"),
            None => String::new(),
        };

        write!(f, "{{ conn. {id}: to {}, {sig_str} }}", self.target_lane_id)
    }
}

#[allow(unused)]
impl Connection {
    pub fn id(&self) -> Option<u8> {
        self.id
    }
    pub fn maneuvers(&self) -> Option<Maneuvers> {
        self.maneuvers
    }
    pub fn signal_group(&self) -> Option<u8> {
        self.signal_group
    }
}

impl From<&etsi_its_dsrc::Connection> for Connection {
    fn from(value: &etsi_its_dsrc::Connection) -> Self {
        let id = value.connection_id.as_ref().map(|v| v.0);
        let target_lane_id = value.connecting_lane.lane.0;
        let maneuvers = value
            .connecting_lane
            .maneuver
            .as_ref()
            .map(Maneuvers::from_dsrc);
        let signal_group = value.signal_group.as_ref().map(|v| v.0);

        Self {
            id,
            target_lane_id,
            maneuvers,
            signal_group,
        }
    }
}

#[derive(Debug, Clone)]
#[allow(unused)]
struct ConnectionExt {
    source_lane_id: u8,
    source_lane_usages: Vec<UsageType>,
    conn: Connection,
}

impl ConnectionExt {
    fn from_lane(value: &Lane) -> Vec<Self> {
        value
            .connections
            .iter()
            .map(|conn| Self {
                source_lane_id: value.id,
                source_lane_usages: value.usages.clone(),
                conn: *conn,
            })
            .collect()
    }
}

/// Different view on a lane connection
///
/// Technically this data is redundant with the `Connection` data of a `Lane`,
/// but it makes processing SPAT information easier, if signal group maneuvers can be accessed directly and not my collecting them from all lane connections.
#[derive(Debug, Clone)]
#[allow(unused)]
pub struct SignalGroup {
    id: u8,
    maneuvers: Option<Maneuvers>,
    source_lane_usages: Vec<UsageType>, // for easier signal group evaluation
    target_lanes: Vec<u8>,              // mainly for debugging

    // SPAT data
    /// current signal phase
    phase: Option<etsi_its_dsrc::MovementPhaseState>,
    min_end_time: Option<chrono::DateTime<chrono::Utc>>,
    max_end_time: Option<chrono::DateTime<chrono::Utc>>,
    likely_time: Option<chrono::DateTime<chrono::Utc>>,
}

impl core::fmt::Display for SignalGroup {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        let manv_str = self.maneuvers.map(|v| format!(" {v}")).unwrap_or_default();
        let phase_str = self.phase_to_str().unwrap_or_default();
        write!(
            f,
            "SG {:2} {{{manv_str} to {:?} {phase_str}}}",
            self.id, self.target_lanes
        )
    }
}

#[allow(unused)]
impl SignalGroup {
    pub fn id(&self) -> u8 {
        self.id
    }
    pub fn maneuvers(&self) -> Option<Maneuvers> {
        self.maneuvers
    }
    pub fn source_lane_usages(&self) -> &[UsageType] {
        &self.source_lane_usages
    }
    pub fn phase(&self) -> Option<etsi_its_dsrc::MovementPhaseState> {
        self.phase
    }
    pub fn min_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
        self.min_end_time
    }
    pub fn likely_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
        self.likely_time
    }
    pub fn max_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
        self.max_end_time
    }

    pub fn phase_to_str(&self) -> Option<String> {
        self.phase.map(|v| {
            match v {
                etsi_its_dsrc::MovementPhaseState::unavailable => "n.a.",
                etsi_its_dsrc::MovementPhaseState::dark => "dark",
                etsi_its_dsrc::MovementPhaseState::stop_Then_Proceed => "stop",
                etsi_its_dsrc::MovementPhaseState::stop_And_Remain => "RED",
                etsi_its_dsrc::MovementPhaseState::pre_Movement => "PRE",
                etsi_its_dsrc::MovementPhaseState::permissive_Movement_Allowed
                | etsi_its_dsrc::MovementPhaseState::protected_Movement_Allowed => "GRN",
                etsi_its_dsrc::MovementPhaseState::permissive_clearance
                | etsi_its_dsrc::MovementPhaseState::protected_clearance => "YEL",

                etsi_its_dsrc::MovementPhaseState::caution_Conflicting_Traffic => "blnk",
            }
            .to_string()
        })
    }
}

#[derive(Debug, Clone)]
#[allow(unused)]
pub struct SignalPhase {
    state: etsi_its_dsrc::SignalStatus,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
#[allow(unused)]
pub enum UsageType {
    /// vehicle lanes that are not limited to Bus or Taxi use, nor restricted from public use
    Vehicle,
    /// vehicle lanes that are restricted to bus use or vehicle lanes shared with bus vehicle traffic
    Bus,
    /// tracked-vehicle lanes or vehicle lanes shared with tracked vehicle traffic
    Tram,
    /// bike-lanes or vehicle lanes shared with cyclist vehicle traffic
    Bike,
    /// crosswalks or sidewalks
    Pedestrian,
}

#[derive(Debug, Default, Copy, Clone, PartialEq, Eq)]
pub struct Maneuvers {
    pub left_allowed: bool,
    pub staight_allowed: bool,
    pub right_allowed: bool,
}

impl core::fmt::Display for Maneuvers {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "Maneuvers {{ {}{}{} }}",
            if self.left_allowed { "l" } else { "_" },
            if self.staight_allowed { "s" } else { "_" },
            if self.right_allowed { "r" } else { "_" },
        )
    }
}

impl PartialOrd for Maneuvers {
    fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
        Some(self.cmp(other))
    }
}
impl Ord for Maneuvers {
    fn cmp(&self, other: &Self) -> core::cmp::Ordering {
        if self.eq(other) {
            core::cmp::Ordering::Equal
        } else {
            #[allow(clippy::unnested_or_patterns)]
            match (self.left_allowed, self.staight_allowed, self.right_allowed) {
                // left (or none) < everything
                (true, false, false) | (false, false, false) => core::cmp::Ordering::Less,
                // left and straight > (just left)
                (true, true, false) => {
                    if other.eq(&Maneuvers::new(true, false, false)) {
                        core::cmp::Ordering::Greater
                    } else {
                        core::cmp::Ordering::Less
                    }
                }
                // omni > (anything without right)
                (true, true, true) | (true, false, true) => {
                    if other.right_allowed {
                        core::cmp::Ordering::Less
                    } else {
                        core::cmp::Ordering::Greater
                    }
                }
                // (only straight) > omni or left or left+straight
                (false, true, false) => {
                    if other.eq(&Maneuvers::new(true, true, true))
                        || other.eq(&Maneuvers::new(true, false, false))
                        || other.eq(&Maneuvers::new(true, true, false))
                    {
                        core::cmp::Ordering::Greater
                    } else {
                        core::cmp::Ordering::Less
                    }
                }
                // (straight and right) < right
                (false, true, true) => {
                    if other.eq(&Maneuvers::new(false, false, true)) {
                        core::cmp::Ordering::Less
                    } else {
                        core::cmp::Ordering::Greater
                    }
                }
                // right > alles
                (false, false, true) => core::cmp::Ordering::Greater,
            }
        }
    }
}

impl core::ops::Add for Maneuvers {
    type Output = Maneuvers;

    fn add(self, rhs: Self) -> Self::Output {
        Self::Output {
            staight_allowed: self.staight_allowed || rhs.staight_allowed,
            left_allowed: self.left_allowed || rhs.left_allowed,
            right_allowed: self.right_allowed || rhs.right_allowed,
        }
    }
}

#[allow(unused)]
impl Maneuvers {
    pub fn new(left_allowed: bool, staight_allowed: bool, right_allowed: bool) -> Self {
        Self {
            left_allowed,
            staight_allowed,
            right_allowed,
        }
    }

    pub fn from_dsrc(value: &etsi_its_dsrc::AllowedManeuvers) -> Self {
        let left_allowed =
            value.get_maneuver_left_allowed() || value.get_maneuver_left_turn_on_red_allowed();
        let right_allowed =
            value.get_maneuver_right_allowed() || value.get_maneuver_right_turn_on_red_allowed();

        Self {
            left_allowed,
            staight_allowed: value.get_maneuver_straight_allowed(),
            right_allowed,
        }
    }

    pub fn is_emtpy(self) -> bool {
        !self.left_allowed && !self.staight_allowed && !self.right_allowed
    }
}

#[allow(unused)]
impl Lane {
    /// Creates a `Lane` from C-ITS data
    pub fn new(
        lane: &etsi_its_dsrc::GenericLane,
        ref_pos: &geo_types::Point,
    ) -> Result<Self, String> {
        let etsi_its_dsrc::NodeListXY::nodes(node_set_xy) = &lane.node_list else {
            return Err(format!(
                "Lane::new(): ComputedLane not supported (used in lane {})",
                lane.lane_id.0
            ));
        };

        let nodes_enu = Self::node_set_to_line_string_xy(node_set_xy, *ref_pos);

        // convert to NED instead of ENU
        let nodes = nodes_enu
            .points()
            .map(|p| geo_types::Point::new(p.y(), p.x()))
            .collect();

        let usages = UsageType::get_usages(&lane.lane_attributes)?;
        let connections = lane
            .connects_to
            .as_ref()
            .map(|v| v.0.iter().map(core::convert::Into::into).collect())
            .unwrap_or_default();

        let maneuvers = alloc::vec![];

        let is_ingress = lane.lane_attributes.directional_use.get_ingress_path();
        let is_egress = lane.lane_attributes.directional_use.get_egress_path();

        Ok(Self {
            id: lane.lane_id.0,
            approach_id: lane.ingress_approach.as_ref().map(|v| v.0),
            nodes,
            usages,
            connections,
            maneuvers,
            is_ingress,
            is_egress,
        })
    }

    fn node_set_to_line_string_xy(
        data: &etsi_its_dsrc::NodeSetXY,
        ref_pos: geo_types::Point,
    ) -> geo_types::LineString {
        let mut prev_delta = geo_types::Point::default();
        let mut prev_ref_pos = ref_pos;
        data.0
            .iter()
            .map(|pt| {
                let node = match &pt.delta {
                    etsi_its_dsrc::NodeOffsetPointXY::node_LatLon(node_llm_d64b) => {
                        let result = pt.delta.to_ddist(&prev_ref_pos);

                        // reference position needs to be the last `node_LatLon`
                        prev_ref_pos = geo_types::Point::new(
                            node_llm_d64b.lon.as_deg(),
                            node_llm_d64b.lat.as_deg(),
                        );
                        result
                    }
                    _ => prev_delta + pt.delta.to_ddist(&ref_pos),
                };
                prev_delta = node;

                node
            })
            .collect()
    }

    pub fn id(&self) -> u8 {
        self.id
    }
    pub fn approach_id(&self) -> Option<u8> {
        self.approach_id
    }
    pub fn is_ingress(&self) -> bool {
        self.is_ingress
    }
    pub fn nodes(&self) -> &geo_types::LineString {
        &self.nodes
    }
    pub fn usages(&self) -> &[UsageType] {
        &self.usages
    }
    pub fn connections(&self) -> &[Connection] {
        &self.connections
    }
    pub fn maneuvers(&self) -> &[Maneuvers] {
        &self.maneuvers
    }

    pub fn get_stop_line(&self) -> Option<geo_types::Coord> {
        self.nodes.0.first().copied()
    }

    /// Determines average heading
    pub fn get_heading(&self) -> f32 {
        // unwrap is fine since a lane always contains at least 2 nodes
        let stop_line = self.nodes.points().next().unwrap();
        // unwrap is fine since a lane always contains at least 2 nodes
        let second_point = self.nodes.points().next_back().unwrap();

        #[allow(clippy::cast_possible_truncation)]
        let average_heading = geo_alg::cartesian_bearing(second_point, stop_line) as f32;

        average_heading
    }

    /// Determines heading at stop line and average
    pub fn get_headings(&self) -> (f32, f32) {
        // unwrap is fine since a lane always contains at least 2 nodes
        let stop_line = self.nodes.points().next().unwrap();
        // unwrap is fine since a lane always contains at least 2 nodes
        let second_point = self.nodes.points().next().unwrap();
        // unwrap is fine since a lane always contains at least 2 nodes
        let entry_point = self.nodes.points().next_back().unwrap();

        #[allow(clippy::cast_possible_truncation)]
        let stop_line_heading = geo_alg::cartesian_bearing(second_point, stop_line) as f32;
        #[allow(clippy::cast_possible_truncation)]
        let average_heading = geo_alg::cartesian_bearing(entry_point, stop_line) as f32;

        (stop_line_heading, average_heading)
    }
}

#[allow(dead_code)]
impl UsageType {
    /// Determine possible usages of the lane
    ///
    /// Lanes may be used my multiple classes of traffic at once, so this needs to
    /// return a vector of [`UsageType`].
    ///
    /// # Errors
    /// Returns display string of the [`LaneAttributes`], if no suitable lane class was found
    pub fn get_usages(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> Result<Vec<Self>, String> {
        let mut result = alloc::vec![];
        if Self::is_vehicle_useable(lane_attrs) {
            result.push(Self::Vehicle);
        }
        if Self::is_bus_useable(lane_attrs) {
            result.push(Self::Bus);
        }
        if Self::is_tram_useable(lane_attrs) {
            result.push(Self::Tram);
        }
        if Self::is_bike_useable(lane_attrs) {
            result.push(Self::Bike);
        }
        if Self::is_pedestrian_useable(lane_attrs) {
            result.push(Self::Pedestrian);
        }

        if result.is_empty() {
            Err(format!("Unsupported LaneAttributes: {lane_attrs}"))
        } else {
            Ok(result)
        }
    }

    /// See [`UsageType::Vehicle`]
    fn is_vehicle_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
        match &lane_attrs.lane_type {
            etsi_its_dsrc::LaneTypeAttributes::vehicle(attrs) => {
                !attrs.get_restricted_to_bus_use()
                    && !attrs.get_restricted_to_taxi_use()
                    && !attrs.get_restricted_from_public_use()
            }
            _ => false,
        }
    }

    /// See [`UsageType::Bus`]
    fn is_bus_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
        match &lane_attrs.lane_type {
            etsi_its_dsrc::LaneTypeAttributes::vehicle(attrs) => {
                attrs.get_restricted_to_bus_use() | lane_attrs.shared_with.get_bus_vehicle_traffic()
            }
            _ => false,
        }
    }

    /// See [`UsageType::Tram`]
    fn is_tram_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
        match &lane_attrs.lane_type {
            etsi_its_dsrc::LaneTypeAttributes::trackedVehicle(_) => true,
            etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
                lane_attrs.shared_with.get_tracked_vehicle_traffic()
            }
            _ => false,
        }
    }

    /// See [`UsageType::Bike`]
    fn is_bike_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
        match &lane_attrs.lane_type {
            etsi_its_dsrc::LaneTypeAttributes::bikeLane(_) => true,
            // etsi_its_dsrc::LaneTypeAttributes::sidewalk(attrs) => {
            //     attrs.get_bicycle_use_allowed()
            // }
            etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
                lane_attrs.shared_with.get_cyclist_vehicle_traffic()
            }
            _ => false,
        }
    }

    /// See [`UsageType::Pedestrian`]
    fn is_pedestrian_useable(lane_attrs: &etsi_its_dsrc::LaneAttributes) -> bool {
        match &lane_attrs.lane_type {
            etsi_its_dsrc::LaneTypeAttributes::crosswalk(_)
            | etsi_its_dsrc::LaneTypeAttributes::sidewalk(_) => true,
            // etsi_its_dsrc::LaneTypeAttributes::vehicle(_) => {
            //     lane_attrs.shared_with.get_pedestrian_traffic()
            //         || lane_attrs.shared_with.get_pedestrians_traffic()
            // }
            _ => false,
        }
    }
}

#[cfg(test)]
mod tests {
    use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::IntersectionGeometry;
    use c_its_parser::standards::mapem_2_2_1::mapem_pdu_descriptions;

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

    #[test]
    fn map_simple() {
        let etsi = decode_mapem(MAPEM_HH_1328);

        let layer_id = etsi.map.layer_id.map(|v| v.0);
        if let Some(map) = &etsi.map.intersections {
            for geom in &map.0 {
                let intersection = Intersection::new(layer_id, geom);

                assert!(intersection.is_complete());
                assert_ne!(geo_types::Point::default(), intersection.center());

                // validate that center position is reasonable
                let ref_pos_dist =
                    geo_alg::haversine_dist(&intersection.center, &intersection.ref_point);
                println!(
                    "Map ref {:?}, center {ref_pos_dist:.1} m away",
                    intersection.ref_point
                );
                assert!(ref_pos_dist < 15.); // for MAP 1328 the reference point is ~12 meters from the intersection center
            }
        } else {
            panic!("MAPEM is missing intersection data")
        }

        // forge a layered mapem
        let layer_id = Some(11);
        if let Some(map) = &etsi.map.intersections {
            for geom in &map.0 {
                let intersection = Intersection::new(layer_id, geom);

                assert!(intersection.is_complete());
                assert_ne!(geo_types::Point::default(), intersection.center());
            }
        } else {
            panic!("MAPEM is missing intersection data")
        }

        let layer_id = Some(22);
        if let Some(map) = &etsi.map.intersections {
            for geom in &map.0 {
                let intersection = Intersection::new(layer_id, geom);

                assert!(!intersection.is_complete());
                assert_eq!(geo_types::Point::default(), intersection.center());
            }
        } else {
            panic!("MAPEM is missing intersection data")
        }
    }

    #[test]
    fn map_layered() {
        let etsi_layer31 = decode_mapem(MAPEM_HH_2349_L31);
        let etsi_layer32 = decode_mapem(MAPEM_HH_2349_L32);
        let etsi_layer33 = decode_mapem(MAPEM_HH_2349_L33);

        // add some layer
        let (layer_id, geom) = extract_geom(&etsi_layer32);
        let mut intersection = Intersection::new(layer_id, geom);
        assert!(!intersection.is_complete());
        assert_eq!(geo_types::Point::default(), intersection.center());

        // add a second layer
        {
            let num_lanes_before = intersection.lanes().len();

            let (layer_id, geom) = extract_geom(&etsi_layer33);
            intersection.add_layer(layer_id, geom);
            assert!(!intersection.is_complete());
            assert_eq!(geo_types::Point::default(), intersection.center());

            let num_lanes_after = intersection.lanes().len();
            assert!(num_lanes_before <= num_lanes_after);
        }

        // add first layer again
        {
            let num_lanes_before = intersection.lanes().len();

            let (layer_id, geom) = extract_geom(&etsi_layer32);
            intersection.add_layer(layer_id, geom);
            assert!(!intersection.is_complete());
            assert_eq!(geo_types::Point::default(), intersection.center());

            let num_lanes_after = intersection.lanes().len();
            assert!(num_lanes_before == num_lanes_after);
        }

        // add final layer
        {
            let num_lanes_before = intersection.lanes().len();

            let (layer_id, geom) = extract_geom(&etsi_layer31);
            intersection.add_layer(layer_id, geom);
            assert!(intersection.is_complete());
            assert_ne!(geo_types::Point::default(), intersection.center());

            let num_lanes_after = intersection.lanes().len();
            assert!(num_lanes_before <= num_lanes_after);
        }
    }

    #[test]
    fn map_signal_groups() {
        init_test_env_logger();

        let etsi = decode_mapem(MAPEM_HH_1328);
        let layer_id = etsi.map.layer_id.map(|v| v.0);
        if let Some(map) = &etsi.map.intersections {
            for geom in &map.0 {
                let intersection = Intersection::new(layer_id, geom);

                assert!(intersection.is_complete());
                assert_eq!(2, intersection.signal_groups.len());
            }
        } else {
            panic!("MAPEM is missing intersection data")
        }

        let etsi = decode_mapem(MAPEM_HH_560);
        let layer_id = etsi.map.layer_id.map(|v| v.0);
        if let Some(map) = &etsi.map.intersections {
            for geom in &map.0 {
                let intersection = Intersection::new(layer_id, geom);

                assert!(intersection.is_complete());
                assert_eq!(10, intersection.signal_groups.len());
            }
        } else {
            panic!("MAPEM is missing intersection data")
        }
    }

    fn decode_mapem(uper: &[u8]) -> Box<mapem_pdu_descriptions::MAPEM> {
        if let c_its_parser::ItsMessage::Mapem {
            geonetworking: _,
            transport: _,
            etsi,
        } = c_its_parser::de::decode(uper, c_its_parser::Headers::None).unwrap()
        {
            etsi
        } else {
            panic!("Unexpected C-ITS message")
        }
    }

    fn extract_geom(
        etsi: &Box<mapem_pdu_descriptions::MAPEM>,
    ) -> (Option<u8>, &IntersectionGeometry) {
        let layer_id = etsi.map.layer_id.as_ref().map(|v| v.0);
        if let Some(map) = &etsi.map.intersections {
            // just assume we only have one intersection per mapem
            (layer_id, map.0.first().unwrap())
        } else {
            panic!("MAPEM is missing intersection data")
        }
    }

    #[test]
    fn maneuver_order() {
        let left = Maneuvers::new(true, false, false);
        let straight_and_left = Maneuvers::new(true, true, false);
        let straight = Maneuvers::new(false, true, false);
        let omni = Maneuvers::new(true, true, true);
        let straight_and_right = Maneuvers::new(false, true, true);
        let right = Maneuvers::new(false, false, true);

        let maneuvers = vec![
            right,
            omni,
            straight,
            left,
            straight_and_left,
            straight_and_right,
        ];

        let mut sorted = maneuvers.clone();
        sorted.sort();

        println!("Sorted maneuvers:");
        for manv in &sorted {
            println!("- {manv}");
        }

        assert_eq!(left, sorted[0]);
        assert_eq!(straight_and_left, sorted[1]);
        assert_eq!(omni, sorted[2]);
        assert_eq!(straight, sorted[3]);
        assert_eq!(straight_and_right, sorted[4]);
        assert_eq!(right, sorted[5]);

        // was a bug at 2026-06-21
        let maneuvers = vec![right, left, straight_and_left, straight];

        let mut sorted = maneuvers.clone();
        sorted.sort();

        println!("Sorted maneuvers:");
        for manv in &sorted {
            println!("- {manv}");
        }

        assert_eq!(left, sorted[0]);
        assert_eq!(straight_and_left, sorted[1]);
        assert_eq!(straight, sorted[2]);
        assert_eq!(right, sorted[3]);
    }
}