//
// Created by martin on 27. 3. 2021.
//

#include "eliFit.h"
#include <eigen3/Eigen/Eigenvalues>
#include <eigen3/unsupported/Eigen/MatrixFunctions>


#include <iostream>


constexpr double eps = 1e-7;



Eigen::Matrix<double, 6, 1> fit(std::set<int> &arcGroup,std::map<int,Eigen::Matrix<double,6,6>> &ScatterMatrices) {

    // create the constraint matrix C : all values are set to 0 and C(3,3) = 1 indexed from 1
    Eigen::Matrix<double, 6, 6> C;
    C.setZero();
    C(2, 2) = 1; // C(3,3) if indexed from 1
    Eigen::Matrix<double, 6, 6> S; // calc the fitting matrix and change the values to double
    S.setZero();
    for (int arc: arcGroup) {
        S += ScatterMatrices.at(arc);
    }

    // eigen decomposition
    Eigen::EigenSolver<Eigen::Matrix<double,6,6>> solver;
    solver.compute(S,true);


    Eigen::Matrix<double,6,1> values = solver.eigenvalues().real();
    Eigen::Matrix<double,6,6> vectors = solver.eigenvectors().real();



    //set small values to 0
    for (int i = 0; i <6; i++) {
        if (values(i) < 0) {
            if (abs(values(i)) < eps) {
                values(i) = 0;
            }
            else{
                Eigen::Matrix<double,6,1> wrongFitting;
                wrongFitting.setZero();
                return wrongFitting;
                }
        }
    }


    Eigen::Matrix<double,6,6> diagonal = values.asDiagonal();

    auto lambda = diagonal.sqrt();

    auto inversed_lambda = lambda.inverse();


    Eigen::Matrix<double,6,6> copyVectors = vectors;
    Eigen::Matrix<double,6,6> transposedVectors = vectors.transpose();


    Eigen::Matrix<double, 6, 6> M = inversed_lambda * transposedVectors * C * vectors * inversed_lambda;

    solver.compute(M);

    values = solver.eigenvalues().real();
    vectors = solver.eigenvectors().real();


    int max_value_position = 0;
    for (int i = 1; i <6; i++) {
        if (values[max_value_position] < values[i]){
            max_value_position = i;
        }
    }


    Eigen::Matrix<double,6,1> solution = vectors.col(max_value_position);

    return  copyVectors * inversed_lambda * solution;

}



Eigen::Matrix<double, 6, 6> cumulativeFactor(cv::Point2i &point) {
    Eigen::Matrix<double, 6, 6> factor;

    double x = point.x;
    double y = point.y;

    std::vector<double> base({x*x, 2 * x*y,
                             y*y, 2 * x,
                             2 * y, 1});
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            factor(i, j) = base[i] * base[j];
        }
    }

    return factor;
}


Eigen::Matrix<double, 6, 6> cumulativeMatrix(std::vector<cv::Point2i> &arc) {
    Eigen::Matrix<double, 6, 6> matrix;
    matrix.setZero();
        for (cv::Point2i point : arc) {
            matrix += cumulativeFactor(point);
        }

    return matrix;
}