#include <simgear/compiler.h>
#include <simgear/constants.h>

#include <iostream>

#include "route.hxx"
#include "waypoint.hxx"

using std::cout;
using std::endl;

#ifdef _MSC_VER
#include <windows.h>
#include <time.h>
typedef  struct tagGTM { /* gtm */
   BOOL  tm_bt;
   LARGE_INTEGER  tm_lif, tm_lib, tm_lie;
   DWORD tm_dwtc;
   char  tm_buf[264];
}GTM, * PGTM;

class MyTimer
{
public:
    MyTimer() { reset(); };
    ~MyTimer() { };
    double elapsed(void);
    void reset(void);
    char * elap_stg(void);
    char * time_stg(double tm);
    GTM gtm;
};

// === a simple WIN32 timer ===
void MyTimer::reset()
{
    PGTM ptm = &gtm;
    ptm->tm_bt = QueryPerformanceFrequency( &ptm->tm_lif );
    if(ptm->tm_bt)
        QueryPerformanceCounter( &ptm->tm_lib ); // counter value
    else
        ptm->tm_dwtc = GetTickCount(); // ms since system started
}

double MyTimer::elapsed(void)
{
    PGTM ptm = &gtm;
    double db = 0.0;
    if( ptm->tm_bt ) {
        LARGE_INTEGER lid;
        QueryPerformanceCounter( &ptm->tm_lie ); // counter value
        lid.QuadPart = ( ptm->tm_lie.QuadPart - ptm->tm_lib.QuadPart ); // get difference
        db  = (double)lid.QuadPart / (double)ptm->tm_lif.QuadPart;
    } else {
        DWORD dwd = (GetTickCount() - ptm->tm_dwtc);   // ms elapsed
        db = ((double)dwd / 1000.0);
    }
    return db;
}

char * MyTimer::time_stg(double elap)
{
    char * cp = &gtm.tm_buf[0];
    if (elap < 0.000000001) {
        sprintf(cp,"%2.6f ps", elap * 1000000000000.0);
    } else if (elap < 0.000001) {
        sprintf(cp,"%2.6f ns", elap * 1000000000.0);
    } else if (elap < 0.001) {
        sprintf(cp,"%2.6f us", elap * 1000000.0);
    } else if (elap < 1.0) {
        sprintf(cp,"%2.2f ms", elap * 1000.0);
    } else if (elap < 60.0) {
        sprintf(cp,"%2.2f secs", elap);
    } else {
        int secs = (int)(elap + 0.5);
        int mins = (int)(secs / 60);
        secs = (secs % 60);
        if (mins >= 60) {
            int hrs = mins / 60;
            mins = mins % 60;
            sprintf(cp, "%2d:%02d:%02d hh:mm:ss", hrs, mins, secs);
        } else {
            sprintf(cp, "%2d:%02d mm:ss", mins, secs);
        }
    }
    return cp;
}

char * MyTimer::elap_stg(void)
{
    return time_stg(elapsed());
}

SGVec3<double> oldC, newC;
#endif // _MSC_VER

void dump_route(const SGRoute& route, const char* message)
{
    cout << "Route dump: " << message << endl;
    for (int i = 0; i < route.size(); i++) {
        const SGWayPoint wp = route.get_waypoint(i);
        cout << "\t#" << i << " " << wp.get_id() << " (" << wp.get_target_lat()
                << ", " << wp.get_target_lon() << ") @" << wp.get_target_alt()
                << " dist: " << wp.get_distance() << endl;
    }
}

int main()
{
#ifdef _MSC_VER
    int max = 100000000;
    int i;
    int curr_x, curr_y, x, y, ipc, j;
    MyTimer tm;
    double d, d1, d2, av1, av2, pct;
    double tot1, tot2;
    int cnt1, cnt2;
    srand(time(NULL));
    tot1 = tot2 = 0.0;
    printf("Compiled %s, at %s\n", __DATE__, __TIME__);
    for (j = 0; j < 10; j++) {
        curr_x = curr_y = x = y = rand();
        oldC[0] = oldC[1] = oldC[2] = (double)x;
        newC[0] = newC[1] = newC[2] = (double)x;
        cnt1 = cnt2 = 0;
        tm.reset();
        for (i = 0; i < max; i++) {
            if ( !((curr_x == x) && (curr_y == y)) )
                printf("impossible\n");
            else
                cnt1++;
        }
        d1 = tm.elapsed();
        tot1 += d1;
        printf("1: Elapsed %s, ", tm.time_stg(d1));
        av1 = d1 / (double)max;
        printf("av. %s (for %d iterations)\n", tm.time_stg(av1), cnt1);
        i = 0;
        tm.reset();
        for (i = 0; i < max; i++) {
            if ( !(oldC == newC) )
                printf("impossible\n");
            else
                cnt2++;
        }
        d2 = tm.elapsed();
        tot2 += d2;
        printf("2: Elapsed %s, ", tm.time_stg(d2));
        av2 = d2 / (double)max;
        printf("av. %s (for %d interations)\n", tm.time_stg(av2), cnt2);
        if (d2 > d1) {
            d = d2 - d1;
            pct = (d2 / d1) * 100.0;
            ipc = (int)(pct * 100.0);
            printf("1 is %.1f %% faster than 2 (%g) diff %s\n",
                ((double)ipc / 100.0), pct, tm.time_stg(d));
        } else if (d2 < d1) {
            d = d1 - d2;
            pct = (d1 / d2) * 100.0;
            ipc = (int)(pct * 100.0);
            printf("2 is %.1f %% faster than 1 (%g) diff %s\n",
                ((double)ipc / 100.0), pct, tm.time_stg(d));
        } else {
            printf("Same average speed\n");
        }
    }
    printf("Total tot1 = %s, ", tm.time_stg(tot1));
    printf("tot2 = %s\n", tm.time_stg(tot2));
    i = 0;
#endif
    SGRoute route;
/*
    route.add_waypoint( SGWayPoint(0, 0, 0, SGWayPoint::CARTESIAN, "Start") );
    route.add_waypoint( SGWayPoint(1, 0, 0, SGWayPoint::CARTESIAN, "1") );
    route.add_waypoint( SGWayPoint(2, 0, 0, SGWayPoint::CARTESIAN, "2") );
    route.add_waypoint( SGWayPoint(2, 2, 0, SGWayPoint::CARTESIAN, "3") );
    route.add_waypoint( SGWayPoint(4, 2, 0, SGWayPoint::CARTESIAN, "4") );

    dump_route(route, "Init");
    route.set_current( 1 );

    cout << "( 0.5, 0 ) = " << route.distance_off_route( 0.5, 0 ) << endl;
    cout << "( 0.5, 1 ) = " << route.distance_off_route( 0.5, 1 ) << endl;
    cout << "( 0.5, -1 ) = " << route.distance_off_route( 0.5, 1 ) << endl;

    route.set_current( 3 );

    cout << "( 2, 4 ) = " << route.distance_off_route( 2, 4 ) << endl;
    cout << "( 2.5, 4 ) = " << route.distance_off_route( 2.5, 4 ) << endl;

    SGWayPoint wp2 = route.get_waypoint(2);
    route.delete_waypoint(2);
    dump_route(route, "removed WP2");

    route.add_waypoint(wp2, 3);
    dump_route(route, "added back WP2 after WP3");
*/
    return 0;
}
