#![allow(clippy::eq_op)]

use math_utils::*;
use either::Either;

#[expect(unused_macros)]
macro_rules! show {
  ($e:expr) => { println!("{}:\n{:?}", stringify!($e), $e); }
}

macro_rules! display {
  ($e:expr) => { println!("{}:\n{}", stringify!($e), $e); }
}

#[expect(unused_variables)]
fn main() {
  use std::f32::consts::FRAC_PI_2;
  use num::{One, Zero};

  report_sizes();

  //
  //  Scalars
  //
  let s = 2.0f32;
  const _N : NonNegative <f32> = NonNegative::noisy_f32 (2.0);
  let zero = NonNegative::<f32>::zero();
  let one = NonNegative::<f32>::one();
  let n = NonNegative::new (s).unwrap();
  let n = NonNegative::noisy (s);
  let n = NonNegative::unchecked (s);
  let n1 = NonNegative::abs (-2.0f32);
  assert_eq!(n, n1);
  const _P : Positive <f32> = Positive::noisy_f32 (2.0);
  let p = Positive::new (s).unwrap();
  let p = Positive::noisy (s);
  let p = Positive::unchecked (s);
  let p = Positive::<f32>::infinity();
  let z = NonZero::new (-1.0).unwrap();
  let z = NonZero::noisy (-1.0);
  const _Z : NonZero <f32> = NonZero::noisy_f32 (1.0);
  const _U : Normalized <f32> = Normalized::noisy_f32 (1.0);
  let zero = Normalized::<f32>::zero();
  let one = Normalized::one();
  let u = Normalized::new (1.0).unwrap();
  let u = Normalized::noisy (1.0);
  let u = Normalized::clamp (2.0);
  assert_eq!(u, one);
  const _W : NormalSigned <f32> = NormalSigned::noisy_f32 (1.0);
  let zero = NormalSigned::<f32>::zero();
  let one = NormalSigned::one();
  let w = NormalSigned::new (1.0).unwrap();
  let w = NormalSigned::noisy (1.0);
  let w = NormalSigned::clamp (2.0);
  assert_eq!(w, one);
  let a = Deg (s);
  let a : Deg <f32> = s.into();
  let a = Turn (s);
  let a : Turn <f32> = s.into();
  let a = Rad (s);
  let a : Rad <f32> = s.into();
  let w = AngleWrapped::wrap (a);
  assert_eq!(w.angle(), a);
  let w = AngleWrappedSigned::wrap (a);
  assert_eq!(w.angle(), a);

  //
  //  Vectors & Points
  //
  // construction
  let zero = Vector3::<f32>::zero();
  let one = Vector3::one();
  let v1 = Vector3::broadcast (1.0);
  let v2 = Vector3::from (1.0);
  assert_eq!(one, v1);
  assert_eq!(one, v2);
  let vx = Vector3::<f32>::unit_x();
  let vy = Vector3::<f32>::unit_y();
  let vz = Vector3::<f32>::unit_z();
  let iota = Vector3::iota();
  let v = vector!(0.0, 1.0);
  let v = vector!(0.0, 1.0, 2.0);
  let v = vector!(0.0, 1.0, 2.0, 3.0);
  let v = vector3 (0.0, 1.0, 2.0);
  assert_eq!(iota, v);
  let v = Vector3::new (1.0, 2.0, 3.0);
  let v = Vector3::<f32>::from ([1.0, 2.0, 3.0]);
  let v : Vector3 <f32> = [1.0, 2.0, 3.0].into();
  let v : Vector3 <f32> = (1.0, 2.0, 3.0).into();
  let v = Vector3::from_slice (&[1.0, 2.0, 3.0]);
  let v = Vector3::from_iter ((1..=3).map (|i| i as f32));
  let v = Vector3::<f32>::from (vector2 (1.0, 2.0));
  assert_eq!(v, vector3 (1.0, 2.0, 0.0));
  let v = Vector3::<f32>::from ((vector2 (1.0, 2.0), 3.0));
  let p2 = Vector3::new_point_2d (1.0, 2.0);
  assert_eq!(p2, vector3 (1.0, 2.0, 1.0));
  let v2 = Vector3::new_direction_2d (1.0, 2.0);
  assert_eq!(v2, vector3 (1.0, 2.0, 0.0));
  let origin = Point3::<f32>::origin();
  let p = Point3 (v);
  let p = Point3::from (v);
  let p = Point3::from_vector (v);
  let v = Point3::to_vector (p);
  let p : Point3 <f32> = v.into();
  let p = point3 (1.0, 2.0, 3.0);
  let p = Point3::new (1.0, 2.0, 3.0);
  let p = Point3::from ([1.0, 2.0, 3.0]);
  let p : Point3 <f32> = [1.0, 2.0, 3.0].into();
  let ux = Unit3::<f32>::axis_x();
  let uy = Unit3::<f32>::axis_y();
  let uz = Unit3::<f32>::axis_z();
  let u = Unit3::new (vector3 (1.0, 0.0, 0.0)).unwrap();
  let u1= Unit3::new_approx (v.normalized()).unwrap();
  let u2 = Unit3::normalize (v);
  let u3 = Unit3::normalize_approx (v);
  assert_eq!(u1, u2);
  assert_eq!(u1, u3);
  let u = Unit3::noisy (vector3 (1.0, 0.0, 0.0));
  let u = Unit3::unchecked (vector3 (1.0, 0.0, 0.0));
  let u = Unit3::unchecked_approx (vector3 (1.0, 0.0, 0.0));
  let n = NonZero3::new (v).unwrap();
  let n = NonZero3::noisy (v);
  // access
  let x = v.x;
  let x = v[0];
  let x = p.0.x;
  let x = p.x();
  // operations
  let p = point3 (1.0, 2.0, 3.0);
  let v = vector3 (1.0, 2.0, 3.0);
  let p2 : Point3 <f32>  = p + v;
  let v2 : Vector3 <f32> = p2 - p;
  assert_eq!(v2, v);
  display!(v);
  display!(v + v);
  display!(v - v);
  display!(v * v);
  display!(v / v);
  display!(v.dot (v));
  display!(v.norm_squared());
  display!(v.magnitude_squared());
  display!(v.norm());
  display!(v.magnitude());
  display!(v.cross (vector3 (3.0, 2.0, 1.0)));
  assert_eq!(v.normalized(), v / *v.norm());

  //
  //  Matrices
  //
  // construction
  let zero = Matrix3::<f32>::zero();
  let id = Matrix3::<f32>::identity();
  let scaling = Matrix3::<f32>::broadcast_diagonal (2.0);
  let diagonal = Matrix3::<f32>::with_diagonal ([2.0, 2.0, 2.0].into());
  assert_eq!(scaling, Matrix3::scaling_3d (2.0));
  assert_eq!(scaling, diagonal);
  let m = Matrix3::<f32> {
    cols: [
      [1.0, 4.0, 7.0],
      [2.0, 5.0, 8.0],
      [3.0, 6.0, 9.0],
    ].map (Vector3::from).into()
  };
  let m1 = Matrix3::<f32>::new (
    1.0, 2.0, 3.0,
    4.0, 5.0, 6.0,
    7.0, 8.0, 9.0);
  let m2 = Matrix3::<f32>::from_col_array (
    [ 1.0, 4.0, 7.0,
      2.0, 5.0, 8.0,
      3.0, 6.0, 9.0 ]);
  let m3 = Matrix3::<f32>::from_col_arrays (
    [ [1.0, 4.0, 7.0],
      [2.0, 5.0, 8.0],
      [3.0, 6.0, 9.0] ]);
  let m4 = Matrix3::<f32>::from_row_array (
    [ 1.0, 2.0, 3.0,
      4.0, 5.0, 6.0,
      7.0, 8.0, 9.0 ]);
  let m5 = Matrix3::<f32>::from_row_arrays (
    [ [1.0, 2.0, 3.0],
      [4.0, 5.0, 6.0],
      [7.0, 8.0, 9.0] ]);
  let m6 = matrix![
    1.0, 2.0, 3.0;
    4.0, 5.0, 6.0;
    7.0, 8.0, 9.0;
  ];
  assert_eq!(m, m1);
  assert_eq!(m, m2);
  assert_eq!(m, m3);
  assert_eq!(m, m4);
  assert_eq!(m, m5);
  assert_eq!(m, m6);
  let m = Matrix3::scaling_3d (2.0);
  let m = Matrix3::rotation_x (FRAC_PI_2);
  let m = Matrix3::rotation_y (FRAC_PI_2);
  let m = Matrix3::rotation_z (FRAC_PI_2);
  let m1 = Matrix3::rotation_3d (FRAC_PI_2, Vector3::unit_z());
  let m2 = Matrix3::<f32>::rotation_from_to_3d (Vector3::unit_x(), Vector3::unit_y());
  approx::assert_relative_eq!(m, m1);
  approx::assert_relative_eq!(m, m2);
  let m = Matrix3::<f32>::translation_2d ([1.0, 2.0]);
  // access
  let s = m[(0,0)];
  let s1 = m.cols.x.x;
  let s2 = m.as_col_slice()[0];
  let s3 = m.rows().x.x;
  assert_eq!(s, s1);
  assert_eq!(s, s2);
  assert_eq!(s, s3);
  // transformations
  let v = vector3 (1.0, 2.0, 3.0);
  let m = Matrix3::rotation_z (FRAC_PI_2);
  display!(m);
  display!(v);
  display!(m * v);
  display!(v * m);
  let v1 = m * v;
  let v2 = (0..=2).map (|i| m.cols[i] * v[i]).sum();
  let v3 = std::array::from_fn (|i| m.rows()[i].dot (v)).into();
  assert_eq!(v1, v2);
  assert_eq!(v1, v3);
  let v4 = v * m;
  let v5 = (0..=2).map (|i| m.rows()[i] * v[i]).sum();
  let v6 = std::array::from_fn (|i| m.cols[i].dot (v)).into();
  assert_eq!(v4, v5);
  assert_eq!(v4, v6);

  //
  //  Isomorphisms & Automorphisms
  //
  let m = Matrix3::rotation_z (FRAC_PI_2);
  let t = LinearIso::new (m).unwrap();
  let t = LinearIso::new_approx (m, None).unwrap();
  let tt = LinearAuto (t);
  let v = vector3 (1.0, 2.0, 3.0);
  let v1 = t * v;
  let v2 = tt * v;

  //
  //  Rotations
  //
  let zero = Angles3::<f32>::zero();
  let a = AngleWrapped::wrap (2.0.into());
  let aa = Angles3 {
    yaw:   a,
    pitch: a,
    roll:  a
  };
  let aa = Angles3::new (a, a, a);
  let aa = Angles3::wrap (4.0.into(), 4.0.into(), 4.0.into());
  let r = Rotation3::from (aa);
  let a = Angles3::from (r);
  let id = Rotation3::<f32>::identity();
  assert_eq!(id, Angles3::zero().into());
  let m = Matrix3::rotation_z (FRAC_PI_2);
  let r = Rotation3::new (m);
  let r = Rotation3::new_approx (m);
  let r = Rotation3::noisy (m);
  let r = Rotation3::noisy_approx (m);
  let r = Rotation3::unchecked (m);
  let r = Rotation3::unchecked_approx (m);
  let r = Rotation3::from_angle_x (FRAC_PI_2.into());
  let r = Rotation3::from_angle_y (FRAC_PI_2.into());
  let r = Rotation3::from_angle_z (FRAC_PI_2.into());
  let r2 = Rotation3::from_angles_intrinsic (FRAC_PI_2.into(), 0.0.into(), 0.0.into());
  let r3 = Rotation3::from_axis_angle ([0.0, 0.0, 1.0].into(), FRAC_PI_2.into());
  assert_eq!(r, r2);
  approx::assert_relative_eq!(r.mat(), r3.mat());
  let eye1 = Rotation3::look_at ([0.0, 1.0, 0.0].into());
  approx::assert_relative_eq!(eye1.mat(), id.mat());
  let eye2 = Rotation3::look_at ([-1.0, 0.0, 0.0].into());
  approx::assert_relative_eq!(eye2.mat(), r.mat());
  let r = Rotation3::<f32>::orthonormalize (
    [1.0, 0.1, 0.0].into(),
    [0.0, 1.0, 0.1].into(),
    [0.1, 0.0, 1.0].into()
  ).unwrap();
  let v = vector3 (1.0, 2.0, 3.0);
  let v1 = r * v;
  let p = point3 (1.0, 2.0, 3.0);
  let p1 = r.rotate (p);

  //
  //  Affinities
  //
  let r = Rotation3::from_angle_z (FRAC_PI_2.into());
  let v = vector3 (1.0, 2.0, 3.0);
  let a = AffineMap {
    linear_map: r.mat(),
    translation: v,
    _phantom: Default::default()
  };
  let p = point3 (1.0, 2.0, 3.0);
  let p1 : Point3 <f32> = a.map (p);
  let v1 : Vector3 <f32> = a.transform (v);
  let a = AffineMap::new (m, v);
  let p2 : Point3 <f32> = a.map (p);
  let v2 : Vector3 <f32> = a.transform (v);
  let t : LinearIso <_, _, _, _> = **r;
  let a = Affinity::<_, Point3 <_>, Point3 <_>, _> {
    linear_iso: t,
    translation: v
  };
  let a = Affinity::new (t, v);
  let h = Projectivity::new (t);
  assert_eq!(h.mat().cols.len(), 3);
  let p3 : Point3 <f32> = h * p1;
  let h1 = Point4::homography (a);
  assert_eq!(h1.mat().cols.len(), 4);
  let p = point3 (1.0, 2.0, 3.0);
  let p4 = Point4::homogeneous (Either::Left (p));
  assert_eq!(p4.0.len(), 4);
  let v4 = Point4::homogeneous (Either::Right (v));
  assert_eq!(v4.0.len(), 4);
  let p5 = h1 * p4;

  //
  //  Quaternions & Versors
  //
  let zero = Quaternion::<f32>::zero();
  let id = Quaternion::<f32>::identity();
  let q = Quaternion::from_xyzw (1.0, 2.0, 3.0, 4.0);
  let q = Quaternion::from_scalar_and_vec3 ((1.0, vector3 (2.0, 3.0, 4.0)));
  let q = Quaternion::<f32>::rotation_from_to_3d ([1.0, 2.0, 3.0], [3.0, 2.0, 1.0]);
  let q = Quaternion::rotation_3d (1.0, [1.0, 2.0, 3.0]);
  let q = Quaternion::rotation_x (2.0);
  let q = Quaternion::rotation_y (2.0);
  let q = Quaternion::rotation_z (2.0);
  let q = Quaternion::from_vec4 (vector4 (1.0, 2.0, 3.0, 4.0));
  let q = Quaternion::from (vector4 (1.0, 2.0, 3.0, 4.0));
  let q : Quaternion <f32> = vector4 (1.0, 2.0, 3.0, 4.0).into();
  let v = vector3 (1.0, 2.0, 3.0);
  let v1 = q * v;
  let v = vector4 (1.0, 2.0, 3.0, 4.0);
  let v2 = q * v;
  let q1 = q * q;
  let s = Versor::normalize (q);
  let s = Versor::noisy (s.normalized());
  let s = Versor::noisy_approx (*s);
  let s = Versor::unchecked (*s);
  let s = Versor::unchecked_approx (*s);
  let s = Versor::from_scaled_axis (vector3 (2.0, 2.0, 2.0));
  let r = Rotation3::from_angle_z (FRAC_PI_2.into());
  let s = r.versor();
  let s = Versor::from (r);
  let s : Versor <f32> = r.into();
  let v = vector3 (1.0, 2.0, 3.0);
  let v1 = s * v;
}
