#include "MathFunc.h"

using namespace std;

float DistancePoints(int x1, int y1, int x2, int y2) {
    return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
}

float DistancePoints3D(int x1, int y1, int z1, int x2, int y2, int z2) {
    return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
}

float DistancePointToLine(int x1, int y1, int x2, int y2, int x0, int y0) {
    float A, B, C;
    A = y2 - y1;
    B = x1 - x2;
    C = x2 * y1 - x1 * y2;
    float d = fabsf(A*x0 + B * y0 + C) / sqrtf(A*A + B * B);
    return d;
}

float DistancePointToLine3D(int x1, int y1, int z1, int x2, int y2, int z2, int x0, int y0, int z0) {
    float Ax = x1 - x2;
    float Ay = y1 - y2;
    float Az = z1 - z2;
    float Am = sqrtf(Ax*Ax + Ay * Ay + Az * Az);
    if (Am < 0.000001)
    {
        return DistancePoints3D(x1, y1, z1, x0, y0, z0);
    }
    float Bx = x1 - x0;
    float By = y1 - y0;
    float Bz = z1 - z0;
    float h = Ay / Am * Bz - Az / Am * By;  //计算三阶行列式
    float m = Az / Am * Bx - Ax / Am * Bz;
    float n = Ax / Am * By - Ay / Am * Bx;

    if (DistancePoints3D(x1, y1, z1, x0, y0, z0) <= DistancePoints3D(x2, y2, z2, x0, y0, z0) && (Ax*Bx + Ay * By + Az * Bz) / (sqrt(Ax*Ax + Ay * Ay + Az * Az) * sqrt(Bx*Bx + By * By + Bz * Bz))<0.0)
    {
        return DistancePoints3D(x1, y1, z1, x0, y0, z0);
    }
    else if (DistancePoints3D(x1, y1, z1, x0, y0, z0) > DistancePoints3D(x2, y2, z2, x0, y0, z0) && ((-Ax)*(x2 - x0) + (-Ay) *(y2 - y0) + (-Az)  *(z2 - z0)) / (sqrt(Ax*Ax + Ay * Ay + Az * Az) * sqrt((x2 - x0)*(x2 - x0) + (y2 - y0) * (y2 - y0) + (z2 - z0) *(z2 - z0))) < 0.0)
    {
        return DistancePoints3D(x2, y2, z2, x0, y0, z0);
    }
    else
    {
        return sqrtf(h*h + m * m + n * n);
    }
}

