catenary 0.5.0

use levenberg_marquardt::LevenbergMarquardt;
use nalgebra::{ComplexField, Point2, RealField};
use num_dual::{DualNum, DualNumFloat};
use serde::{Deserialize, Serialize};
use solve::SolveCatenary;

use crate::roots::Roots;

/// A builder for creating a [`Catenary`] with the specified parameters.
pub mod catmaker;
mod ops;
mod solve;

#[derive(Debug, Default, Clone, Copy, Serialize, Deserialize, PartialEq)]
/// Represents a [`Catenary`] curve.
///
/// The [`Catenary`] curve is defined by the equation: `y(x) = a * cosh((x - c) / a) - a + h`
/// where:
/// - `a` is the parameter that controls the shape of the curve.
/// - `c` is the horizontal shift of the curve.
/// - `h` is the vertical shift of the curve.
/// - `s_0` and `s_1` are arc lengths of end points (negative on the left of minimum, positive on the right).
pub struct Catenary<D: DualNum<F>, F> {
    /// The parameter that controls the shape of the curve.
    pub a: D,
    /// The horizontal shift of the curve.
    pub c: D,
    /// The vertical shift of the curve.
    pub h: D,
    /// The arc length of the left endpoint  (from min).
    pub s_0: D,
    /// The arc length of the right endpoint (from min).
    pub s_1: D,

    pub(crate) _f: std::marker::PhantomData<F>,
}

pub type Catenary64 = Catenary<f64, f64>;
pub type Catenary32 = Catenary<f32, f32>;

impl<D, F> Catenary<D, F>
where
    D: DualNum<F>,
    F: DualNumFloat + std::convert::From<f32>,
{
    pub fn y_from_x(&self, x: &D) -> D {
        debug_assert!(
            self.a.re() != F::zero(),
            "Cannot compute y from x if a is zero"
        );
        ((x.clone() - &self.c) / &self.a).cosh() * &self.a - &self.a + &self.h
    }

    /// Computes the x-coordinate(s) from the given y-coordinate on the catenary curve.
    ///
    /// # Arguments
    ///
    /// * `y` - The y-coordinate.
    ///
    /// # Returns
    ///
    /// The roots of the equation representing the x-coordinate(s).
    pub fn x_from_y(&self, y: &D) -> Roots<D> {
        match y {
            _ if y.re() < self.h.re() => Roots::None,
            _ if (y.re()) == self.h.re() || self.a.re() == F::zero() => Roots::One(self.c.clone()),
            _ => {
                let d = ((y.clone() - &self.h) / &self.a + <F as From<f32>>::from(1.0)).acosh()
                    * &self.a;
                Roots::Two(-d.clone() + &self.c, d + &self.c)
            }
        }
    }

    /// Computes the y-coordinate(s) from the given arc length on the catenary curve.
    ///
    /// # Arguments
    ///
    /// * `s` - The arc length.
    ///
    /// # Returns
    ///
    /// The y-coordinate(s) corresponding to the given arc length.
    pub fn y_from_arc_length(&self, s: &D) -> D {
        if self.a.re() == 0.0.into() {
            self.h.clone() + s.abs()
        } else {
            (self.a.powi(2) + s.powi(2)).sqrt() - &self.a + &self.h
        }
    }

    /// Computes the arc length `s` from the given x-coordinate on the catenary curve.
    ///
    /// # Arguments
    ///
    /// * `x` - The x-coordinate.
    ///
    /// # Returns
    ///
    /// The arc length `s` corresponding to the given x-coordinate.
    pub fn s_from_x(&self, x: &D) -> D {
        debug_assert!(
            self.a.re() != 0.0.into(),
            "Cannot compute s from x if a is zero"
        );
        ((x.clone() - &self.c) / &self.a).sinh() * &self.a
    }

    /// Computes the x-coordinate from the given arc length on the catenary curve.
    ///
    /// # Arguments
    ///
    /// * `s` - The arc length.
    ///
    /// # Returns
    ///
    /// The x-coordinate corresponding to the given arc length.
    pub fn x_from_arc_length(&self, s: &D) -> D {
        if self.a.re() == 0.0.into() {
            self.c.clone()
        } else {
            (s.clone() / &self.a).asinh() * &self.a + &self.c
        }
    }

    /// Returns the length of the catenary curve.
    pub fn length(&self) -> D {
        (self.s_1.clone() - &self.s_0).abs()
    }

    /// Returns the end points of the catenary curve.
    ///
    /// # Returns
    ///
    /// A tuple containing the start and end points of the catenary curve.
    pub fn end_points(&self) -> (Point2<D>, Point2<D>) {
        (
            Point2::<D>::new(
                self.x_from_arc_length(&self.s_0),
                self.y_from_arc_length(&self.s_0),
            ),
            Point2::<D>::new(
                self.x_from_arc_length(&self.s_1),
                self.y_from_arc_length(&self.s_1),
            ),
        )
    }
}
pub(crate) type P2<F> = Point2<F>;

