#define PI           3.14159265358979323846  /* pi */
#define DEG2RADs(x) ((PI / 180.0) * x)
#define RAD2DEGs(x) ((180.0 / PI) * x)
#define SQR(x) (x * x)


class Coordinate {
public:
    public: double lat;
    public: double lon;
    public: double rad;

    public: Coordinate(double lat, double lon, double rad){
        this->lat=lat; this->lon=lon; this->rad=rad;
    }

};


    Coordinate triangulate(Coordinate a, Coordinate b, Coordinate c){

        //Constants
        double earthR = 6371;
        double LatA = a.lat;
        double LonA = a.lon;
        double DistA = a.rad;
        double LatB = b.lat;
        double LonB = b.lon;
        double DistB = b.rad;
        double LatC = c.lat;
        double LonC = c.lon;
        double DistC = c.rad;

        //using authalic sphere
        //if using an ellipsoid this step is slightly different
        //Convert geodetic Lat/Long to ECEF xyz
        //   1. Convert Lat/Long to radians
        //   2. Convert Lat/Long(radians) to ECEF
        double xA = earthR *(cos(DEG2RADs(LatA)) * cos(DEG2RADs(LonA)));
        double yA = earthR *(cos(DEG2RADs(LatA)) * sin(DEG2RADs(LonA)));
        double zA = earthR *(sin(DEG2RADs(LatA)));

        double xB = earthR *(cos(DEG2RADs(LatB)) * cos(DEG2RADs(LonB)));
        double yB = earthR *(cos(DEG2RADs(LatB)) * sin(DEG2RADs(LonB)));
        double zB = earthR *(sin(DEG2RADs(LatB)));

        double xC = earthR *(cos(DEG2RADs(LatC)) * cos(DEG2RADs(LonC)));
        double yC = earthR *(cos(DEG2RADs(LatC)) * sin(DEG2RADs(LonC)));
        double zC = earthR *(sin(DEG2RADs(LatC)));

        //REQUIRE C++ 11
        vector<double> P1 = {xA,yA,zA};
        vector<double> P2 = {xB, yB, zB};
        vector<double> P3 = {xC, yC, zC};


        //ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
        //i = dot(ex, P3 - P1)
        //ey = (P3 - P1 - i*ex)/(numpy.linalg.norm(P3 - P1 - i*ex))
        //ez = numpy.cross(ex,ey)
        //d = numpy.linalg.norm(P2 - P1)
        //j = dot(ey, P3 - P1)
        vector<double> ex = vectorDiv( vectorDiff(P2, P1) , vectorNorm(vectorDiff(P2, P1)));
        double i = vectorDot(ex, vectorDiff(P3, P1)); //PROB
        vector<double> diff = vectorDiff(vectorDiff(P3, P1), vectorMul(i, ex));
        vector<double> ey = vectorDiv(diff, vectorNorm(diff));
        vector<double> ez = vectorCross(ex, ey);
        double d = vectorNorm( vectorDiff(P2, P1) );
        double j = vectorDot(ey, vectorDiff(P3, P1));

        //from wikipedia
        //plug and chug using above values
        double x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2*d);
        double y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x);

        //only one case shown here
        double z = sqrt(pow(DistA,2) - pow(x,2) - pow(y,2));

        //triPt is an array with ECEF x,y,z of trilateration point
        //triPt = P1 + x*ex + y*ey + z*ez
        vector<double> triPt = vectorAdd(vectorAdd(P1, vectorMul(x, ex)), vectorAdd(vectorMul(y, ey), vectorMul(z, ez)));

        //convert back to lat/long from ECEF
        //convert to degrees
        double lat = RAD2DEGs(asin(triPt[2] / earthR));
        double lon = RAD2DEGs(atan2(triPt[1],triPt[0]));

        cout << lat << endl;
        cout << lon << endl;
        Coordinate result(lat,lon,0);
        return result;
    }

    private: vector<double> vectorMul(vector<double> a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0]=a[0]*b[0]; result[1]=a[1]*b[1]; result[2]=a[2]*b[2];
        return result;
    }

    private: vector<double> vectorMul(double a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0]=a*b[0]; result[1]=a*b[1]; result[2]=a*b[2];
        return result;
    }

    private: vector<double> vectorDiv(vector<double> a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0]=a[0]/b[0]; result[1]=a[1]/b[1]; result[2]=a[2]/b[2];
        return result;
    }

    private: vector<double> vectorDiv(vector<double> a,double b){
        vector<double> result = {0,0,0};
        result[0]=a[0]/b; result[1]=a[1]/b; result[2]=a[2]/b;
        return result;
    }

    private: vector<double> vectorDiff(vector<double> a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0]=a[0]-b[0]; result[1]=a[1]-b[1]; result[2]=a[2]-b[2];
        return result;
    }

    private: vector<double> vectorAdd(vector<double> a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0]=a[0]+b[0]; result[1]=a[1]+b[1]; result[2]=a[2]+b[2];
        return result;
    }

    private: double vectorDot(vector<double> a,vector<double> b){
        return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]);
    }

    private: vector<double> vectorCross(vector<double> a,vector<double> b){
        vector<double> result = {0,0,0};
        result[0] = a[1]*b[2]-a[2]*b[1];
        result[1] = a[2]*b[0]-a[0]*b[2];
        result[2] = a[0]*b[1]-a[1]*b[0];
        return result;
    }

    private: double vectorNorm(vector<double> a){
        return sqrt(vectorDot(a,a));
    }

    private: vector<double> vectorNormalize(vector<double> a){
        vector<double> result = {0,0,0};
        double mag = sqrt(vectorDot(a,a));
        result[0] = a[0]/mag;
        result[1] = a[1]/mag;
        result[2] = a[2]/mag;
        return result;
    }

};