Quaternions¶

use brahe as bh;
use brahe::attitude::FromAttitude;
use nalgebra as na;
use std::f64::consts::PI;

fn main() {
    // Initialize from individual components (w, x, y, z)
    // Always scalar-first in constructor
    let q1 = bh::Quaternion::new(0.924, 0.0, 0.0, 0.383);
    println!("From components (identity):");
    println!("  q = [{:.3}, {:.3}, {:.3}, {:.3}]", q1[0], q1[1], q1[2], q1[3]);

    // Initialize from vector/array [w, x, y, z]
    // Can specify if scalar is first or last
    let vec = na::SVector::<f64, 4>::new(0.924, 0.0, 0.0, 0.383);
    let q2 = bh::Quaternion::from_vector(vec, true);  // scalar_first = true
    println!("\nFrom vector:");
    println!("  q = [{:.3}, {:.3}, {:.3}, {:.3}]", q2[0], q2[1], q2[2], q2[3]);

    // Initialize from another representation (rotation matrix)
    // 45° rotation about Z-axis
    let rm = bh::RotationMatrix::Rz(45.0, bh::AngleFormat::Degrees);
    let q3 = bh::Quaternion::from_rotation_matrix(rm);
    println!("\nFrom rotation matrix (45° about Z-axis):");
    println!("  q = [{:.3}, {:.3}, {:.3}, {:.3}]", q3[0], q3[1], q3[2], q3[3]);

    // Initialize from Euler angles (ZYX sequence)
    let ea = bh::EulerAngle::new(bh::EulerAngleOrder::ZYX, PI/4.0, 0.0, 0.0, bh::AngleFormat::Radians);
    let q4 = bh::Quaternion::from_euler_angle(ea);
    println!("\nFrom Euler angles (45° yaw, ZYX):");
    println!("  q = [{:.3}, {:.3}, {:.3}, {:.3}]", q4[0], q4[1], q4[2], q4[3]);

    // Initialize from Euler axis (axis-angle representation)
    let axis = na::SVector::<f64, 3>::new(0.0, 0.0, 1.0);  // Z-axis
    let angle = PI / 4.0;  // 45°
    let ea_rep = bh::EulerAxis::new(axis, angle, bh::AngleFormat::Radians);
    let q5 = bh::Quaternion::from_euler_axis(ea_rep);
    println!("\nFrom Euler axis (45° about Z-axis):");
    println!("  q = [{:.3}, {:.3}, {:.3}, {:.3}]", q5[0], q5[1], q5[2], q5[3]);
}

// Expected output:
// From components (identity):
//   q = [0.924, 0.000, 0.000, 0.383]
//
// From vector:
//   q = [0.924, 0.000, 0.000, 0.383]
//
// From rotation matrix (45° about Z-axis):
//   q = [0.924, 0.000, 0.000, 0.383]
//
// From Euler angles (45° yaw, ZYX):
//   q = [0.924, 0.000, 0.000, 0.383]
//
// From Euler axis (45° about Z-axis):
//   q = [0.924, 0.000, 0.000, 0.383]

use brahe as bh;
use brahe::attitude::FromAttitude;

fn main() {
    // Create a quaternion (45° rotation about Z-axis)
    let q = bh::Quaternion::from_rotation_matrix(
        bh::RotationMatrix::Rz(45.0, bh::AngleFormat::Degrees)
    );

    // Access individual components
    println!("Individual components:");
    println!("  w (scalar): {:.6}", q[0]);
    println!("  x: {:.6}", q[1]);
    println!("  y: {:.6}", q[2]);
    println!("  z: {:.6}", q[3]);

    // Directly access as a vector/array
    let vec = q.to_vector(true);
    println!("\nAs vector [w, x, y, z]:");
    println!("  [{}, {}, {}, {}]", vec[0], vec[1], vec[2], vec[3]);

    // Or return copy as a vector
    let vec_np = q.to_vector(true);
    println!("\nAs vector [w, x, y, z]:");
    println!("  [{}, {}, {}, {}]", vec_np[0], vec_np[1], vec_np[2], vec_np[3]);

    // Return in different order (scalar last)
    let vec_np_last = q.to_vector(false);
    println!("\nAs scalar-last [x, y, z, w]:");
    println!("  [{}, {}, {}, {}]", vec_np_last[0], vec_np_last[1], vec_np_last[2], vec_np_last[3]);

    // Display as string (Debug)
    println!("\nString representation:");
    println!("  {}", q);

    println!("\nDebug representation:");
    println!("  {:?}", q);
}

// Expected output:
// Individual components:
//   w (scalar): 0.923880
//   x: 0.000000
//   y: 0.000000
//   z: 0.382683