impl<D, F> Catenary<D, F>
where
    D: DualNum<F>,
    F: DualNumFloat + std::convert::From<f32> + num_dual::DualNum<F> + RealField,
{
    /// Creates a catenary curve from two points and a specified length, and an initial guess
    ///
    /// # Arguments
    ///
    /// * `p0` - The start point of the catenary curve.
    /// * `p1` - The end point of the catenary curve.
    /// * `length` - The desired length of the catenary curve.
    /// * `cat0` - The initial catenary curve to use as a starting point.
    ///
    /// # Returns
    ///
    /// An [`Option`] containing the catenary curve if it could be created, or `None` otherwise.
    #[must_use]
    pub fn from_points_length_init(
        p0: &P2<F>,
        p1: &P2<F>,
        length: F,
        cat0: &Catenary<F, F>,
    ) -> Option<Catenary<F, F>> {
        let dist = (p0 - p1).norm();

        if length < dist {
            println!("Length: {length} < Distance: {dist}");
            return None;
        }
        if p0.x == p1.x {
            let a = F::zero();
            let c = p0.x;
            let h = (p0.y + p1.y - length) / <F as From<f32>>::from(2.0);
            let s_0 = -(p0.y - h);
            let s_1 = p1.y - h;
            return Some(Catenary {
                a,
                c,
                h,
                s_0,
                s_1,
                _f: std::marker::PhantomData,
            });
        }
        let mut problem = SolveCatenary::new_best(p0, p1, length);
        problem.set_catenary(*cat0);

        let (problem, report) = LevenbergMarquardt::<F>::new()
            .with_stepbound(0.000_001.into())
            .with_xtol(1e-12.into())
            // .with_ftol(1e-8)
            // .with_gtol(1e-12)
            .minimize(problem);
        if problem.catenary().a.re() < 0.0.into() {
            // println!("{report:?}");
            problem.catenary().a = -problem.catenary().a;
            problem.catenary().h = p0.y + p1.y - problem.catenary().h;
            problem.catenary().c = p0.x + p1.x - problem.catenary().c;
        }
        // problem.catenary.s_0 = problem.catenary.s_from_x(problem.p0.x);
        // problem.catenary.s_1 = problem.catenary.s_from_x(problem.p1.x);
        if report.objective_function > 1e-6.into() {
            // println!("####BAD CONV####\np0: {:?}; p1:{:?}", p0, p1);
            // println!("init: {cat0:?}");
            // let fp = problem.catenary().end_points();
            // println!("final points 0: {},{}", fp.0.x, fp.0.y);
            // println!("final points 1: {},{}", fp.1.x, fp.1.y);
            // println!("final length: {:?}", problem.catenary().length());
            // println!("report: {report:?}");
            // println!("solved: {:?}", problem.catenary());
            // let j = problem.jacobian();
            // println!("jacobian: {j:#?}");
            return None;
        }
        Some(problem.catenary())
        // Some(problem.catenary)
    }

    /// Creates a catenary curve from two points and a specified length.
    /// (same as [`from_points_length_init`](#method.from_points_length_init) but without the initial guess)
    ///
    /// # Arguments
    ///
    /// * `p0` - The start point of the catenary curve.
    /// * `p1` - The end point of the catenary curve.
    /// * `length` - The desired length of the catenary curve.
    ///
    /// # Returns
    ///
    /// An `Option` containing the catenary curve if it could be created, or `None` otherwise.
    #[must_use]
    pub fn from_points_length(p0: &P2<F>, p1: &P2<F>, length: F) -> Option<Catenary<F, F>> {
        Self::from_points_length_init(p0, p1, length, &SolveCatenary::best_init(p0, p1, length))
    }

    #[must_use]
    /// Creates a (almost line) Catenary from a line segment defined by two points (degenerate case with big a, or 0 if vertical)
    pub fn from_segment(p0: &P2<F>, p1: &P2<F>) -> Catenary<F, F> {
        if p0.x == p1.x {
            // let h = (p0.y + p1.y) / 2.0;
            return Catenary {
                a: 0.0.into(),
                c: p0.x,
                h: p0.y,
                s_0: 0.0.into(),
                s_1: p1.y - p0.y,
                _f: std::marker::PhantomData,
            };
        }
        let slope = (p1.y - p0.y) / (p1.x - p0.x);
        let a: F = 1000.0.into();
        // f'=sh((x-c)/a); f'(px)=sh((px-c)/a)=slope; c=px-a*ash(slope)
        let c = p0.x - a * ComplexField::asinh(slope);
        // f(px)=a*ch((px-c)/a)-a+h=p0.y
        // h=p0.y-a*ch((px-c)/a)+a
        let h = p0.y - a * ComplexField::cosh((p0.x - c) / a) + a;
        let s_0 = a * ComplexField::sinh((p0.x - c) / a);
        let s_1 = a * ComplexField::sinh((p1.x - c) / a);
        Catenary {
            a,
            c,
            h,
            s_0,
            s_1,
            _f: std::marker::PhantomData,
        }
    }

    #[must_use]
    /// Creates a Catenary from a line segment defined by two points and a optimize with the length.
    /// To use when the catenary is almost a straight line. The optimization starts from a straight line catenary.
    pub fn from_segment_length(p0: &P2<F>, p1: &P2<F>, length: F) -> Option<Catenary<F, F>> {
        let cat0 = Catenary::<D, F>::from_segment(p0, p1);
        Self::from_points_length_init(p0, p1, length, &cat0)
    }
}

