#include "toolsclass.h"
#include "mydef.h"

#include <QDebug>
#include <QString>

ToolsClass::ToolsClass()
{

}

ToolsClass::~ToolsClass()
{

}

void ToolsClass::CalcAngle()
{
    // test for 角度
    double Ax1, Ay1;
    double Bx1, By1;
    double Cx1, Cy1;
    double a, b, c, A, B, C;

    Ax1 = -137.0;
    Ay1 = 128.0;
    Bx1 = 0.0;
    By1 = 0.0;
    Cx1 = -72.0;
    Cy1 = 33.0;

    a = sqrt((Bx1 - Cx1) * (Bx1 - Cx1) + (By1 - Cy1) * (By1 - Cy1));
    b = sqrt((Ax1 - Cx1) * (Ax1 - Cx1) + (Ay1 - Cy1) * (Ay1 - Cy1));
    c = sqrt((Ax1 - Bx1) * (Ax1 - Bx1) + (Ay1 - By1) * (Ay1 - By1));
    A = acos((b * b + c * c - a * a) / (2 * b * c)) * 180 / PI; // 角度
    B = acos((a * a + c * c - b * b) / (2 * a * c)) * 180 / PI;
    C = acos((a * a + b * b - c * c) / (2 * a * b)) * 180 / PI;

    qDebug() << "A:" << QString::number(A);
    qDebug() << "A:" << QString::number(B);
    qDebug() << "A:" << QString::number(C);
}

void ToolsClass::mathRotatedPoint(double degree, double x1, double y1, double x0, double y0, double *x2, double *y2)
{
    // 平面直角坐标系中，一点x1,y1,绕平面上另一点x0,y0顺时针旋转θ(弧度表示)角度后的点x2 y2
    // x2=(x1-x0)cosθ+(y1-y0)sinθ+x0
    // y2=(y1-y0)cosθ-(x1-x0)sinθ+y0

    double a = sin(degree), b = cos(degree);

    *x2 = (x1 - x0) * b + (y1 - y0) * a + x0;
    *y2 = (y1 - y0) * b - (x1 - x0) * a + y0;
}

void ToolsClass::imgRotatedPoint(double degree, double x, double y, double x0, double y0, double *rx, double *ry)
{
    // 图像坐标系中，一点x,y,绕平面上另一点x0,y0顺时针旋转θ角度后的点rx, ry

    // 已知平面中一点(x1,y1)饶原点顺时针旋转θ（弧度表示），旋转后点公式为：
    // x = x1cosθ + y1sinθ
    // y = y1cosθ - x1sinθ
    // 图像坐标系中点(x1,y1)以（x0,y0）为原点，转换成 平面直角坐标系坐标为（x',y'）
    // x' = x1 - x0;
    // y' = y0 - y1;
    // 因此得
    // rx = (x-x0)cosθ + (y0-y)sinθ + x0;
    // ry = y0 - (y0-y)cosθ + (x-x0)sinθ;

    double a, b;
    double radian;

    radian = degree * PI / 180; // 角度转弧度
    a = sin(radian);
    b = cos(radian);

    *rx = (x - x0) * b + (y0 - y) * a + x0;
    *ry = y0 + (x - x0) * a - (y0 - y) * b;
}

///
/// \brief ToolsClass::GetMathCircleCenter    三点确认圆心
/// \param x1
/// \param y1
/// \param x2
/// \param y2
/// \param x3
/// \param y3
/// \param cx
/// \param cy
/// \param radius
/// \return
///
int ToolsClass::GetMathCircleCenter(double x1, double y1, double x2, double y2, double x3, double y3, double *cx,
                                    double *cy, int *radius)
{
    // (x1-X)²-(y1-Y)²=R²    ==>  2(x1-x2)X+2(y1-y2)Y=x1²-x2²+y1²-y2²
    // (x2-X)²-(y2-Y)²=R²         2(x2-x3)X+2(y2-y3)Y=x2²-x3²+y2²-y3²
    // (x3-X)²-(y3-Y)²=R²
    double x, y;
    int r;
    double a, b, c, d, e, f;
    double k1, k2;

    if ((y1 == y2) && (y2 == y3))
    {
        qCritical() << "Point in a straight line,Can not calculate center of cricle.";
        return RETURE_ERROR;
    }
    if ((y1 != y2) && (y2 != y3))
    {
        k1 = (x1 - x2) / (y1 - y2);
        k2 = (x2 - x3) / (y2 - y3);

        if (k1 == k2)
        {
            qCritical() << "Point in a straight line,Can not calculate center of cricle.";
            return RETURE_ERROR;
        }

        a = 2 * (x1 - x2);
        b = 2 * (y1 - y2);
        c = x1 * x1 + y1 * y1 - x2 * x2 - y2 * y2;
        d = 2 * (x2 - x3);
        e = 2 * (y2 - y3);
        f = x2 * x2 + y2 * y2 - x3 * x3 - y3 * y3;

        if (b * d != e * a)
        {
            x = (b * f - e * c) / (b * d - e * a);
            y = (d * c - a * f) / (b * d - e * a);
            r = sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));

            *cx = x;
            *cy = y;
            *radius = r;
            qDebug() << "center of cricle" << QString::number(x) << QString::number(y);

            return RETURE_SUCCESS;
        }
    }

    return RETURE_ERROR;
}

