#include "geocomputation.h"
#include "Stream/ugdefs.h"

#include "erkir/sphericalpoint.h"

GeoComputation::GeoComputation()
{

}

/*@brief 计算两点间球面距离
 * @param lon1(-180至180),lat1(-90到90) 经纬度点1(度)
 * @param lon2,lat3 经纬度点2(度)
 * @return 距离（m）
 */
double GeoComputation::getSphericalDistance(double lon1, double lat1, double lon2, double lat2)
{
    erkir::spherical::Point p1{ lat1, lon1 };
    erkir::spherical::Point p2{ lat2, lon2 };
    double distance = p1.distanceTo(p2);
    return distance;
}

/*@brief  根据起点坐标、方位角、距离，计算另一点坐标。
* startPoint:起始点地理坐标点（lat(-90到90)，lon(-180,180)）
* bearing:方位角(0-360)（度）
* dist:两点之间距离(m)
*/
QPointF GeoComputation::getDestinationPoint(double lon1, double lat1, double bearing, double dist)
{
    erkir::spherical::Point p{ lat1, lon1 };
    auto dest = p.destinationPoint(dist, bearing); // 51.5135°N, 000.0983°W
    QPointF point(dest.longitude().degrees(),dest.latitude().degrees());
    return point;
}

/*@brief  根据起点坐标、方位角、距离，计算另一点坐标。
* 使用Vincenty's公式求解,使用WGS-84椭球
* startPoint:起始点地理坐标点（lat(-90到90)，lon(-180,180)）
* bearing:方位角（度）
* dist:两点之间距离(km)
*/
QPointF GeoComputation::computeOffsetGeoPosition(double lon1, double lat1, double bearing, double dist)
{
    QPointF endPoint;
    //角度转化为弧度
//    qreal lon1 = (fmod(startPoint.x()+540,360)-180.0)*PI/180;
    lon1 = lon1*PI/180.0;
    lat1 = lat1*PI/180.0;
    bearing = bearing*PI/180.0;
    dist = dist*1000;

    //WGS-84椭球参数
    double flat = 298.257223563;
    double a = 6378137.0;
    double b = 6356752.314245;
    //开始求解坐标
    double f = 1/flat;
    double sb = sin(bearing);
    double cb = cos(bearing);
    double tu1 = (1-f)*tan(lat1);
    double cu1 = 1/sqrt((1+tu1*tu1));
    double su1 = tu1*cu1;
    double s2 = atan2(tu1, cb);
    double sa = cu1*sb;
    double csa = 1-sa*sa;
    double us = csa*(a*a - b*b)/(b*b);
    double A = 1+us/16384*(4096+us*(-768+us*(320-175*us)));
    double B = us/1024*(256+us*(-128+us*(74-47*us)));
    double s1 = dist/(b*A);
    double s1p = 2*PI;


    double cs1m = 0.0;
    double ss1 = 0.0;
    double cs1 = 0.0;
    double ds1 = 0.0;

    while (abs(s1-s1p) > 1e-12)
    {
        cs1m = cos(2*s2+s1);
        ss1 = sin(s1);
        cs1 = cos(s1);
        ds1 = B*ss1*(cs1m+B/4*(cs1*(-1+2*cs1m*cs1m)- B/6*cs1m*(-3+4*ss1*ss1)*(-3+4*cs1m*cs1m)));
        s1p = s1;
        s1 = dist/(b*A)+ds1;
    }

    double t = su1*ss1-cu1*cs1*cb;
    double lat2 = atan2(su1*cs1+cu1*ss1*cb, (1-f)*sqrt(sa*sa + t*t));
    double l2 = atan2(ss1*sb, cu1*cs1-su1*ss1*cb);
    double c = f/16*csa*(4+f*(4-3*csa));
    double l = l2-(1-c)*f*sa* (s1+c*ss1*(cs1m+c*cs1*(-1+2*cs1m*cs1m)));
    double lon2 = lon1+l;

    endPoint.setX(lon2*180/PI);
    endPoint.setY(lat2*180/PI);
    return endPoint;
}

