//#include "opencv2/core.hpp"
//#include "opencv2/imgproc.hpp"
//#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
#include <cstdio>
#include <fstream>
#include <vector>
#include <math.h>
#include <Eigen/Dense>
#include "pch.h"


//using namespace cv;
using namespace std;
using namespace Eigen;

//Angle, Distance, robot(x,y,a), sensor(x,y,a), unitVector, orthogonal to line (ri)
/*
    Return a vector with four double values (x, y, a, C) that represent the

    @param int &iAngle
    @param int &iDistance
    @param Eigen::Vector3d &robotPose
    @param Eigen::Vector3d &sensorPose
    @param Eigen::Vector2d &unitVector
    @param Eigen::Vector2d &ri
    @return
*/
Eigen::RowVector4d CoxScanMatch(int &iAngle, int &iDistance, Eigen::Vector3d &robotPose, Eigen::Vector3d &sensorPose, Eigen::Vector2d &unitVector, Eigen::Vector2d &ri)
{

    int Rx, Ry, Ra;
    Rx = sensorPose(0); // ?
    Ry = sensorPose(1); // ?
    Ra = sensorPose(2); // ?
    int maxIter = 15;   // ?
    int ddx = 0;
    int ddy = 0;
    int dda = 0;
    double alpha = sensorPose(0);
    double beta = sensorPose(1);
    double gamma = sensorPose(2);

    // Repeat until process converge or maximum iterations is reached
    for (int i = 0; i < maxIter; i++)
    {
        // 1)   Transform Laser Points to Cartesian Coordinates
        //      Rotate and translate data from sensor to Robot Coordinate System
        //      Rotate and translate Robot Coordinate System to World Coordinate System

        Rx = Rx + ddx;
        Ry = Ry + ddy;
        Ra = Ra + dda;

        double x = iDistance * cos(iAngle); // Ska dessa vara doubles?
        double y = iDistance * sin(iAngle);

        MatrixXd R1(3, 3);
        R1(0, 0) = cos(gamma);  R1(1, 0) = -sin(gamma); R1(2, 0) = alpha;
        R1(1, 0) = sin(gamma);  R1(1, 1) = cos(gamma);  R1(1, 2) = beta;
        R1(2, 0) = 0;           R1(2, 1) = 0;           R1(2, 2) = 1;

        RowVector3d temp;
        temp(0) = x;            temp(1) = y;            temp(2) = 1;

        RowVector3d Xs;
        Xs = R1 * temp;

        MatrixXd R2(3, 3);
        R2(0, 0) = cos(Ra); R2(1, 0) = -sin(Ra);    R2(2, 0) = Rx;
        R2(1, 0) = sin(Ra); R2(1, 1) = cos(Ra);     R2(1, 2) = Ry;
        R2(2, 0) = 0;       R2(2, 1) = 0;           R2(2, 2) = 1;

        temp(0) = Xs.row(0);    temp(1) = Xs.row(1);    temp(2) = 1;

        RowVector3d Xw;
        Xw = R2 * temp;

        // 2)   Find and assign the closest line to each Laser Point
        //      Calculate the distance between Li and Pi

        VectorXd targetT;   // (Should have) size of Xw in matlab code, change so initialized with size?
        VectorXd yTargetT;  // (Should have) size of Xw,
        RowVectorXd Y;      // (Should have) size of U,


        for (int j = 0; j < Xw.size(); j++)
        {
            for (int k = 0; k < unitVector.size(); k++) // TODO: Fix how many iterations, should be: for i=1:length(U(:,1))
            {
                Y(k) = ri(k) - unitVector.col(k).dot(Xw.block(0:1, j)); // TODO: is Xw.block correct???
            }
            double minAbsY = Y.cwiseAbs().minCoeff();
            targetT(j) = minAbsY;
            yTargetT(j) = Y(minAbsY);
        }

        // Check th and reject outliers
        int sizeYTargetT = yTargetT.size();
        double yTargetTMedian = yTargetT(sizeYTargetT / 2);     // Does this get the median?
        double th = abs(yTargetTMedian * 2);
        vector target <double>;                                 // how to init?
        vector yTarget <double>;                                // how to init?
        vector search_idx <double>;                             // how to init?

        for (int j = 0; j < targetT.size(); j++)
        {
            if (abs(yTargetT(j) < th))
            {
                search_idx.push_back(j);
                target.push_back(targetT(j));
                yTarget.push_back(yTargetT(j));
            }
        }


        // 3)   Set up the matrix A with measured values (distances to line)
        //      Set up matrix Y with lines Li from map. By solving b from Ab = Y
        //      with least square gives an error in match.

        RowVectorXd points;

        // 4)   Summarize matches (errors)

        // 5)   If match is short or too many repetitions:
        //      Go to step 6 and take care of the summarized match
        //      Else go to step 1

        // 6)

    }
}