// As vector :
//   [0.9238795325112867, 0, 0, 0.3826834323650897]

// As vector :
//   [0.9238795325112867, 0, 0, 0.3826834323650897]

// As scalar-last :
//   [0, 0, 0.3826834323650897, 0.9238795325112867]

// String representation:
//   Quaternion: [s: 0.9238795325112867, v: [0, 0, 0.3826834323650897]]

// Debug representation:
//   Quaternion<0.9238795325112867, 0, 0, 0.3826834323650897>

use brahe as bh;
use brahe::attitude::FromAttitude;
use std::f64::consts::PI;

fn main() {
    // Create a quaternion from rotation matrix (90° about X, then 45° about Z)
    let mut q = bh::Quaternion::from_rotation_matrix(
        bh::RotationMatrix::Rx(90.0, bh::AngleFormat::Degrees) * bh::RotationMatrix::Rz(45.0, bh::AngleFormat::Degrees)
    );

    println!("Original quaternion:");
    println!("  q = [{:.6}, {:.6}, {:.6}, {:.6}]", q[0], q[1], q[2], q[3]);

    // Compute norm
    let norm = q.norm();
    println!("\nNorm: {:.6}", norm);

    // Normalize quaternion (in-place)
    q.normalize();  // In-place normalization (This shouldn't really do anything here since q already applies normalization on creation)
    println!("After normalization:");
    println!("  q = [{:.6}, {:.6}, {:.6}, {:.6}]", q[0], q[1], q[2], q[3]);
    println!("  Norm: {:.6}", q.norm());

    // Compute conjugate
    let q_conj = q.conjugate();
    println!("\nConjugate:");
    println!("  q* = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_conj[0], q_conj[1], q_conj[2], q_conj[3]);

    // Compute inverse (same as conjugate for normalized quaternions)
    let q_inv = q.inverse();
    println!("\nInverse:");
    println!("  q^-1 = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_inv[0], q_inv[1], q_inv[2], q_inv[3]);

    // Quaternion multiplication (compose rotations)
    // 90° about X, then 90° about Z
    let q_x = bh::Quaternion::new((PI/4.0).cos(), (PI/4.0).sin(), 0.0, 0.0);
    let q_z = bh::Quaternion::new((PI/4.0).cos(), 0.0, 0.0, (PI/4.0).sin());
    let q_composed = q_z * q_x;  // Apply q_x first, then q_z
    println!("\nComposed rotation (90° X then 90° Z):");
    println!("  q_x = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_x[0], q_x[1], q_x[2], q_x[3]);
    println!("  q_z = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_z[0], q_z[1], q_z[2], q_z[3]);
    println!("  q_composed = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_composed[0], q_composed[1], q_composed[2], q_composed[3]);

    // Multiply q and its inverse to verify identity
    let identity = q * q_inv;
    println!("\nq * q^-1 (should be identity):");
    println!("  q_identity = [{:.6}, {:.6}, {:.6}, {:.6}]",
             identity[0], identity[1], identity[2], identity[3]);

    // SLERP (Spherical Linear Interpolation) between two quaternions
    // Interpolate from q_x (90° about X) to q_z (90° about Z)
    println!("\nSLERP interpolation from q_x to q_z:");
    let q_slerp_0 = q_x.slerp(q_z, 0.0);  // t=0, should equal q_x
    println!("  t=0.0: [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_slerp_0[0], q_slerp_0[1], q_slerp_0[2], q_slerp_0[3]);
    let q_slerp_25 = q_x.slerp(q_z, 0.25);
    println!("  t=0.25: [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_slerp_25[0], q_slerp_25[1], q_slerp_25[2], q_slerp_25[3]);
    let q_slerp_5 = q_x.slerp(q_z, 0.5);  // t=0.5, halfway
    println!("  t=0.5: [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_slerp_5[0], q_slerp_5[1], q_slerp_5[2], q_slerp_5[3]);
    let q_slerp_75 = q_x.slerp(q_z, 0.75);
    println!("  t=0.75: [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_slerp_75[0], q_slerp_75[1], q_slerp_75[2], q_slerp_75[3]);
    let q_slerp_1 = q_x.slerp(q_z, 1.0);  // t=1, should equal q_z
    println!("  t=1.0: [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_slerp_1[0], q_slerp_1[1], q_slerp_1[2], q_slerp_1[3]);
}

// Expected output:
// Original quaternion:
//   q = [0.653281, 0.653281, 0.270598, 0.270598]
//
// Norm: 1.000000
// After normalization:
//   q = [0.653281, 0.653281, 0.270598, 0.270598]
//   Norm: 1.000000
//
// Conjugate:
//   q* = [0.653281, -0.653281, -0.270598, -0.270598]
//
// Inverse:
//   q^-1 = [0.653281, -0.653281, -0.270598, -0.270598]
//
// Composed rotation (90° X then 90° Z):
//   q_x = [0.707107, 0.707107, 0.000000, 0.000000]
//   q_z = [0.707107, 0.000000, 0.000000, 0.707107]
//   q_composed = [0.500000, 0.500000, 0.500000, 0.500000]
//
// q * q^-1 (should be identity):
//   q_identity = [1.000000, 0.000000, 0.000000, 0.000000]
//
// SLERP interpolation from q_x to q_z:
//   t=0.0: [0.707107, 0.707107, 0.000000, 0.000000]
//   t=0.25: [0.788675, 0.577350, 0.000000, 0.211325]
//   t=0.5: [0.816497, 0.408248, 0.000000, 0.408248]
//   t=0.75: [0.788675, 0.211325, 0.000000, 0.577350]
//   t=1.0: [0.707107, 0.000000, 0.000000, 0.707107]

use brahe as bh;
use brahe::attitude::FromAttitude;
use brahe::attitude::ToAttitude;
use std::f64::consts::PI;

fn main() {
    // Create a quaternion (45° rotation about Z-axis)
    let q = bh::Quaternion::new((PI/8.0).cos(), 0.0, 0.0, (PI/8.0).sin());

    println!("Original quaternion:");
    println!("  q = [{:.6}, {:.6}, {:.6}, {:.6}]", q[0], q[1], q[2], q[3]);

    // Convert to rotation matrix
    let rm = q.to_rotation_matrix();
    println!("\nTo rotation matrix:");
    println!("  [{:.6}, {:.6}, {:.6}]", rm[(0, 0)], rm[(0, 1)], rm[(0, 2)]);
    println!("  [{:.6}, {:.6}, {:.6}]", rm[(1, 0)], rm[(1, 1)], rm[(1, 2)]);
    println!("  [{:.6}, {:.6}, {:.6}]", rm[(2, 0)], rm[(2, 1)], rm[(2, 2)]);

    // Convert to Euler angles (ZYX sequence)
    let ea_zyx = q.to_euler_angle(bh::EulerAngleOrder::ZYX);
    println!("\nTo Euler angles (ZYX):");
    println!("  Yaw (Z):   {:.3}°", ea_zyx.phi.to_degrees());
    println!("  Pitch (Y): {:.3}°", ea_zyx.theta.to_degrees());
    println!("  Roll (X):  {:.3}°", ea_zyx.psi.to_degrees());

    // Convert to Euler angles (XYZ sequence)
    let ea_xyz = q.to_euler_angle(bh::EulerAngleOrder::XYZ);
    println!("\nTo Euler angles (XYZ):");
    println!("  Angle 1 (X): {:.3}°", ea_xyz.phi.to_degrees());
    println!("  Angle 2 (Y): {:.3}°", ea_xyz.theta.to_degrees());
    println!("  Angle 3 (Z): {:.3}°", ea_xyz.psi.to_degrees());

    // Convert to Euler axis (axis-angle)
    let ea = q.to_euler_axis();
    println!("\nTo Euler axis:");
    println!("  Axis: [{:.6}, {:.6}, {:.6}]", ea.axis[0], ea.axis[1], ea.axis[2]);
    println!("  Angle: {:.3}°", ea.angle.to_degrees());

    // Round-trip conversion test
    let q_roundtrip = bh::Quaternion::from_rotation_matrix(rm);
    println!("\nRound-trip (Quaternion → RotationMatrix → Quaternion):");
    println!("  q_rt = [{:.6}, {:.6}, {:.6}, {:.6}]",
             q_roundtrip[0], q_roundtrip[1], q_roundtrip[2], q_roundtrip[3]);
}

// Expected output:
// Original quaternion:
//   q = [0.923880, 0.000000, 0.000000, 0.382683]

// To rotation matrix:
//   [0.707107, 0.707107, 0.000000]
//   [-0.707107, 0.707107, 0.000000]
//   [0.000000, 0.000000, 1.000000]

// To Euler angles (ZYX):
//   Yaw (Z):   45.000°
//   Pitch (Y): 0.000°
//   Roll (X):  -0.000°

// To Euler angles (XYZ):
//   Angle 1 (X): 0.000°
//   Angle 2 (Y): -0.000°
//   Angle 3 (Z): 45.000°

// To Euler axis:
//   Axis: [0.000000, 0.000000, 1.000000]
//   Angle: 45.000°

// Round-trip (Quaternion → RotationMatrix → Quaternion):
//   q_rt = [0.923880, 0.000000, 0.000000, 0.382683]