use nalgebra::{DMatrix, DVector};

use dyna3::engine::{Q, examples::realize_kaleidocycle};

fn main() {
    const SCALED_TOL: f64 = 1.0e-12;
    let (config, tangent, success, history) = realize_kaleidocycle(SCALED_TOL);
    print!("Completed Gram matrix:{}", config.tr_mul(&*Q) * &config);
    print!("Configuration:{}", config);
    if success {
        println!("Target accuracy achieved!");
    } else {
        println!("Failed to reach target accuracy");
    }
    println!("Steps: {}", history.scaled_loss.len() - 1);
    println!("Loss: {}\n", history.scaled_loss.last().unwrap());
    
    // find the kaleidocycle's twist motion by projecting onto the tangent space
    const N_POINTS: usize = 12;
    let up = DVector::from_column_slice(&[0.0, 0.0, 1.0, 0.0]);
    let down = -&up;
    let twist_motion: DMatrix<_> = (0..N_POINTS).step_by(4).flat_map(
        |n| [
            tangent.proj(&up.as_view(), n),
            tangent.proj(&down.as_view(), n+1)
        ]
    ).sum();
    let normalization = 5.0 / twist_motion[(2, 0)];
    print!("Twist motion:{}", normalization * twist_motion);
}