/*
 * =====================================================================================
 *
 *       Filename:  test_feature.cpp
 *
 *    Description:  Test the feature class.
 *
 *        Version:  1.0
 *        Created:  03/25/2017 12:43:53 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */


#include "test_feature.h"


/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  test_findDepth
 *  Description:  Test the Feature::findDepth() method.
 * =====================================================================================
 */
bool
test_findDepth ( Camera &cam )
{
    bool success;
    UTM pos;
    /* 16.bmp
    pos.northing = 4451490.899494356;
    pos.easting = 381887.311030777;
    pos.up = 206.256890383;
    */
    // 9494.bmp
    pos.northing = 4451693.139997130;
    pos.easting = 382292.412884794;
    pos.up = 203.230833591;
    pos.zone_i = 16;
    pos.zone_c = 'T';

    /*
    // Quat for 16.bmp
    attitude_t att{ -1.073641623878,2.685395530695,16.357370508605};
    */
    // Quat for 9494.bmp
    attitude_t att{ 1.669725078950,4.920310214269,49.251704514034};

    double roll, pitch, yaw;
    roll = M_PI*att.roll/180.;
    pitch = M_PI*att.pitch/180.;
    yaw = M_PI*(att.yaw)/180.;
    Matrix3d R;
    Quaterniond q;
    R = AngleAxisd(yaw, Vector3d::UnitZ())
      * AngleAxisd(roll, Vector3d::UnitX())
      *  AngleAxisd(pitch, Vector3d::UnitY());
    q = Quaterniond(R);

    Matrix<double,48,5> meas;
    meas << 94,201,258,192,509,
    184,210,265,200,480,
    588,297,265,296,386,
    614,483,267,487,316,
    810,352,251,355,415,
    996,147,267,142,384,
    1062,182,250,179,510,
    1070,82,246,73,416,
    1072,280,278,282,373,
    1146,274,257,272,402,
    1152,156,282,149,382,
    1274,249,263,246,393,
    1370,134,253,123,419,
    1432,238,265,242,381,
    1474,235,239,242,431,
    1520,270,276,271,368,
    1588,479,217,463,444,
    1640,249,273,252,363,
    1692,158,236,142,463,
    1724,271,255,272,387,
    1784,187,260,185,383,
    1792,335,266,338,348,
    1812,111,242,96,422,
    1820,59,226,49,437,
    1826,193,208,193,507,
    1834,160,245,154,412,
    1856,184,169,191,502,
    1862,288,219,293,422,
    1870,220,235,216,432,
    1872,309,283,309,348,
    1938,89,237,81,414,
    1976,161,189,168,496,
    1984,192,187,196,506,
    2002,156,226,153,421,
    2004,273,208,273,471,
    2008,63,267,59,390,
    2014,306,267,308,363,
    2034,205,235,201,400,
    2084,149,167,148,518,
    2114,243,295,239,330,
    2120,143,223,141,417,
    2134,176,188,163,470,
    2142,194,229,189,400,
    2168,55,237,47,399,
    2192,110,94,115,491,
    2194,221,268,220,353,
    2200,197,118,201,506,
    2220,212,245,212,381;
/*
    meas << 162,493,260,493,331,
        66,370,229,369,352,
        268,763,235,764,364,
        414,554,250,554,337,
        452,365,210,365,370,
        580,353,239,353,344,
        606,455,253,455,333,
        680,748,237,747,357,
        706,650,233,651,358,
        724,671,199,668,400,
        736,768,283,768,318,
        774,368,159,369,370,
        842,281,221,282,352,
        882,748,249,748,349,
        910,386,217,387,369,
        916,713,210,713,383,
        966,519,218,519,368,
        1026,567,264,567,325,
        1050,686,227,686,361,
        1070,650,220,650,370,
        1084,531,267,531,323,
        1160,331,222,332,353,
        1184,388,273,388,314,
        1262,547,219,546,360,
        1274,314,218,313,354,
        1278,469,217,469,365;
        */

    double z;
    z = -0.55;
    z -= 0.65*3.43*sin(pitch);

    Vector3d xbw;
    xbw << pos.northing, pos.easting, -pos.up;

    printf("id,utm_z,utm_c,utm-northing,utm-easting,altitude\n");
    for (int i=0; i<meas.rows(); ++i) {
        Matrix<double,1,5> row;
        row = meas.block<1,5>(i,0);
        int id = row[0];

        Vector3d xsi,xri,xs,xr;
        xsi << row[1], row[2], 1;
        xri << row[3], row[4], 1;

        xs = cam.img2body(xsi);
        xr = cam.img2body(xri);

        Feature f(id,xs,xr,xbw,q,z);
        //Feature f(id,xs,xbw,q);
        if (f.sane()) 
        {
            UTM utmi;
            utmi = f.utm(xbw,q);
            printf("%d,%d,%c,%f,%f,%f\n", id,utmi.zone_i, utmi.zone_c, utmi.northing, utmi.easting, utmi.up);
        }
    }

    success = true;
    return success;
}		/* -----  end of function test_findDepth  ----- */

int main(int argc, char **argv)
{
    Camera cam(argv[1]);
    if (test_findDepth(cam)) {
        ;//printf("PASSED\n");
    } else {
        printf("FAILED\n");
    }
	return 0;
}