//! Geo Algorithms
//!
//! Since the `geo` crate is not `no_std`, we need to implement the most important algorithms on our own

use alloc::string::{String, ToString};
use core::ops::{Mul, Sub};

use log::warn;
#[cfg(not(feature = "std"))]
use num_traits::float::Float;

const EARTH_RADIUS: f64 = 6_371_008.8; // mean radius of GRS80 ellipsoid
const EARTH_CIRCUMFERENCE: f64 = 40_030_228.884; // calculated from `EARTH_RADIUS * 2 * PI`

/// Haversine distance between two geo coordinates (in meters)
#[allow(unused)]
pub fn haversine_dist(a: &geo_types::Point, b: &geo_types::Point) -> f64 {
    let theta1 = a.y().to_radians();
    let theta2 = b.y().to_radians();
    let delta_theta = (b.y() - a.y()).to_radians();
    let delta_lambda = (b.x() - a.x()).to_radians();
    let a = (delta_theta / 2.).sin().powi(2)
        + theta1.cos() * theta2.cos() * (delta_lambda / 2.).sin().powi(2);
    let c = 2. * a.sqrt().asin();
    EARTH_RADIUS * c
}

/// Haversine bearing from point `a` to point `b`
#[allow(unused)]
pub fn haversine_bearing(a: &geo_types::Point, b: &geo_types::Point) -> f32 {
    let (lng_a, lat_a) = (a.x().to_radians(), a.y().to_radians());
    let (lng_b, lat_b) = (b.x().to_radians(), b.y().to_radians());
    let delta_lng = lng_b - lng_a;

    #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
    let s = (lat_b.cos() * delta_lng.sin()) as f32;
    #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
    let c = (lat_a.cos() * lat_b.sin() - lat_a.sin() * lat_b.cos() * delta_lng.cos()) as f32;

    let degrees = f32::atan2(s, c).to_degrees();
    (degrees + 360.0) % 360.0
}

/// Cartesian distance between to X/Y coordinates (in unit of the coordinates)
#[allow(unused)]
pub fn cartesian_dist(a: geo_types::Point, b: geo_types::Point) -> f64 {
    let vector = b.sub(a);
    cartesian_norm(vector)
}

/// Cartesian bearing from X/Y point `a` to X/Y point `b`
#[allow(unused)]
pub fn cartesian_bearing(a: geo_types::Point, b: geo_types::Point) -> f64 {
    let vector = b.sub(a);
    cartesian_theta(vector)
}

/// Cartesian vector norm (length)
#[allow(unused)]
pub fn cartesian_norm(vec: geo_types::Point) -> f64 {
    f64::sqrt(vec.x() * vec.x() + vec.y() * vec.y())
}

/// Cartesian vector direction (polar coordinate angle)
#[allow(unused)]
pub fn cartesian_theta(vec: geo_types::Point) -> f64 {
    let theta_rad = f64::atan2(vec.y(), vec.x());
    theta_rad * 180. / core::f64::consts::PI
}

/// Normalizes an angle (in degrees) to (-180, 180] interval
#[allow(unused)]
pub fn deg_180(angle_deg: f32) -> f32 {
    let full_circle_deg = 360.0;

    let angle = angle_deg % full_circle_deg;
    if angle > full_circle_deg / 2. {
        return angle - full_circle_deg;
    }
    if angle <= -full_circle_deg / 2. {
        return angle + full_circle_deg;
    }
    angle
}

/// Normalizes an angle (in degrees) to [0, 360) interval
#[allow(unused)]
pub fn deg_360(angle_deg: f32) -> f32 {
    let full_circle_deg = 360.0;

    let angle = angle_deg % full_circle_deg;
    if (angle < 0.) {
        return angle + full_circle_deg;
    }
    angle
}

