#[cfg(test)]
mod tests {
    use fts_units::ops::*;
    use fts_units::quantity::*;
    use fts_units::si_system::quantities::traits::{Acceleration, Length, Velocity};
    use fts_units::si_system::quantities::*;
    //use fts_vecmath::angle::*;
    //use fts_vecmath::scalar::*;
    //use fts_vecmath::vector3::*;
    use std::ops::{Add, Div, Mul};

    type Pure<T> = fts_units::si_system::quantities::Dimensionless<T>;

    // $$$FTS - Don't want num
    fn calc_ballistic_range2<T, V, A, H>(
        speed: V,
        gravity: A,
        initial_height: H,
    ) -> impl Length<AmountType = T>
    where
        T: Amount
            + num_traits::Num
            + num_traits::real::Real
            + Copy
            + PartialOrd
            + From<u8>
            + From<f32>,
        V: Copy + Velocity<AmountType = T> + Mul<Pure<T>> + Mul<V>,
        A: Copy + Acceleration<AmountType = T>,
        H: Copy + Length<AmountType = T>,
        Pure<T>: Mul<A>,
        MulOutput<V, Pure<T>>: Div<A>,
        MulOutput<V, V>: Mul<Pure<T>>,
        MulOutput3<V, V, Pure<T>>: Mul<Pure<T>>,
        MulOutput<Pure<T>, A>: Mul<H>,
        MulOutput4<V, V, Pure<T>, Pure<T>>: Add<MulOutput3<Pure<T>, A, H>>,
        AddOutput<MulOutput4<V, V, Pure<T>, Pure<T>>, MulOutput3<Pure<T>, A, H>>: Sqrt,
        SqrtOutput<AddOutput<MulOutput4<V, V, Pure<T>, Pure<T>>, MulOutput3<Pure<T>, A, H>>>:
            Add<MulOutput<V, Pure<T>>>,
        DivOutput<MulOutput<V, Pure<T>>, A>: Mul<
            AddOutput<
                SqrtOutput<
                    AddOutput<MulOutput4<V, V, Pure<T>, Pure<T>>, MulOutput3<Pure<T>, A, H>>,
                >,
                MulOutput<V, Pure<T>>,
            >,
        >,
        MulOutput<
            DivOutput<MulOutput<V, Pure<T>>, A>,
            AddOutput<
                SqrtOutput<
                    AddOutput<MulOutput4<V, V, Pure<T>, Pure<T>>, MulOutput3<Pure<T>, A, H>>,
                >,
                MulOutput<V, Pure<T>>,
            >,
        >: Length<AmountType = T>,
    {
        assert!(speed.amount() > 0.into());
        assert!(gravity.amount() > 0.into());
        assert!(initial_height.amount() > 0.into());

        let d2r = 0.01745329252;
        let angle: T = (45.0 * d2r).into();
        let cos = Pure::<T>::new(angle.cos());
        let sin = Pure::<T>::new(angle.sin());

        // speed = m/s
        // gravity = m/s^2
        let a = speed * cos / gravity; // s
        let b = speed * sin; // m/s
        let c = speed * speed * sin * sin; // m^2/s^2
        let d = Pure::<T>::new(2.into()) * gravity * initial_height; // m^2/s^2
        let e = c + d; // m^2/s^2
        let f = e.sqrt(); // m/s
        let g = f + b; // m/s
        let _h = a * g; // m
                        //h

        //let range = (speed*cos/gravity) * (speed*sin + (speed*speed*sin*sin + Pure::<f32>::new(2.0)*gravity*initial_height).sqrt());
        let range = (speed * cos / gravity)
            * ((speed * speed * sin * sin + Pure::<T>::new(2.into()) * gravity * initial_height)
                .sqrt()
                + speed * sin);
        range
    }

    fn calc_ballistic_range(
        speed: MetersPerSecond<f32>,
        gravity: MetersPerSecond2<f32>,
        initial_height: Meters<f32>,
    ) -> Meters<f32> {
        assert!(speed.amount() > 0.0);
        assert!(gravity.amount() > 0.0);
        assert!(initial_height.amount() > 0.0);

        let d2r = 0.01745329252;
        let angle: f32 = 45.0 * d2r;
        let cos = Pure::<f32>::new(angle.cos());
        let sin = Pure::<f32>::new(angle.sin());

        let range = (speed * cos / gravity)
            * (speed * sin
                + (speed * speed * sin * sin + Pure::<f32>::new(2.0) * gravity * initial_height)
                    .sqrt());
        range
    }