///
/// \brief ToolsClass::GetGravityCenter   // 根据传入星点坐标，获取星点重心
/// \param img
/// \param refPosX
/// \param refPosY
/// \param gravityPosX
/// \param gravityPosY
/// \return
///
int ToolsClass::GetGravityCenter(cv::Mat *img, double refPosX, double refPosY, double *gravityPosX, double *gravityPosY)
{
    cv::Mat thImg;
    cv::Mat thImg2;
    cv::Mat srcImg;

    cv::Scalar RMS, STD;
    double max, min;
    double rms, SNR;
    double WX, WY;
    cv::Moments StarCenter;
    int ret = RETURE_ERROR;
    int roiStartX;
    int roiStartY;
    int roiSizeX;
    int roiSizeY;

    *gravityPosX = refPosX;
    *gravityPosY = refPosY;

    roiStartX = refPosX - 10;
    roiStartY = refPosY - 10;
    roiSizeX = 20;
    roiSizeY = 20;

    if(img)
    {
        if ((img->cols <= 0) || (img->rows <= 0) || (roiStartX < 0) || (roiStartY < 0) ||
                (roiStartX + roiSizeX > img->cols) || (roiStartY + roiSizeY > img->rows))
        {
            ret = RETURE_ERROR;
            qCritical() << "GetGravityCenter - Out of Whole FOV";
        }
        else
        {
            srcImg = cv::Mat::zeros(img->size(), CV_8UC1);
            thImg = cv::Mat::zeros(roiSizeY, roiSizeX, CV_8UC1);
            thImg2 = cv::Mat::zeros(roiSizeY, roiSizeX, CV_8UC1);

            cv::medianBlur(*img, srcImg, 3); // 中值滤波 去除热噪点

            cv::Rect roi(roiStartX, roiStartY, roiSizeX, roiSizeY);
            cv::Mat srcRoi = srcImg(roi);
            if (img->channels() == 1)
            {
                srcRoi.copyTo(thImg);
            }
            else
            {
                cv::cvtColor(srcRoi, thImg, cv::COLOR_RGB2GRAY);
            }

            thImg.copyTo(thImg2);

            cv::minMaxLoc(thImg, &min, &max); // 计算最大最小值
            cv::meanStdDev(thImg, RMS, STD); // 计算平均值与方差
            rms = RMS[0];
            SNR = max / rms;

            if (SNR < 2.5)
            {
                qWarning() << "GetGravityCenter - Too low SNR, Keep stay";
                *gravityPosX = refPosX;
                *gravityPosY = refPosY;

                ret = RETURE_ERROR;
            }
            else
            {
                RMS[0] = rms + (max - rms) * 0.3;
                cv::subtract(thImg2, RMS, thImg2);
                StarCenter = cv::moments(thImg2, false); // 计算最高达三阶的空间和中心矩

                WX = StarCenter.m10 / StarCenter.m00; // 求出重心坐标
                WY = StarCenter.m01 / StarCenter.m00;

                *gravityPosX = WX + roiStartX;
                *gravityPosY = WY + roiStartY;

                ret = RETURE_SUCCESS;
            }
            // cv::Mat handles memory automatically, no need for cvReleaseImage

        }
    }

    return ret;
}

