#include "triangulation.h"

Triangulator::Triangulator(){
	triangulationMethod = 0; //Default method - linearLS
	reprojectionError = 0;
}

Triangulator::Triangulator(int method){
	triangulationMethod = method;
	reprojectionError = 0;
}

void Triangulator::setTriangulationMethod (int method){
	triangulationMethod = method;
}

double Triangulator::getReprojectionError(){
    return reprojectionError;
}

void Triangulator::triangulate(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
{
    switch(triangulationMethod){
        case ITERATIVE_LS    :	break;
        case ITERATIVE_EIGEN :	break;
        case DIRECT_AFFINE   :	triangulateAffine(W,P,pointCloud); break;
        default : break;
    }
}

pcl::PointCloud<pcl::PointXYZ> Triangulator::triangulatePoints_LinearLS(const cv::Mat &Wf, const cv::Mat &Pm)
{
    //std::cout<<Wf.size()<<std::endl;
    //std::cout<<Pm<<std::endl;
    int nPts  = (int) Wf.cols ;
    int nCams = (int) Wf.rows / 2;

    cv::Mat A;
    cv::Mat B;
    cv::Mat_<double> pt;
    cv::Mat temp;
    pcl::PointCloud<pcl::PointXYZ> cloud;
    //cloud.is_dense = false;

    for ( int p = 0 ; p < nPts ; p++ ){
	
        temp = cv::Mat(0,4,cv::DataType<double>::type);

        for ( int i = 0 ; i < nCams ; i++ ){
       
                temp.push_back( Wf.at<double> ( 2*i + 0, p) * Pm.row( 3*i+2 ) - Pm.row( 3*i + 0 ));
                temp.push_back( Wf.at<double> ( 2*i + 1, p) * Pm.row( 3*i+2 ) - Pm.row( 3*i + 1 ));
         
        }

        A = temp.colRange(0,3);
        B = -1*temp.col(3);

        cv::solve(A,B,pt,cv::DECOMP_SVD);
	//cout<< pt<<endl;
	
        cloud.push_back(pcl::PointXYZ(pt(0),pt(1),pt(2)));
	//cloud.points[p].x = pt(0);
        //cloud.points[p].y = pt(1);
        //cloud.points[p].z = pt(2);
	//std::cout<<pt(2)<<std::endl;
	temp.release();
	
    }
	return cloud;
}

void Triangulator::triangulateAffine(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
{
    if (W.rows%3 == 0){

    }

    cv::Mat q1x = W.row(0);
    cv::Mat q1y = W.row(1);
    cv::Mat q2x;
    if (W.rows%3 == 0) q2x = W.row(3);
    else               q2x = W.row(2);

    double t1,rho,t2;
    //cv::Mat R = rotmFromCameraMat(P.rowRange(0,0+3));
    cv::Mat R = rotmat::fromCameraMatrixAffine(P.rowRange(3,3+3).clone());
    rotmat::decomposeEulerZYZ(R,t1,rho,t2);

    std::cout << rho/PI*180;
    //rho = 3.0/180.0*PI;
    double cosRho = cos(rho);
    double sinRho = sin(rho);

    cv::Mat q1z = (q1x * cosRho - q2x) / sinRho;

    for (auto i = 0; i < q1x.cols; i++){
        pointCloud.push_back(pcl::PointXYZ(
                                 q1x.at<double>(0,i),
                                 q1y.at<double>(0,i),
                                 q1z.at<double>(0,i)));
    }
}