#[cfg(test)]
mod tests {
    use core::panic;

    use crate::{roots::Roots, CatMaker};

    use super::*;
    use approx::assert_relative_eq;
    use contourable::Contour;
    use nalgebra::ComplexField;
    use rand::Rng;
    #[test]
    fn catenary_from_points_length() {
        let catenary = CatMaker::a(1.1).c(2.2).h(3.3).s_0(-4.4).s_1(5.5);
        let (p0, p1) = catenary.end_points();

        let solved = Catenary::<f64, f64>::from_points_length(&p0, &p1, catenary.length()).unwrap();

        assert_relative_eq!(catenary.a, solved.a, epsilon = 1e-5);
        assert_relative_eq!(catenary.c, solved.c, epsilon = 1e-5);
        assert_relative_eq!(catenary.h, solved.h, epsilon = 1e-5);
    }
    #[test]
    fn catenary_from_points_length_p_1_0() {
        let catenary = CatMaker::a(1.1).c(2.2).h(3.3).s_0(-4.4).s_1(5.5);
        let (p0, p1) = catenary.end_points();

        let solved = Catenary64::from_points_length(&p1, &p0, catenary.length()).unwrap();

        assert_relative_eq!(catenary.a, solved.a, epsilon = 1e-5);
        assert_relative_eq!(catenary.c, solved.c, epsilon = 1e-5);
        assert_relative_eq!(catenary.h, solved.h, epsilon = 1e-5);
    }