///
/// \brief ToolsClass::GetPreciseStarPos   获取星点精确坐标
/// \param img
/// \param refPosX
/// \param refPosY
/// \param starPosX
/// \param starPosY
/// \return
///
int ToolsClass::GetPreciseStarPos(cv::Mat *img, double refPosX, double refPosY, double *starPosX, double *starPosY)
{
    float rms;
    double max, min;
    cv::Point MaxLocation;
    cv::Point MinLocation;

    cv::Mat img_src;
    cv::Mat img_star;
    cv::Mat img_erode;
    uint32_t ret = RETURE_ERROR;

    int roiStartX;
    int roiStartY;
    int roiSizeX;
    int roiSizeY;

    double tempPosX;
    double tempPosY;

    *starPosX = refPosX;
    *starPosY = refPosY;
    roiStartX = refPosX - 20;
    roiStartY = refPosY - 20;
    roiSizeX = 40;
    roiSizeY = 40;

    if(img)
    {
        if ((img->cols <= 0) || (img->rows <= 0) || (roiStartX < 0) || (roiStartY < 0) ||
                (roiStartX + roiSizeX > img->cols)  || (roiStartY + roiSizeY > img->rows) )
        {
            ret = RETURE_ERROR;
            qCritical() << "GetPreciseStarPos - Out of Whole FOV";
        }
        else
        {
            img_src = cv::Mat::zeros(img->size(), CV_8UC1);
            img_star = cv::Mat::zeros(roiSizeY, roiSizeX, CV_8UC1);
            img_erode = cv::Mat::zeros(roiSizeY, roiSizeX, CV_8UC1);

            // 中值滤波 去除热噪点
            cv::medianBlur(*img, img_src, 3);
            // ------40x40 roi ------
            cv::Rect roi(roiStartX, roiStartY, roiSizeX, roiSizeY);
            cv::Mat srcRoi = img_src(roi);
            srcRoi.copyTo(img_star);
            // cvResetImageROI is not needed with cv::Mat slicing
            // 计算星点区域 avg  sdv
            cv::Scalar RMS, STD;
            cv::meanStdDev(img_star, RMS, STD);
            rms = RMS[0]; // 记录最初的avg值  rms
            if (rms == 0)
            {
                ret = RETURE_ERROR;
                qCritical() << "GetPreciseStarPos - No Star in the ROI Area.";
            }
            else
            {
                // 记录最初的 max，min值
                cv::minMaxLoc(img_star, &min, &max, &MinLocation, &MaxLocation);
                tempPosX = MaxLocation.x + roiStartX;
                tempPosY = MaxLocation.y + roiStartY;

                // 计算重心
                ret = GetGravityCenter(img, tempPosX, tempPosY, starPosX, starPosY);
                if (ret == RETURE_ERROR)
                {
                    *starPosX = tempPosX;
                    *starPosY = tempPosY;
                }

                ret = RETURE_SUCCESS;
            }
            // cv::Mat handles memory automatically, no need for cvReleaseImage
        }
    }

    return ret;
}