    #[test]
    fn ballistic_range() {
        let v = MetersPerSecond::<f32>::new(20.0);
        let a = MetersPerSecond2::<f32>::new(9.8);
        let h = Meters::<f32>::new(15.0);
        let r1 = calc_ballistic_range(v, a, h);
        let r2 = calc_ballistic_range2(v, a, h);
        assert_eq!(r1.amount(), r2.amount());

        let km = r2.convert::<fts_units::si_system::ratios::Kilo>();
        assert_eq!(km, Kilometers::<f32>::new(r2.amount() / 1000.0));
    }

    #[derive(Copy, Clone, PartialEq)]
    struct Vector3f {
        x: f32,
        y: f32,
        z: f32,
    }

    #[allow(dead_code)]
    impl Vector3f {
        fn y_axis() -> Self {
            Self {
                x: 0.0,
                y: 1.0,
                z: 0.0,
            }
        }
        fn x_z(self, v: f32) -> Self {
            Self {
                x: self.x,
                y: v,
                z: self.z,
            }
        }
        fn length(self) -> f32 {
            (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
        }
        fn normalized(self) -> Self {
            self * (1.0 / self.length())
        }
    }

    impl std::ops::Add<Vector3f> for Vector3f {
        type Output = Self;
        fn add(self, other: Self) -> Self {
            Self {
                x: self.x + other.x,
                y: self.y + other.y,
                z: self.z + other.z,
            }
        }
    }

    impl std::ops::Sub<Vector3f> for Vector3f {
        type Output = Self;
        fn sub(self, other: Self) -> Self {
            Self {
                x: self.x - other.x,
                y: self.y - other.y,
                z: self.z - other.z,
            }
        }
    }

    impl std::ops::Mul<f32> for Vector3f {
        type Output = Self;
        fn mul(self, v: f32) -> Self::Output {
            Self {
                x: self.x * v,
                y: self.y * v,
                z: self.z * v,
            }
        }
    }

    #[allow(dead_code)]
    fn solve_ballistic_arc(
        proj_pos: Vector3f,
        target_pos: Vector3f,
        proj_speed: MetersPerSecond<f32>,
        gravity: MetersPerSecond2<f32>,
    ) -> (Option<Vector3f>, Option<Vector3f>) {
        assert!(proj_pos != target_pos);
        assert!(proj_speed.amount() > 0.0);
        assert!(gravity.amount() >= 0.0);

        let diff = target_pos - proj_pos;
        let diff_xz = diff.x_z(0.0);
        let ground_dist = diff_xz.length();

        let ground_dist = Meters::<f32>::new(ground_dist); // m
        let speed2 = proj_speed * proj_speed; // m²·s⁻²
        let speed4 = speed2 * speed2; // m⁴·s⁻⁴

        let y = Meters::<f32>::new(diff.y); // m
        let x: Meters<f32> = ground_dist; // m
        let gx = gravity * x; // m²·s⁻²

        let two = Pure::<f32>::new(2.0);
        // (m⁴·s⁻⁴) - (m·s⁻²)*(m³·s⁻² + m³·s⁻²)
        // (m⁴·s⁻⁴) - (m⁴·s⁻⁴)
        let root = speed4 - gravity * (gravity * x * x + two * y * speed2);

        if root.amount() < 0.0 {
            return (None, None);
        }

        // m²·s⁻²
        let root = root.sqrt();

        let low_ang = (speed2 - root).amount().atan2(gx.amount());
        let high_ang = (speed2 + root).amount().atan2(gx.amount());
        let num_solutions = if low_ang != high_ang { 2 } else { 1 };

        let ground_dir = diff_xz.normalized();
        let up = Vector3f::y_axis();
        let proj_speed = proj_speed.amount();

        let s0 = Some(ground_dir * low_ang.cos() * proj_speed + up * low_ang.sin() * proj_speed);
        let mut s1 = None;
        if num_solutions > 1 {
            s1 = Some(ground_dir * high_ang.cos() * proj_speed + up * high_ang.sin() * proj_speed);
        }

        (s0, s1)
    }
}