bool PointBetweenLines(int x1, int y1, int x2, int y2, int x0, int y0,int threshold) {
    float A, B, C;
    A = y2 - y1;
    B = x1 - x2;
    C = x2 * y1 - x1 * y2;
    if (A == 0)
    {
        if ((x1 - x0)*(x2 - x0) <= 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else if (B == 0)
    {
        if ((y1 - y0)*(y2 - y0) <= 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        float A1, B1, C1;//Bx - Ay +Ab - Ba = 0 
        A1 = B;
        B1 = -A;
        C1 = A * y1 - B * x1;
        float C2;//Bx - Ay +Ab - Ba = 0 
        C2 = A * y2 - B * x2;
        if (C1*C2 < 0)
        {
            C1 = C1 + A * threshold - B * threshold;
            C2 = C2 + A * threshold - B * threshold;
        }
        else if (C1*C2 >= 0)
        {
            C1 = C1 + A * threshold - B * threshold;
            C2 = C2 -( A * threshold - B * threshold);
        }
        if ((A1*x0 + B1 * y0 + C1)*(A1*x0 + B1 * y0 + C2) <= 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
}

//bool PointBetweenLines(int x1, int y1, int x2, int y2, int x0, int y0) {
//    float A, B, C;
//    A = y2 - y1;
//    B = x1 - x2;
//    C = x2 * y1 - x1 * y2;
//    if (A == 0)
//    {
//        if ((x1 - x0)*(x2 - x0) <= 0)
//        {
//            return true;
//        }
//        else
//        {
//            return false;
//        }
//    }
//    else if (B == 0)
//    {
//        if ((y1 - y0)*(y2 - y0) <= 0)
//        {
//            return true;
//        }
//        else
//        {
//            return false;
//        }
//    }
//    else
//    {
//        float A1, B1, C1;//Bx - Ay +Ab - Ba = 0 
//        A1 = B;
//        B1 = -A;
//        C1 = A * y1 - B * x1;
//        float C2;//Bx - Ay +Ab - Ba = 0 
//        C2 = A * y2 - B * x2;
//        if ((A1*x0 + B1 * y0 + C1)*(A1*x0 + B1 * y0 + C2) <= 0)
//        {
//            return true;
//        }
//        else
//        {
//            return false;
//        }
//    }
//}

double AngleOfVectors(float x1, float y1, float x2, float y2)
{
    int sign = 0;
    if (x1 * y2 - x2 * y1 > 0)//正:2在1逆时针方向
    {
        sign = 1;
    }
    else if (x1 * y2 - x2 * y1 <= 0)//负:顺时针;0:共线
    {
        sign = -1;
    }
    double cosA = (x1*x2 + y1 * y2) / (sqrtf(x1*x1 + y1 * y1) * sqrtf(x2*x2 + y2 * y2));
    if (cosA < -0.999)
        return 180.0;
    if (cosA > 0.999)
        return 0.0;
    //float degree = acosf(cosA);
    //if(degree >PI/2.0)
    //    return sign * (PI-acosf(cosA));
    //else
    return sign * acosf(cosA)*180.0/PI;
}

vector<float> GetRotationVector(vector<float> v1, vector<float> v2) {
    Eigen::Matrix3d rotMatrix;
    Eigen::Vector3d vectorBefore(v1[0], v1[1], v1[2]);
    Eigen::Vector3d vectorAfter(v2[0], v2[1], v2[2]);
    rotMatrix = Eigen::Quaterniond::FromTwoVectors(vectorBefore, vectorAfter).toRotationMatrix();
    //cout << "Rotation_vector1" << endl << rotMatrix << endl;
    vector<float> RotVec(3,0);
    if (vectorBefore == vectorAfter)
    {
        return RotVec;
    }
    else
    {
        RotVec = matrix2angle(rotMatrix);
    }
    //cout << "Rotation_x" << endl << RotVec[0] << endl;
    //cout << "Rotation_y" << endl << RotVec[1] << endl;
    //cout << "Rotation_z" << endl << RotVec[2] << endl;
    return RotVec;
}

double AngleOf3DVectors(float x1, float y1, float z1, float x2, float y2, float z2) {
    return 0.0;
}

vector<float> matrix2angle(Eigen::Matrix3d rotateMatrix)
{
    float sy = (float)sqrtf(rotateMatrix(0, 0) * rotateMatrix(0, 0) + rotateMatrix(1, 0)*rotateMatrix(1, 0));
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = (float)atan2(rotateMatrix(2, 1), rotateMatrix(2, 2));
        y = (float)atan2(-rotateMatrix(2, 0), sy);
        z = (float)atan2(rotateMatrix(1, 0), rotateMatrix(0, 0));
    }
    else
    {
        x = (float)atan2(-rotateMatrix(1, 2), rotateMatrix(1, 1));
        y = (float)atan2(-rotateMatrix(2, 0), sy);
        z = 0;
    }
    vector<float> i;
    i.push_back((float)(x * (180.0f / PI)));
    i.push_back((float)(y * (180.0f / PI)));
    i.push_back((float)(z * (180.0f / PI)));
    return i;
}

std::vector<float> GetRotationQuat(std::vector<float> Quat1, std::vector<float> Quat2) //1为上一帧,2为下一帧
{
    std::vector<float> output(3,0);
    if (Quat1.size() != 4 || Quat2.size() != 4)
    {
        return output;
    }
    float SqrSum = 0;
    for (int i = 0; i < Quat1.size(); ++i)
    {
        SqrSum = SqrSum + Quat1[i]* Quat1[i];
    }
    float MQuat1 = sqrtf(SqrSum);
    std::vector<float> Quat1Inv(4);
    Quat1Inv[0] = Quat1[0] / MQuat1;
    Quat1Inv[1] = -Quat1[1] / MQuat1;
    Quat1Inv[2] = -Quat1[2] / MQuat1;
    Quat1Inv[3] = -Quat1[3] / MQuat1;
    Eigen::Quaterniond Quat;
    Quat.w() = Quat1Inv[0] * Quat2[0] - Quat1Inv[1] * Quat2[1] - Quat1Inv[2] * Quat2[2] - Quat1Inv[3] * Quat2[3];
    Quat.x() = Quat2[1] * Quat1Inv[0] + Quat2[0] * Quat1Inv[1] + Quat2[2] * Quat1Inv[3] - Quat2[3] * Quat1Inv[2];
    Quat.y() = Quat2[2] * Quat1Inv[0] + Quat2[0] * Quat1Inv[2] + Quat2[3] * Quat1Inv[1] - Quat2[2] * Quat1Inv[3];
    Quat.z() = Quat2[3] * Quat1Inv[0] + Quat2[0] * Quat1Inv[3] + Quat2[1] * Quat1Inv[2] - Quat2[2] * Quat1Inv[1];
    Eigen::Matrix3d rotMatrix = Quat.toRotationMatrix();
    vector<float> RotVec(3, 0);
    if (Quat1 == Quat2)
    {
        return RotVec;
    }
    else
    {
        RotVec = matrix2angle(rotMatrix);
    }
    //cout << "Rotation_x" << endl << RotVec[0] << endl;
    //cout << "Rotation_y" << endl << RotVec[1] << endl;
    //cout << "Rotation_z" << endl << RotVec[2] << endl;
    return RotVec;
}