#include "Arith_FrontProj.h"
#include "Arith_Utils.h"
#include "StitchStruct.h"
#include "Arith_GeoSolver.h"
Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep)
{
    // 投影到地面坐标，这一步可以利用前面优化成果
    cv::Point2f grdPt = warpPointWithH(H, pt);

    // 补深度信息并转为常用的NUE坐标系
    PointXYZ grdPtXYZ = { 0 };
    grdPtXYZ.X = grdPt.y;
    grdPtXYZ.Y = -dep;
    grdPtXYZ.Z = grdPt.x;

    // 地面点转极坐标
    Pole pole = getPoleFromXYZ(grdPtXYZ);

    return pole;

}

cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep)
{
    PointXYZ virPt = getXYZFromPole(_pole);

    //虚拟点投影到地面
    float ratio = -dep / virPt.Y;
    PointXYZ realPt = { 0 };
    realPt.X = virPt.X * ratio;
    realPt.Y = virPt.Y * ratio;
    realPt.Z = virPt.Z * ratio;


    // 转东北地
    PointXYZ realPtGeo = { 0 };
    realPtGeo.X = realPt.Z;
    realPtGeo.Y = realPt.X;
    realPtGeo.Z = -realPt.Y;

    // 投影回像方
    cv::Point2f px = warpPointWithH(H_inv, cv::Point2f(realPtGeo.X, realPtGeo.Y));

    return px;
}

Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara)
{
    Pole _pole = { 0 };
    _pole.beta = _panPara.center.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes;
    _pole.alpha = _panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes;
    return _pole;
}

cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara)
{
    cv::Point2f pt = { 0 };
    pt.x = DEGLIM(_pole.beta - _panPara.center.fAz) / _panPara.fAglRes + _panPara.m_pan_width / 2;
    pt.y = DEGLIM(_panPara.center.fPt - _pole.alpha) / _panPara.fAglRes + _panPara.m_pan_height / 2;
    return pt;
}