/// Converts a geo-position (lon/lat) to cartesian coordinates relative to a reference positions
///
/// Both `ref_pos` and `position` are expected to be lon/lat in degrees.
/// The output will be in a local X/Y coordinate system with X pointing north and Y pointing east.
#[allow(unused)]
pub fn lonlat_to_dcart(
    ref_pos: &geo_types::Point,
    position: &geo_types::Point,
) -> geo_types::Point {
    let pos_diff = position.sub(*ref_pos);
    let dlat_deg = pos_diff.y();
    let dlon_deg = pos_diff.x();

    // latitude (north/south) degree per meter is independent from longitude
    let dx = dlat_deg / 360. * EARTH_CIRCUMFERENCE;

    // longitude (east/west) degree per meter has a different value depending on the latitude
    #[allow(clippy::cast_possible_truncation)]
    let ref_lat_rad = ref_pos.to_radians().y();
    let dy = dlon_deg / 360. * (EARTH_CIRCUMFERENCE * ref_lat_rad.cos());

    geo_types::Point::new(dx, dy)
}

/// Converts a X/Y coordinate (NED relative to a reference position) to a geo-position
///
/// The `ref_pos` is expected to be lon/lat in degrees and the output will be the same.
/// The `position` is expected to be in a local X/Y coordinate system with X pointing north and Y pointing east.
#[allow(unused)]
pub fn dcart_to_lonlat(
    ref_pos: &geo_types::Point,
    position: &geo_types::Point,
) -> geo_types::Point {
    // note that we're using NED while c-its-parser uses ITS-G5 style ENU X/Y coordinates
    // so we need to swap X and Y coordinates

    #[allow(clippy::cast_possible_truncation)]
    let dx = position.y() as f32;
    #[allow(clippy::cast_possible_truncation)]
    let dy = position.x() as f32;

    c_its_parser::geo_utils::point_from_dxy(dx, dy, ref_pos)
}

/// Describes how a position is related to a line segment
#[derive(Debug, Default)]
#[allow(unused)]
pub struct LinePosition {
    /// Own position relative to the line segment
    pub semantic: LinePositionType,
    /// Distance to center line (always positive)
    pub parallel_dist: f64,
    /// Distance to first node (negative if `BeforeStart`)
    pub perpendicular_dist: f64,
}

#[derive(Debug, Default, PartialEq, Eq, PartialOrd, Ord)]
#[allow(unused)]
pub enum LinePositionType {
    #[default]
    Undefined,
    BeforeStart,
    InBetween,
    AfterEnd,
}

#[allow(unused)]
impl LinePosition {
    pub fn new(parallel_dist: f64, perpendicular_dist: f64, after_end: bool) -> Self {
        let semantic = if after_end {
            LinePositionType::AfterEnd
        } else if parallel_dist < 0. {
            LinePositionType::BeforeStart
        } else {
            LinePositionType::InBetween
        };
        Self {
            semantic,
            parallel_dist,
            perpendicular_dist,
        }
    }

    /// Determines `point` position relative to a line segment (in X/Y coordinates)
    ///
    /// The `ls` shall consist of 2 points!
    ///
    /// # Algorithm
    /// The line segment can be converted to a vector which describes an infinite line.
    /// So we can calculate the (perpendicular) projection of the point on that line:
    /// ```text
    ///    [Case A]            (point)   |  [Case B]   (point)
    ///                           |      |                |
    ///                           |      |                |
    ///    (node1)---(node2)-----(q)     |  (node1)------(q)------(node2)
    /// ```
    ///
    /// This will already give us the perpendicular and parallel distance.
    /// Since we came from a finite line segment and want to know if the point is bejond the segment,
    /// we also need to compare the parallel distance with the length of the vector.
    /// So both case are the same mathematically.
    ///
    /// # Errors
    /// Fails if points are the same
    pub fn for_ls(point: geo_types::Point, line: &geo_types::Line) -> Result<Self, String> {
        let node1 = line.start_point();
        let node2 = line.end_point();

        let line_vec = node2.sub(node1);
        let line_vec_length = cartesian_norm(line_vec);

        // input validation
        if line_vec_length < f64::EPSILON {
            return Err("Points are the same".to_string());
        }

        // calculate projection of `p` onto the line
        // ```math
        // # n needs to be a unit vector
        // n = (node2 - node1) / |node2 - node1|
        // # vector from node1 to the point
        // p = point - node1
        // # projection is the unit-vector multipled by the dot-product of p and n
        // q = (p {dot} n) * n
        // # perpendicular length is calculated from the vector q->p
        // QP = q - p
        // ```
        let p = point.sub(node1);
        let n = line_vec.mul(1. / line_vec_length);
        let q = n.mul(p.dot(n));
        let qp_length = cartesian_norm(q.sub(p));

        // calculate parallel distance of `q` to `node1`.
        // This can be achieved by the dot-product since `n` is a unit vector and
        // the dot-product of two paralle vectors is the same as the multiplication of the vector norms.
        let q_dist = n.dot(q);

        // generate output
        let after_end = q_dist > line_vec_length;
        let line_pos = Self::new(q_dist, qp_length, after_end);

        Ok(line_pos)
    }