//显示坐标转地图坐标
UGPoint2D GeoComputation::DPtoMP(UGMap *pMap, const QPoint &point)
{
    UGDrawCache &drawing = pMap->GetDrawing()->m_DrawCache;
    UGPoint DP(point.x(), point.y());
    UGPoint2D LP(0,0);
    drawing.DPtoLP(&DP, &LP, 1);

    UGPoint2D MP(0, 0);

    pMap->GetDrawing()->m_DrawParam.LPtoMP(LP, MP);

    return MP;
}

//获取几何对象长度
double GeoComputation::getGeometryLength(UGGeometry *geometry)
{
    int type = geometry->GetType();
    double sum = 0;
    switch (type) {
    case UGGeometry::GeoLine:
        {
            UGGeoLine* line = (UGGeoLine*)geometry;
            const UGPoint2D* pts = line->GetPoints();
            double x1 = pts->x;
            double y1 = pts->y;
            double x2,y2;
            for (int i = 1; i < line->GetPointCount(); ++i)
            {
                pts++;
                x2 = pts->x;
                y2 = pts->y;
                sum += this->getSphericalDistance(x1,y1,x2,y2);
                x1 = x2;
                y1 = y2;
            }
        }
        break;
    default:
        break;
    }
    return sum;
}

//计算高度余量
void GeoComputation::getAlititudeDiff(UGGeoLine3D line3D,UGDatasetRasterPtr dem,QVector<double>& x_dist,QVector<double>& z_diff)
{
    const UGPoint3D* pts3D1 = line3D.GetPoints();
//    QVector<double> z_diff; //高度余量
//    QVector<double> x_dist; //距离累计量
    double dist_sum = 0;
    for (int i = 0; i < line3D.GetPointCount()-1; ++i)
    {
        //多线段分解为单线段进行剖面分析
        UGPoint2D pt0 = UGPoint2D(pts3D1->x,pts3D1->y);
        int z0 = pts3D1->z;
        pts3D1++;
        UGPoint2D pt1 = UGPoint2D(pts3D1->x,pts3D1->y);
        int z1 = pts3D1->z;
        UGGeoLine line;
        line.Make(pt0,pt1);

        //进行剖面分析
//        UGDatasetPtr tmp = m_pWorkspace->GetDataSource(0)->GetDataset(Translator::QStr2UGStr(demName));
//        UGDatasetRasterPtr dem = dynamic_pointer_cast<UGDatasetRaster>(tmp);
        QVector<double> y;
        QVector<QPointF> vec_pt;

        geoAnalysisMag.SurfaceProfile_Parallel(dem,&line,y,vec_pt);

        if(i==0)//折线起点
        {
            x_dist.append(0);
            z_diff.append(z0-y.at(0));
        }
        double d0 = this->getSphericalDistance(pt0.x,pt0.y,pt1.x,pt1.y);
        for(int i=1;i<y.count()-1;i++) //计算内插点的海拔高度
        {
            double d1 = this->getSphericalDistance(pt0.x,pt0.y,vec_pt.at(i).x(),vec_pt.at(i).y());
            double z_pt = z0 + (z1-z0)*(d1/d0); //插值点高度
            //            vec_z.append(z_pt);
            //            dist_sum += sqrt(d1*d1+z_pt*z_pt);
            z_diff.append(z_pt - y.at(i)); //高度余量
            x_dist.append(dist_sum + sqrt(d1*d1+z_pt*z_pt)); //距离累计
        }
        x_dist.append(dist_sum + sqrt(d0*d0+(z1-z0)*(z1-z0)));
        z_diff.append(z1-y.at(y.count()-1));
        dist_sum += sqrt(d0*d0+(z1-z0)*(z1-z0));
    }
}