//
// Created by Scheaven on 2020/5/20.
//

#include "switcher_util.h"


CoordSwitcher* CoordSwitcher::instance = NULL;
SHomogtaphy* CoordSwitcher::SHomogtaphy_pointer = (SHomogtaphy*)malloc(sizeof(SHomogtaphy));

CoordSwitcher* CoordSwitcher::getInstance(CamVec *CV_Reg)
{
    if(instance==NULL)
    {
        instance = new CoordSwitcher();
//        std::cout << "....---.loading Coord....." << std::endl;
    }
    return instance;
}

CoordSwitcher::CoordSwitcher()
{

//    initHomography(CV_Reg);
}


CoordSwitcher::~CoordSwitcher()
{

}

void CoordSwitcher::initHomography(CamVec *CV_Reg)
{
    vector<SPoint2f> reg_points;
    for (int k = 0; k < CV_Reg->count; ++k) {
        for (int i = 0; i < CV_Reg->regInfo[k].count; ++i) {
            reg_points.push_back(CV_Reg->regInfo[k].point[i]);
        }
        reg_points_vec.push_back(reg_points);
        reg_points.clear();
    }

    float reprojThresh = 4.0;

    SHomogtaphy_pointer->count = CV_Reg->count-1;
    SHomogtaphy_pointer->matrix = (SMatrix*)malloc(sizeof(SMatrix)*SHomogtaphy_pointer->count);

    for (int j_index = 1; j_index < CV_Reg->count; ++j_index) {
        creatHomography(reg_points_vec[0], reg_points_vec[j_index], SHomogtaphy_pointer, j_index-1);
    }

//    for (int l = 0; l < 3; ++l) {
//        for (int m = 0; m < 3; ++m) {
//            std::cout <<   SHomogtaphy_pointer->matrix[0].homography[l][m] <<  "---" << std::endl;
//            this->s_homography[l][m] = SHomogtaphy_pointer->matrix[0].homography[l][m];
//        }
//    }
//

}

float CoordSwitcher::compare(SPoint2f A, SPoint2f B, int h_index)
{
    SPoint2f rightPoint = A;
    SPoint2f leftPoint;
    leftPoint = getTransPoint(rightPoint, SHomogtaphy_pointer->matrix[h_index]);

    float distance = calDistance(leftPoint, B);

    return distance;
}

SPoint2f CoordSwitcher::getTransPoint(const SPoint2f rightPoint, SMatrix& matrix)
{
    float rightP[3][1], leftP[3][1];
    rightP[0][0] = rightPoint.x;
    rightP[1][0] = rightPoint.y;
    rightP[2][0] = 1.0;
    for(int i=0;i<3;++i){
        for(int j=0;j<1;++j){
            leftP[i][j]=0;
            for(int k=0;k<3;++k){
                leftP[i][j]+=matrix.homography[i][k]*rightP[k][j];
            }
        }
    }

    float x = leftP[0][0] / leftP[2][0];
    float y = leftP[1][0] / leftP[2][0];
    return SPoint2f(x, y);
}