    /// Determines `point` position relative to nodes of an ingress `Lane`
    ///
    /// Keep in mind, that the lane's node list starts at the stop line and is extending away from the intersection.
    /// But we are driving the other way around!
    ///
    /// # Errors
    /// Returns a human-readable string if position can't be determined
    pub fn for_ingress_lane(
        point: geo_types::Point,
        nodes: &geo_types::LineString,
    ) -> Result<Self, String> {
        let mut result = Self::default();

        let mut lines = nodes.lines();

        // check, that the stop line is not behind us
        let line = lines
            .next()
            .ok_or("Lane has less than two nodes".to_string())?;

        let pos = Self::for_ls(point, &line)?;
        if pos.semantic == LinePositionType::BeforeStart {
            // we have crossed the stop line -> abort search for position inside the lane
            return Ok(pos);
        }
        if pos.semantic == LinePositionType::InBetween {
            // we're at this segment -> search finished
            return Ok(pos);
        }
        // otherwise: continue with next segment (and store this segment's length)
        let mut lane_length = cartesian_norm(line.delta().into());
        let mut last_pos = LinePosition::default();

        // continue with remaining lines
        for line in lines {
            let line_len = cartesian_norm(line.delta().into());

            // ignored line, if points are too close <10cm
            if line_len < 0.1 {
                warn!("Lane nodes are just {line_len:.1} m apart -> ignoring");
                continue;
            }

            let pos = Self::for_ls(point, &line)?;
            match pos.semantic {
                LinePositionType::BeforeStart => {
                    // we're at the outer arc of the lane right at the border between line segments
                    // -> set parallel distance to zero and finish search
                    return Ok(LinePosition::new(
                        lane_length,
                        pos.perpendicular_dist,
                        false,
                    ));
                }
                LinePositionType::InBetween => {
                    // we're at this segment
                    // -> add segment's distance to accumulated lane length and finish search
                    return Ok(LinePosition::new(
                        lane_length + pos.parallel_dist,
                        pos.perpendicular_dist,
                        false,
                    ));
                }
                _ => {
                    // continue searching, add segment's distance to lane length accumulator
                    lane_length += line_len;
                    last_pos = pos;
                }
            }
        }

        // if we came this far, we're after the lane's end (actually the start in driving direction)
        // -> add parallel distance to accumulated lane length
        Ok(LinePosition::new(
            lane_length + last_pos.parallel_dist,
            last_pos.perpendicular_dist,
            true,
        ))
    }
}

#[cfg(test)]
mod tests {
    use crate::geo_alg::{LinePosition, cartesian_bearing};