    #[test]
    fn catenary_from_points_length_points_right() {
        let catenary = CatMaker::a(1.1).c(2.2).h(3.3).s_0(4.4).s_1(5.5);
        let (p0, p1) = catenary.end_points();

        let solved = Catenary64::from_points_length(&p0, &p1, catenary.length()).unwrap();

        assert_relative_eq!(catenary.a, solved.a, max_relative = 1e-2);
        assert_relative_eq!(catenary.c, solved.c, max_relative = 1e-2);
        assert_relative_eq!(catenary.h, solved.h, max_relative = 1e-2);
    }
    #[test]
    fn catenary_from_points_length_points_left() {
        let catenary = CatMaker::a(1.1).c(2.2).h(3.3).s_0(-4.4).s_1(-5.5);
        let (p0, p1) = catenary.end_points();

        let solved = Catenary64::from_points_length(&p0, &p1, catenary.length()).unwrap();

        assert_relative_eq!(catenary.a, solved.a, max_relative = 1e-2);
        assert_relative_eq!(catenary.c, solved.c, max_relative = 1e-2);
        assert_relative_eq!(catenary.h, solved.h, max_relative = 1e-2);
    }
    #[test]
    fn catenary_from_points_length_a0_p_0_1() {
        let p0 = P2::new(-1.123, 1.1);
        let p1 = P2::new(-1.123, 2.1);

        let solved = Catenary64::from_points_length(&p0, &p1, 1.0).unwrap();

        assert_relative_eq!(solved.a, 0.0, max_relative = 1e-2);
        assert_relative_eq!(solved.c, -1.123, max_relative = 1e-2);
        assert_relative_eq!(solved.h, 1.1, max_relative = 1e-2);
    }
    #[test]
    fn catenary_from_points_length_a0_p_1_0() {
        let p0 = P2::new(-1.123, 1.1);
        let p1 = P2::new(-1.123, 2.1);

        let solved = Catenary64::from_points_length(&p1, &p0, 1.0).unwrap();

        assert_relative_eq!(solved.a, 0.0, epsilon = 1e-2);
        assert_relative_eq!(solved.c, -1.123, max_relative = 1e-2);
        assert_relative_eq!(solved.h, 1.1, max_relative = 1e-2);
    }
    #[test]
    fn catenary_from_points_length_a0_big_length() {
        let p0 = P2::new(-1.123, 1.1);
        let p1 = P2::new(-1.123, 2.1);

        let solved = Catenary64::from_points_length(&p1, &p0, 3.0).unwrap();

        assert_relative_eq!(solved.a, 0.0, max_relative = 1e-2);
        assert_relative_eq!(solved.c, -1.123, max_relative = 1e-2);
        assert_relative_eq!(solved.h, 0.1, max_relative = 1e-2);
    }
    #[test]
    fn catenary_from_segment() {
        let catenary = CatMaker::a(50.0).c(0.0).h(0.0).s_0(90.0).s_1(90.1);
        let (p0, p1) = catenary.end_points();
        let cat = Catenary64::from_segment_length(&p0, &p1, catenary.length()).unwrap();
        let (q0, q1) = cat.end_points();
        assert_relative_eq!(cat.length(), catenary.length(), epsilon = 1e-9);
        assert_relative_eq!((q0 - q1).norm(), (p0 - p1).norm(), max_relative = 1e-6);
    }
    #[test]
    fn catenary_from_segment_vertical() {
        let (p0, p1) = (P2::new(0.0, 0.0), P2::new(0.0, 1.0));
        let cat = Catenary64::from_segment(&p0, &p1);
        let cat0 = CatMaker::a(0.0).c(0.0).h(0.0).s_0(0.0).s_1(1.0);
        assert_relative_eq!(cat.a, cat0.a, epsilon = 1e-6);
        assert_relative_eq!(cat.c, cat0.c, epsilon = 1e-6);
        assert_relative_eq!(cat.h, cat0.h, epsilon = 1e-6);
        assert_relative_eq!(cat.s_0, cat0.s_0, epsilon = 1e-6);
        assert_relative_eq!(cat.s_1, cat0.s_1, epsilon = 1e-6);
    }
    #[test]
    fn catenary_random_n() {
        let n = 100;
        let mut rng = rand::rng();
        for _ in 0..n {
            let catenary = CatMaker::a(rng.random_range(0.0..2.0))
                .c(rng.random_range(-10.0..10.0))
                .h(rng.random_range(-10.0..10.0))
                .s_0(rng.random_range(-10.0..10.0))
                .s_1(rng.random_range(-10.0..10.0));
            let (p0, p1) = catenary.end_points();
            let solved = Catenary64::from_points_length(&p0, &p1, catenary.length());
            if solved.is_none() {
                println!("{catenary:?}",);
                panic!("Failed to solve catenary");
            }
            let solved = solved.unwrap();
            assert_relative_eq!(solved.length(), catenary.length(), max_relative = 1e-4);
            let (q0, q1) = solved.end_points();
            let (p0, p1) = catenary.end_points();
            assert_relative_eq!((q0 - q1).norm(), (p0 - p1).norm(), max_relative = 1e-6);
        }
    }