///
/// \brief ToolsClass::CalcPoleCOOR   计算极点坐标
/// \param aX
/// \param aY
/// \param A
/// \param bX
/// \param bY
/// \param B
/// \param pX
/// \param pY
/// \param cx
/// \param cy
/// \return
///
int ToolsClass::CalcPoleCOOR(double aX, double aY, double A, double bX, double bY, double B, double *pX, double *pY,
                             double cx, double cy)
{
    int ret = RETURE_ERROR;
    double a, b, c; // 边长
    double SA, SB, SC; // sinA  SinB  sinC
    double dx, dy, k;
    double Da, Db, Dc;
    double tempy1, tempy2, tempx1, tempx2;
    double poleX = 0.0, poleY = 0.0;

    SA = sin(A * PI / 180);
    SB = sin(B * PI / 180);
    SC = sin((180 - A - B) * PI / 180);

    if ((SC != 0) && (bX != aX))
    {
        c = sqrt((aX - bX) * (aX - bX) + (aY - bY) * (aY - bY));
        a = SA * (c / SC);
        b = SB * (c / SC);

        dx = bX - aX;
        dy = bY - aY;
        k = b * b - a * a + bX * bX - aX * aX + bY * bY - aY * aY;

        Da = (dy / dx) * (dy / dx) + 1;
        Db = (2 * aX - (k / dx)) * (dy / dx) - 2 * aY;
        Dc = (aX - k / (2 * dx)) * (aX - k / (2 * dx)) + aY * aY - b * b;

        double temp = Db * Db - 4 * Da * Dc;
        if (temp > 0)
        {
            tempy1 = (1 * sqrt(Db * Db - 4 * Da * Dc) - Db) / (2 * Da); // 第一组解
            tempx1 = (k - 2 * tempy1 * dy) / (2 * dx);

            tempy2 = (-1 * sqrt(Db * Db - 4 * Da * Dc) - Db) / (2 * Da); // 第二组解
            tempx2 = (k - 2 * tempy2 * dy) / (2 * dx);

            double value1 = (tempx1 - cx) * (tempx1 - cx) + (tempy1 - cy) *
                            (tempy1 - cy);
            double value2 = (tempx2 - cx) * (tempx2 - cx) + (tempy2 - cy) *
                            (tempy2 - cy);
            if (value1 < value2) // 选距离圆心近的为正解
            {
                poleY = tempy1;
                poleX = tempx1;
            }
            else
            {
                poleY = tempy2;
                poleX = tempx2;
            }
            ret = RETURE_SUCCESS;
        }
        else if (temp == 0)
        {
            poleY = (-1) * Db / (2 * Da);
            poleX = (-1) * Db / (2 * Da);
            ret = RETURE_SUCCESS;
        }
        else if (temp < 0)
        {
            poleY = 0;
            poleX = 0;
            ret = RETURE_ERROR;
            qCritical() << "CalcPoleCOOR - Calculate pole failed.";
        }
    }
    else
    {
        poleX = 0;
        poleY = 0;
        ret = RETURE_ERROR;
        qCritical() << "CalcPoleCOOR - Calculate pole failed as invalid value.";
    }

    *pX = poleX;
    *pY = poleY;

    return ret;
}

///
/// \brief ToolsClass::CorrectAtmRefraction  根据当前纬度，大气压，温度，对传入坐标进行蒙气差校正
/// \param posY
/// \param latitude
/// \param P
/// \param T
/// \param corY
/// \return
///
int ToolsClass::CorrectAtmRefraction(double posY, double latitude, double P, double T, double *corY)
{
    double R, pt;
    double degree, radian;

    degree = latitude + 7.31 / (latitude + 4.4);
    radian = PI / 180 * degree;
    R = tan(PI / 2 - radian); // 蒙气差值
    if (R < 0)
    {
        R = 0;
    }

    if (T + 273 == 0)
    {
        pt = 1;
    }
    else
    {
        pt = (P / 101) * (283 / (273 + T)); // 气压，温度校正值
    }

    *corY = posY - R * pt; // 坐标校正

    return RETURE_SUCCESS;
}

void ToolsClass::ImgStretch(unsigned char *inputdata, unsigned char *outputdata, int w, int h, int offset, double ratio)
{
    //offset 与ratio值 待定。。。需进行实际测试以取的较为合理的值
    //int offset = 50;
    //double ratio = 150.0 / 255.0;
    uint32_t pixel;

    if(ratio == 0) ratio = 1.0;

    for(int i = 0; i < w * h; i++)
    {
        pixel = inputdata[i];
        if(pixel <= (uint32_t)offset)
            pixel = 0;
        else
        {
            pixel = (pixel - offset) / ratio;
        }

        if(pixel > 255) pixel = 255;

        outputdata[i] = pixel;

    }
}