    #[test]
    fn test_cart_bearing() {
        let start = geo_types::Point::new(0., 0.);

        assert_float_eq::assert_float_absolute_eq!(
            0.,
            cartesian_bearing(start, geo_types::Point::new(10., 0.))
        );

        assert_float_eq::assert_float_absolute_eq!(
            90.,
            cartesian_bearing(start, geo_types::Point::new(0., 10.))
        );

        assert_float_eq::assert_float_absolute_eq!(
            45.,
            cartesian_bearing(start, geo_types::Point::new(10., 10.))
        );

        assert_float_eq::assert_float_absolute_eq!(
            135.,
            cartesian_bearing(start, geo_types::Point::new(-10., 10.))
        );

        assert_float_eq::assert_float_absolute_eq!(
            -135.,
            cartesian_bearing(start, geo_types::Point::new(-10., -10.))
        );
    }

    #[test]
    fn linepos_for_ls_validation() {
        // identical nodes shall result in an error

        let point = geo_types::Point::new(0., 0.);
        let node1 = geo_types::Point::new(0., 0.);
        let node2 = geo_types::Point::new(10., -5.);

        let ls = geo_types::Line::new(node1, node1);
        assert!(LinePosition::for_ls(point, &ls).is_err());

        let ls = geo_types::Line::new(node2, node2);
        assert!(LinePosition::for_ls(point, &ls).is_err());
    }

    #[test]
    fn linepos_for_ls_test() {
        use crate::geo_alg::LinePositionType;

        // test on X axis
        {
            let node1 = geo_types::Point::new(0., 0.);
            let node2 = geo_types::Point::new(5., 0.);

            let ls = geo_types::Line::new(node1, node2);

            // test some positions
            let test_tuples = vec![
                (node1.x() - 1000., LinePositionType::BeforeStart),
                (node1.x() - 1., LinePositionType::BeforeStart),
                (node1.x(), LinePositionType::InBetween),
                (node1.x() + 1., LinePositionType::InBetween),
                (node2.x() - 1., LinePositionType::InBetween),
                (node2.x(), LinePositionType::InBetween),
                (node2.x() + 1., LinePositionType::AfterEnd),
                (node2.x() + 1000., LinePositionType::AfterEnd),
            ];
            let y_values = vec![(node1.y() - 1.), (node1.y()), (node1.y() + 1.)];

            // y position should only affect the perpendicular distance
            for y in &y_values {
                // x position affects the position type and parallel distance
                for (x, semantic) in &test_tuples {
                    let point = geo_types::Point::new(*x, *y);

                    let result = LinePosition::for_ls(point, &ls).unwrap();

                    assert_eq!(semantic, &result.semantic);
                    assert_eq!((y - node1.y()).abs(), result.perpendicular_dist);
                    assert_eq!(x - node1.x(), result.parallel_dist);
                }
            }
        }

        // test on Y axis
        {
            let node1 = geo_types::Point::new(3., 5.);
            let node2 = geo_types::Point::new(3., 100.);

            let ls = geo_types::Line::new(node1, node2);

            // test some positions
            let test_tuples = vec![
                (node1.y() - 1000., LinePositionType::BeforeStart),
                (node1.y() - 1., LinePositionType::BeforeStart),
                (node1.y(), LinePositionType::InBetween),
                (node1.y() + 1., LinePositionType::InBetween),
                (node2.y() - 1., LinePositionType::InBetween),
                (node2.y(), LinePositionType::InBetween),
                (node2.y() + 1., LinePositionType::AfterEnd),
                (node2.y() + 1000., LinePositionType::AfterEnd),
            ];
            let x_values = vec![(node1.x() - 1.), (node1.x()), (node1.x() + 1.)];

            // x position should only affect the perpendicular distance
            for x in &x_values {
                // y position affects the position type and parallel distance
                for (y, semantic) in &test_tuples {
                    let point = geo_types::Point::new(*x, *y);

                    let result = LinePosition::for_ls(point, &ls).unwrap();

                    assert_eq!(semantic, &result.semantic);
                    assert_eq!((x - node1.x()).abs(), result.perpendicular_dist);
                    assert_eq!(y - node1.y(), result.parallel_dist);
                }
            }
        }
    }
}