    #[test]
    fn catenary_contour_position() {
        let catenary = CatMaker::a(1.1).c(2.2).h(3.3).s_0(4.4).s_1(5.5);
        assert_relative_eq!(catenary.position(&0.0).x, 2.2);
        assert_relative_eq!(catenary.position(&0.0).y, 3.3);
        let (p0, p1) = catenary.end_points();
        assert_relative_eq!(catenary.position(&4.4).x, p0.x);
        assert_relative_eq!(catenary.position(&4.4).y, p0.y);
        assert_relative_eq!(catenary.position(&5.5).x, p1.x);
        assert_relative_eq!(catenary.position(&5.5).y, p1.y);
    }

    #[test]
    fn test_cat_x_from_y() {
        // y=ch(x)-1
        let cat = CatMaker::a(1.0).c(0.0).h(0.0).s_0(-1.0).s_1(1.0);
        let y = 0.5;
        if let Roots::Two(a, b) = cat.x_from_y(&y) {
            assert_relative_eq!(a, -(0.5 + 1.0).acosh(), epsilon = 1e-6);
            assert_relative_eq!(b, (0.5 + 1.0).acosh(), epsilon = 1e-6);
        }
    }

    #[test]
    fn test_from_points_length_init_none() {
        let p0 = P2::new(0.0, 0.0);
        let p1 = P2::new(1.0, 1.0);
        let l = 0.5; //length too short
        let cat0 = CatMaker::a(0.0).c(0.0).h(0.0).s_0(0.0).s_1(0.0);
        let cat = Catenary64::from_points_length_init(&p0, &p1, l, &cat0);
        assert!(cat.is_none());
    }

    #[test]
    fn s_from_x() {
        let cat = CatMaker::a(1.1).c(2.2).h(3.3).s_0_from_x(-4.4).s_1(5.5);
        let x = 6.6;
        let n = 1000;
        let length = (0..=n)
            .map(|i| i as f32 / n as f32)
            .map(|p| cat.c + p * (x - cat.c))
            .map(|x| (x, cat.y_from_x(&x)))
            .map_windows(|[p0, p1]| {
                let dx = p1.0 - p0.0;
                let dy = p1.1 - p0.1;
                (dx * dx + dy * dy).sqrt()
            })
            .sum::<f32>();
        let s = cat.s_from_x(&x);
        assert_relative_eq!(s, length);
    }
}