// Create the matrix to be decomposed: A
// Calculate the least squares problem from A'A: the solution should
// be the eigenvector (v_t) with the smallest eigenvalue
// This solution is called h, Ah = 0
// From this it is possible to use h to calculate the destination
// points from source points

extern crate nalgebra as na;

use crate::transform::utils::{log, Point};

macro_rules! console_log {
    ($($t:tt)*) => (log(&format_args!($($t)*).to_string()));
}

pub fn decompose_matrix(src_points: &[Point], dest_points: &[Point]) -> na::DMatrix<f32> {
    assert_eq!(src_points.len(), dest_points.len());

    let mut d_matrix: na::DMatrix<f32> = na::DMatrix::<f32>::zeros(src_points.len()*2, 9);

    for i in 0..src_points.len() {
        let x_row = &na::RowSVector::<f32, 9>::from_iterator([src_points[i].x, src_points[i].y, 1.0, 0.0, 0.0, 0.0, -dest_points[i].x*src_points[i].x, -dest_points[i].x*src_points[i].y, -dest_points[i].x]);
        let y_row = &na::RowSVector::<f32, 9>::from_iterator([0.0, 0.0, 0.0, src_points[i].x, src_points[i].y, 1.0, -dest_points[i].y*src_points[i].x, -dest_points[i].y*src_points[i].y, -dest_points[i].y]);

        d_matrix.set_row(i*2, x_row);
        d_matrix.set_row(i*2+1, y_row);
    }

    return d_matrix;
}

pub fn homography_matrix(src_points: &[Point], dest_points: &[Point]) {
    let d_matrix = decompose_matrix(src_points, dest_points);
    let d_assert_matrix = d_matrix.clone();

    let svd = na::SVD::new(d_matrix, false, true);
    let singular_vectors = svd.v_t.unwrap();
    let min_vec = singular_vectors.row(svd.singular_values.imin());

    console_log!("{:?}", d_assert_matrix * min_vec);

    let mut h = std::vec::Vec::<f32>::new();

    for i in 0..min_vec.ncols() {
        h.push(min_vec[i]);
    }

    console_log!("{:?}", h);
}