/*
 * =====================================================================================
 *
 *       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;
    pos.northing = 4451490.899494356;
    pos.easting = 381887.311030777;
    pos.up = 206.256890383;
    pos.zone_i = 16;
    pos.zone_c = 'T';

    // Quat for 16.bmp
    attitude_t att{ -1.073641623878,2.685395530695,16.357370508605};

    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,26,5> meas;
    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.34;
    z -= 0.6*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);
        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;
}