///
/// \brief ToolsClass::CalcMaskPoleCoor   计算蒙版图中极点坐标   传入蒙版图中星A,B,D坐标以及角度BAP,ABP, DBP,计算并得到极点坐标P
/// \param aX
/// \param aY
/// \param A
/// \param bX
/// \param bY
/// \param B
/// \param pX
/// \param pY
/// \param dX
/// \param dY
/// \param angle_DBP
/// \return
///
int ToolsClass::CalcMaskPoleCoor(double aX, double aY, double A, double bX, double bY, double B, double *pX, double *pY,
                                 double dX, double dY, double angle_DBP)
{
    int ret = RETURE_ERROR;
    double a, b, c; // 边长
    double SA, SB, SC; // sinA  SinB  sinC
    double dx, dy, k;
    double Da, Db, Dc;
    double tempy1, tempy2, tempx1, tempx2;
    double poleX, poleY;

    SA = sin(A * PI / 180);
    SB = sin(B * PI / 180);
    SC = sin((180 - A - B) * PI / 180);

    if ((SC != 0) && (bX != aX))
    {
        c = sqrt((aX - bX) * (aX - bX) + (aY - bY) * (aY - bY));
        a = SA * (c / SC);
        b = SB * (c / SC);

        dx = bX - aX;
        dy = bY - aY;
        k = b * b - a * a + bX * bX - aX * aX + bY * bY - aY * aY;

        Da = (dy / dx) * (dy / dx) + 1;
        Db = (2 * aX - (k / dx)) * (dy / dx) - 2 * aY;
        Dc = (aX - k / (2 * dx)) * (aX - k / (2 * dx)) + aY * aY - b * b;

        double temp = Db * Db - 4 * Da * Dc;
        if (temp > 0)
        {
            tempy1 = (1 * sqrt(Db * Db - 4 * Da * Dc) - Db) / (2 * Da); // 第一组解
            tempx1 = (k - 2 * tempy1 * dy) / (2 * dx);

            tempy2 = (-1 * sqrt(Db * Db - 4 * Da * Dc) - Db) / (2 * Da); // 第二组解
            tempx2 = (k - 2 * tempy2 * dy) / (2 * dx);

            //去除错误解，得到正确坐标值
            GetCorrectValueOfPoles(tempx1, tempy1, tempx2, tempy2, dX, dY, bX, bY, angle_DBP, &poleX, &poleY);
            //qDebug() << "calculated P" << QString::number(poleX, 'f', 2) << QString::number(poleY, 'f', 2);

            ret = RETURE_SUCCESS;
        }
        else if (temp == 0)
        {
            poleY = (-1) * Db / (2 * Da);
            poleX = (-1) * Db / (2 * Da);
            //qDebug() << "calculated P" << QString::number(poleX, 'f', 2) << QString::number(poleY, 'f', 2);
            ret = RETURE_SUCCESS;
        }
        else //if (temp < 0)
        {
            qCritical() << "did not calculate the appropriate pole";
            poleY = 0;
            poleX = 0;
            ret = RETURE_ERROR;
        }
    }
    else
    {
        qCritical() << "invalid points, can not calculate pole.";
        poleX = 0;
        poleY = 0;
        ret = RETURE_ERROR;
    }

    *pX = poleX;
    *pY = poleY;

    return ret;
}

///
/// \brief ToolsClass::GetCorrectValueOfPoles    比较两组极点坐标值，得到正确的一组解
/// \param x1
/// \param y1
/// \param x2
/// \param y2
/// \param maskDX
/// \param maskDY
/// \param maskBX
/// \param maskBY
/// \param angle_DBP
/// \param x
/// \param y
///
void ToolsClass::GetCorrectValueOfPoles(double x1, double y1, double x2, double y2, double maskDX, double maskDY,
                                        double maskBX, double maskBY, double angle_DBP, double *x, double *y)
{
    //△PBD
    double dx_P;  //D B坐标差值
    double dy_P;
    double P;   //BD边长
    double dx_D; //P B坐标差值
    double dy_D;
    double D;  //BP边长
    double dx_B; //P D坐标差值
    double dy_B;
    double B;  //DP边长

    double angleB1; //∠DBP
    double angleB2; //∠DBP


    dx_P = maskDX - maskBX;
    dy_P = maskDY - maskBY;
    P = sqrt(pow(dx_P, 2) + pow(dy_P, 2));

    //angleB1
    dx_D = x1 - maskBX;
    dy_D = y1 - maskBY;
    D = sqrt(pow(dx_D, 2) + pow(dy_D, 2));

    dx_B = maskDX - x1;
    dy_B = maskDY - y1;
    B = sqrt(pow(dx_B, 2) + pow(dy_B, 2));

    angleB1 = acos((P * P + D * D - B * B) / (2 * P * D));
    angleB1 = angleB1 * 180 / PI;

    //angleB2
    dx_D = x2 - maskBX;
    dy_D = y2 - maskBY;
    D = sqrt(pow(dx_D, 2) + pow(dy_D, 2));

    dx_B = maskDX - x2;
    dy_B = maskDY - y2;
    B = sqrt(pow(dx_B, 2) + pow(dy_B, 2));

    angleB2 = acos((P * P + D * D - B * B) / (2 * P * D));
    angleB2 = angleB2 * 180 / PI;

    //与资源文件中存储的角度值比较，误差小的即为正解
    if(fabs(angleB1 - angle_DBP) < fabs(angleB2 - angle_DBP))
    {
        *x = x1;
        *y = y1;
    }
    else
    {
        *x = x2;
        *y = y2;
    }
}
