/*
 * =====================================================================================
 *
 *       Filename:  vision.cpp
 *
 *    Description:  Class for vision tasks such as opening images, plotting
 *    points in the image frame, performing measurements.
 *
 *        Version:  1.0
 *        Created:  04/07/2017 11:34:16 AM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */
#include "vision.h"


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Vision
 *      Method:  Vision
 * Description:  constructor
 *--------------------------------------------------------------------------------------
 */
Vision::Vision ()
{
    ;
}  /* -----  end of method Vision::Vision  (constructor)  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Vision
 *      Method:  Vision :: open
 * Description:  Opens the given image, undistorts.
 *--------------------------------------------------------------------------------------
 */
void
Vision::open ( const char *fn, const Camera &cam )
{
    Mat bayered ;
    bayered = cv::imread(fn, -1);
    if (!bayered.data) {
        fprintf(stderr, "Could not read %s.\n", fn);
        exit(1);
    }
    cv::cvtColor(bayered, original, CV_BayerBG2BGR ,3);
    display = original.clone();
    return ;
}		/* -----  end of method Vision::open  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Vision
 *      Method:  Vision :: drawMeasurements
 * Description:  Draws the measured points onto the image.
 *--------------------------------------------------------------------------------------
 */
void
Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam,
        const cv::Scalar &colsrc, const cv::Scalar &colref, bool drawEllipse)
{
    cv::Scalar ellcol(0,0,200);
    for (auto i=z.begin(); i!=z.end(); ++i) {
        if (i->z_type==HEIGHT) continue;
        cerr << i->id << endl;
        Vector3d xis, xir;
        xis = cam.body2img(i->source);
        xis /= xis[2];
        xir = cam.body2img(i->reflection);
        xir /= xir[2];
        cv::Point ps(xis[0],xis[1]);
        cv::Point pr(xir[0],xir[1]);

        cv::circle(display, ps, 3, colsrc,2);
        cv::circle(display, pr, 3, colref);

        if (drawEllipse) {
            double xs, ys, xr, yr;
            xs = 2*sqrt(i->Ssrc(0));
            ys = 2*sqrt(i->Ssrc(1));
            xr = 2*sqrt(i->Sref(0));
            yr = 2*sqrt(i->Sref(1));
            
            // Create rotated rect for ellipse
            cv::RotatedRect rrs(ps, cv::Size(2*5.9*xs, 2*5.9*ys), 0.);
            cv::RotatedRect rrr(pr, cv::Size(2*5.9*xr, 2*5.9*yr), 0.);
            cv::ellipse(display, rrs, ellcol );
            cv::ellipse(display, rrr, ellcol);
        }
    }
    return ;
}		/* -----  end of method Vision::drawMeasurements  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Vision
 *      Method:  Vision :: show
 * Description:  Show the display image.
 *--------------------------------------------------------------------------------------
 */
void
Vision::show (  )
{
    cv::imshow("Image view", display);
    cv::waitKey(1);
    return ;
}		/* -----  end of method Vision::show  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Vision
 *      Method:  Vision :: acquireFeatures
 * Description:  Finds new features, assigns IDs, extracts descriptors.
 *--------------------------------------------------------------------------------------
 */
void
Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z )
{
    vector<cv::Point2f> corners;
    Mat gray;
    cv::cvtColor(original, gray, CV_BGR2GRAY);
    cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20);
    for (auto i=corners.begin(); i!=corners.end(); ++i) {
        measurement_t m;
        m.id = _id++;
        m.z_type = MONO;
        Vector3d xi;
        xi << i->x, i->y, 1.;
        m.source = cam.img2body(xi);
        m.reflection << 0,0,1;
        z.push_back(m);
    }
    return ;
}		/* -----  end of method Vision::acquireFeatures  ----- */

int Vision::_id = 1;