#include "config.hpp"
#include "transform_uv.h"
#include "coordinate_convert.h"
#include "vision_config.h"
using namespace jfx_vision;

std::unique_ptr<VisionConfig>  jfx_vision::g_visionConfig;

void initTransUV(VisionConfig& cvConfig, jfx::TransformUV& uv, int cameraIndex)
    {
        std::vector<double> f_c_x_y;
        std::vector<float> transform_M;
        std::vector<double> dist;

        f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.fx);
        f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.cx);
        f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.fy);
        f_c_x_y.emplace_back(cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.cy);
        transform_M = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.transform_M;
        dist = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate.intrinsics.dist;
        printf("fx,cx,fy,cy:%.12f,%.12f,%.12f,%.12f \n",f_c_x_y[0],f_c_x_y[1],f_c_x_y[2],f_c_x_y[3] );

        for(int i=0; i<dist.size(); i++) {
            printf("%.12f,",dist[i]);
        }
        uv.Init(f_c_x_y, transform_M, dist);
    }

    void initCoordCVT(VisionConfig& cvConfig, CoordinateConvert& cvt, int cameraIndex)
    {
        auto coord = cvConfig.cameraParamConfigs_map.at(cameraIndex).coordinate;
        printf("\nhorn_r,rx,ry,horn_v,vx,vy %.12f, %.8f,%.8f,%.12f,%.8f,%.8f\n",coord.horn_r, coord.rx, coord.ry, coord.horn_v, coord.vx, coord.vy);
        cvt.Init(coord.horn_r, coord.rx, coord.ry, coord.horn_v, coord.vx, coord.vy);
    }


int main() {
    std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";

    g_visionConfig.reset(new VisionConfig(config_yaml));

    // b_center=1566 380  rect=1515 298 1617 380 result=2.76558304,106.13797760 juefx_lat=31.58605464 juefx_lon=120.41351219, crossname=50272, taskid=xianfengchunfengn, Hit_Streak=9 Coast_Cycles=0
    // cam:idx2 :center=1566 380 Trans X=2.80015530 Y=106.79021222  Lat=31.58604875,Lon=120.41351181
    jfx::TransformUV transUV;
    CoordinateConvert coordinatCvt;
    //for(int i=0;i<4;i++) {
        initTransUV(*g_visionConfig, transUV, 2);
        initCoordCVT(*g_visionConfig, coordinatCvt, 2);

        vector<float> transf_M = g_visionConfig->cameraParamConfigs_map.at(2).coordinate.transform_M;
        for(int i=0; i< transf_M.size();i++) {
            printf("%.12f,", transf_M[i]);
        }
        vector<float> r;
        int lx = 1515;
        int ly = 298;
        int rx = 1617;
        int ry = 380;

        int bcenter_x = (lx + rx)/ 2;
        int bcenter_y = ry;
        int ret = transUV.Process(bcenter_x, bcenter_y, r);
        if (ret == 0) {
            float m_NowX = r[0];
            float m_NowY = r[1];
            jfx::Array result84;
            jfx::Array objPoint = { r[0], r[1] };
            coordinatCvt.Dev2WGS84(objPoint, result84, false);
            double m_Lat = result84[0];
            double m_Lon = result84[1];
            printf("\ncenter=%d %d Trans X=%.8f Y=%.8f  Lat=%.8f,Lon=%.8f\n", bcenter_x, bcenter_y, m_NowX, m_NowY, m_Lat, m_Lon);

        }
    //}
}