//
// C++ Implementation: test_geometry
//
// Description: unit test of geometry classes
//
//
// Author: Nicolas PASCAL ; <nicolas.pascal@icare.univ-lille1.fr>, (C) 2012
//
// Copyright: See COPYING file that comes with this distribution
//
//

#include <cstdio>
#include <vector>
#include <iostream>
using namespace std;

#include "geometry.h"
#include "viewing_geometry.h"

 int main(int argc, char *argv[]) {
    cout << "--- Haversine distances computations ---" << endl;
    /*
      compute distance between 2 points :
        P1 : Cluses 46.067° North ; 6.600° East
        P2 : Lille  50.633° North ; 3.067° East
      Should be ~571.37km
    */
    double lat1 = 46.067, lon1 = 6.600;
    double lat2 = 50.633, lon2 = 3.067;
    double d = Geocentric::get_haversine_dist ( lat1, lon1, lat2, lon2 );
    printf ( "Haversine Distance between (%f,%f) and (%f,%f) is %.15f km\n", lat1, lon1, lat2, lon2, d ); // OK, 571.37 waited, 570.749 km

    /* test antipodal points case
       Result should be the half-diameter of the Earth : 20015.10942 km
    */
    lat1 = 0., lat2 = 180.;
    lon1 = 0., lon2 = 0.;
    d = Geocentric::get_haversine_dist ( lat1, lon1, lat2, lon2 );
    printf ( "Haversine Distance between (%f,%f) and (%f,%f) is %.15f km\n", lat1, lon1, lat2, lon2, d ); // OK, 20015.109375 obtained

    /** test coordinates conversions **/
    Carthesian::Point3D p_car (0., 1., 0.);
    cout << p_car << endl;
    cout << p_car.to_spherical() << endl;
    cout << p_car.to_geocentric() << endl;

    cout << "--- coordinates conversions ---" << endl;

    /* Cluses 46.067° North ; 6.600° East ; altitude 220 m  */
    double lat = 46.067, lon = 6.600, alt = 220.;
    Geocentric::Point3D p_ecr (lat, lon, alt);
    cout << p_ecr << endl;
    cout << p_ecr.to_spherical() << endl;
    cout << p_ecr.to_carthesian() << endl;

    p_car = p_ecr.to_carthesian();
    cout << p_ecr << " =?= " << p_car.to_geocentric() << endl;

    Spherical::Point3D p_sph (p_ecr.to_spherical());
    cout << p_ecr << " =?= " << p_sph.to_geocentric() << endl;

    /* geocentric -> carthesian */
    cout << "geocentric -> carthesian" << endl;
    lat = -75.416664, lon = -95.183823, alt = 0.;
    p_ecr.set (lat, lon, alt);
    cout << p_ecr << endl;
    cout << p_ecr.to_carthesian() << endl;

    /* carthesian -> geocentric */
    cout << "carthesian -> geocentric" << endl;
    p_car = p_ecr.to_carthesian();
    cout << p_car << endl;
    cout << p_car.to_geocentric() << endl;

    /* geodetic -> carthesian */
    Geodetic::Point3D p_geod;
    cout << "geodetic -> carthesian" << endl;
    lat = -75.416664, lon = -95.183823, alt = 0.;
//     lat = -45., lon = -95.183823, alt = 4000.;

    p_geod.set (lat, lon, alt);
    p_car = p_geod.to_carthesian();
    cout << p_geod << endl;
    cout << p_car << endl;

    /* carthesian -> geodetic */
    cout << "carthesian -> geodetic" << endl;
    p_geod = p_car.to_geodetic();
    cout << p_car << endl;
    cout << p_geod << endl;

    double v_lat [] = {-75.680290, -75.682732, -75.685165, -75.687607, -75.690048, -75.692490, -75.694931, -75.697372, -75.699814, -75.702248, -75.704689, -75.707130, -75.709572, -75.712013, -75.714447, -75.716888, -75.719330, -75.721764, -75.724205, -75.726639};
    double v_lon [] = {-22.077993, -22.085190, -22.092390, -22.099590, -22.106756, -22.113926, -22.121098, -22.128271, -22.135447, -22.142624, -22.149805, -22.156988, -22.164173, -22.171360, -22.178551, -22.185743, -22.192938, -22.200134, -22.207333, -22.214535};
    double v_alt [] = {0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 1.000000, 3.000000, 4.000000, 20.000000, 20.000000, 23.000000, 50.999999, 54.000001, 57.000000, 90.000004, 92.000000, 94.999999, 94.999999, 128.000006};

    int sz = sizeof (v_lat) / sizeof (double);
    for (int i = 0 ; i < sz ; ++i) {
        p_ecr = Geocentric::Point3D (v_lat[i], v_lon[i], v_alt[i]);
        p_sph = p_ecr.to_spherical();
        p_car = p_ecr.to_carthesian();
        cout << p_ecr.lat << "\t" << p_ecr.lon   << "\t" << p_ecr.alt << "\t"
             << p_sph.r   << "\t" << p_sph.theta << "\t" << p_sph.phi << "\t"
             << p_car.x   << "\t" << p_car.y     << "\t" << p_car.z   << endl;
    }

    /** test segments inplementations **/
    /* Cluses : 46.067° North ; 6.600° East ; altitude 220m  */
    lat = 46.067, lon = 6.600, alt = 220.;
    Geodetic::Point3D p_start (lat, lon, alt);
    /* Lille : Lille  50.633° North ; 3.067° East ; altitude 20 m */
    lat = 50.6337, lon = 3.067, alt = 20.;
    Geodetic::Point3D p_end (lat, lon, alt);

    // constructs the segment between the cities
    Carthesian::Segment3D segment (p_start, p_end);
    cout << segment << " length = " << segment.length() << endl;

    /** test intersections between line of sights */
    double v_lat2[4] = {3.063333, 5., 6.579444, 7.};
    double v_lon2[4] = {50.637222, 47., 46.061111, 52.};
    double v_alt2[4] = {6371.0072, 7075.0, 6371.0072, 7075.0};

    // set intersecting points
    vector <Carthesian::Point3D> points (4);
    for (size_t i = 0 ; i < points.size() ; ++i) {
        p_ecr.lat = v_lat2[i];
        p_ecr.lon = v_lon2[i];
        p_ecr.alt = v_alt2[i];
        points [i] = p_ecr.to_carthesian();
    }

    // compute intersection
    Carthesian::Point3D pa, pb;
    Carthesian::Segment3D p1p2 (points[0], points[1]);
    Carthesian::Segment3D p3p4 (points[2], points[3]);
    Carthesian::Segment3D papb (points[2], points[3]);

    double mua, mub;
    cout << "--- intersection between --- " << endl;
//     cout << "p1p2 : " << p1p2 << endl;
//     cout << "p3p4 : " << p3p4 << endl;
    cout << "p1 " << points[0] << " " << points[0].to_spherical() << endl;
    cout << "p2 " << points[1] << " " << points[1].to_spherical() << endl;
    cout << "p3 " << points[2] << " " << points[2].to_spherical() << endl;
    cout << "p4 " << points[3] << " " << points[3].to_spherical() << endl;
    Carthesian::intersection (points[0], points[1], points[2], points[3], pa, pb, mua, mub);
    papb.p1 = pa;
    papb.p2 = pb;
    cout << "pa " << pa << " " << pa.to_spherical() << endl;
    cout << "pb " << pb << " " << pb.to_spherical() << endl;
    cout << "distance = " << papb.length() << endl;

    cout << "--- intersection computing benchmark --- " << endl;
    int niter = 1;
//     int niter = 160000 * 6000; // 112 s
    cout << "niter = " << niter << endl;
    time_t t_start = time (NULL);
    for (int iter = 0 ; iter < niter ; ++iter) {
        Carthesian::intersection (points[0], points[1], points[2], points[3], pa, pb, mua, mub);
    }
    printf ("elapsed time = %d seconds\n", (int)(time (NULL) - t_start));

    cout << "--- vector operations --- " << endl;
    Carthesian::Vector3D v1 (8654.123, 156.67, 983.46);
    Carthesian::Vector3D v2 (7264.456, 85197.348, 492.36);
    cout << v1 << " norm = " << v1.norm() << endl;
    cout << v2 << " norm = " << v2.norm() << endl;
    cout << "v1.v2 = " << Carthesian::dotproduct(v1, v2) << endl;
    cout << "v1^v2 = " << Carthesian::crossproduct(v1, v2) << endl;
    cout << "angle(v1,v2) = " << Carthesian::angle(v1, v2) << " radians ; " << degrees(Carthesian::angle(v1, v2)) << " degrees" << endl;
    // rotation of a given angle, using a vector as axis
    v1.set (-0.703374609048, -0.611046374305, -0.363161792862); // vector to rotate
    Carthesian::Vector3D v_axis (-0.0259774764607, -0.02256755163, -0.999407762793); // rotation axis
    double theta = -64.694;
    v2 = Carthesian::rotate(v1, v_axis, radians (theta)); // apply rotation
    cout << "Rotation of " << v1 << " along axis " << v_axis << " of angle " << theta << endl;
    cout << " -> " << v2 << endl; // must be [ 0.23813831, -0.89326915, -0.38126156]

    cout << "--- earth to satellite operations --- " << endl;
    cout << "* using slant range *" << endl;
    // - test case for MODIS near north pole -
    // lat_v 85.0325622559 lon_v -129.742752075 alt_v 7.0 slant_range 1429250.0 theta_v 65.38 phi_v -62.4
    // p_s [ -887629.86629635   733475.4164815   6992395.87787131]
    Geodetic::Point3D p_earth (85.0325622559, -129.742752075, 7.0); // earth observer position
    double slant_range = 1429250.0;
    double theta_v = 65.38; // zenith angle
    double phi_v   = -62.4; // azimuth angle
    Carthesian::Point3D p_sat;
    Viewing::earth_to_sat_from_range(p_earth, theta_v, phi_v, slant_range, p_sat);

    cout << "Earth observer position : (geolocation) " << p_earth << " (carthesian) " << p_earth.to_carthesian() << endl;
    cout << "Zenith " << theta_v << " Azimuth " << phi_v << " slant range " << endl;
    cout << "Satellite position : (geolocation) " << p_sat.to_geocentric() << " (carthesian) " << p_sat << endl;

    cout << "* using satellite altitude *" << endl;
    // - test case for PARASOL near south pole -
    // p_earth -88.028000000000006, -139.018, 2546
    // theta_v = 66.7335 phi_v = 64.694 alt_sat =  705000
    // p_sat (-78.588766686452743, -86.077482792334692, 717784.57965302095)
    p_earth.set (-88.028000000000006, -139.018, 2546.0);
    theta_v = 66.7335;
    phi_v = 64.694;
    double alt_sat = 705000.0;
    Viewing::earth_to_sat_from_alt(p_earth, theta_v, phi_v, alt_sat, p_sat);
    cout << "Earth observer position : (geolocation) " << p_earth << " (carthesian) " << p_earth.to_carthesian() << endl;
    cout << "Zenith " << theta_v << " Azimuth " << phi_v << " alt_sat " << alt_sat << endl;
    cout << "Satellite position : (geolocation) " << p_sat.to_geocentric() << " (spherical) " << p_sat.to_spherical() << " (carthesian) " << p_sat << endl;


}