/****************************************************************************/
/* Copyright 2017 MBARI.                                                    */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
#include "lcmUtils.h"

#include <string>
#include <iostream>

#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>

#include <lcm/lcm-cpp.hpp>
#include "mbot/double_array_t.hpp"

#define DISPLAY_BLOBS           1
#define PERFOM_EIGNEN_LIB_TEST  1

#define STD_OUT_MODULUS         10

#if PERFOM_EIGNEN_LIB_TEST
#include <Eigen/Dense>
#endif

void testEigenLib();

int main(int argc, char *argv[]) 
{
    double start, end, loop_start, loop_end;

    // opencv image capture and storage 
    cv::VideoCapture capLeft, capRight;
    cv::Mat imageLeft, imageRight, imLeftKeypoints, imRightKeypoints;

    // parameters for simple blob detector
    cv::SimpleBlobDetector::Params params;

    // simple blob detector
    cv::Ptr<cv::SimpleBlobDetector> detector;

    // storage for blobs
    std::vector<cv::KeyPoint> keypointsLeft, keypointsRight;

    // LCM variables
    lcm::LCM lcm;
    mbot::double_array_t blobPositions;

    //////////////////////////////////////////////////////////////////////////
    // Filter by Color
    //////////////////////////////////////////////////////////////////////////
    // Note: this feature appears to be broken
    //
    // 0 for dark blobs
    // 255 for light blobs
    //
    params.filterByColor = true;
    params.blobColor = 255;

    //////////////////////////////////////////////////////////////////////////
    // Set the thresholds
    //////////////////////////////////////////////////////////////////////////
    //http://answers.opencv.org/question/60374/simpleblobdetector-threshold/
    //
    // In your problem, since you have a dark area with very bright spots, 
    // the maxThreshold shouldn't be used (set it to 255 which is maximum 
    // for grayscale 8bit images), and minThreshold should be rather high 
    // (i'd start at 40 and go up till my bright spots start to disappear), 
    // to ignore most dark areas and keep only the bright spots in the image.
    //
    // You want this threshold to discard everything but the bright spots, 
    // so you have to set minThreshold to as high as you can without 
    // compromising bright spots.
    //
//    params.minThreshold = 175;
    params.minThreshold = 125;
    params.maxThreshold = 255;
 
    //////////////////////////////////////////////////////////////////////////
    // Filter by Area.
    //////////////////////////////////////////////////////////////////////////
    // Number of pixels in the blob
    //
    params.filterByArea = true;
//    params.minArea = 1500;
    params.minArea = 1000;
    params.maxArea = 25000;
  
    //////////////////////////////////////////////////////////////////////////
    // Filter by Inertia
    //////////////////////////////////////////////////////////////////////////
    //https://www.learnopencv.com/blob-detection-using-opencv-python-c/
    //
    // This measures how elongated a shape is. E.g. for a circle, this
    // value is 1, for an ellipse it is between 0 and 1, and for a line 
    // it is 0. To filter by inertia ratio, set filterByInertia = 1, and 
    // set 0 ≤ minInertiaRatio ≤ 1 and maxInertiaRatio (≤ 1 ) appropriately. 
    //
//    params.filterByInertia = true;
    params.filterByInertia = false;
    params.minInertiaRatio = 0.0;
    params.maxInertiaRatio = 0.25;

    //////////////////////////////////////////////////////////////////////////
    // Filter by Circularity
    //////////////////////////////////////////////////////////////////////////
    params.filterByCircularity = false;
    params.minCircularity = 0.1;
 
    //////////////////////////////////////////////////////////////////////////
    // Filter by Convexity
    //////////////////////////////////////////////////////////////////////////
    params.filterByConvexity = false;
    params.minConvexity = 0.87;

    // create a detector with the parameters above
    detector = cv::SimpleBlobDetector::create(params);   

    //////////////////////////////////////////////////////////////////////////
    // Display the startup message
    //////////////////////////////////////////////////////////////////////////
    std::cout << "blobDetector built on " << __DATE__ << " at " << __TIME__;
    std::cout << std::endl;

    std::cout << "Using OpenCV " << CV_VERSION << std::endl;

    //////////////////////////////////////////////////////////////////////////
    // Check LCM status and init message
    //////////////////////////////////////////////////////////////////////////

    // make sure lcm initialized OK
    if ( !lcm.good() )
    {
        std::cout << "LCM failed to initialize " << std::endl;
        return -1;
    } 

    // initialize MBOT_BLOB_POS message
    blobPositions.epoch_time =  getTimeStamp();
    blobPositions.sequence = 0;
    blobPositions.sender_name = getPublisherName();
    blobPositions.sender_pid = getPublisherPid();

    blobPositions.array_size = 4;
    blobPositions.data.resize(blobPositions.array_size);

    for (int i = 0; i < blobPositions.array_size; i++)
        blobPositions.data[i] = -1.0;

    //////////////////////////////////////////////////////////////////////////
    // Optional test of Eigen Lib
    //////////////////////////////////////////////////////////////////////////

    testEigenLib();

    //////////////////////////////////////////////////////////////////////////
    // Open capture device, either hardware or sample videos
    //////////////////////////////////////////////////////////////////////////

    if (argc < 3)
    {
        capLeft.open(0);
        capRight.open(1);
    }    
    else
    {
        capLeft.open(argv[1]);
        capRight.open(argv[2]);
    }

    //if you have a capture object start reading images
    if (capLeft.isOpened() && capRight.isOpened())
    {
        std::cout << "Capture is opened" << std::endl;

        // main processing loop
        for (;;)
        {
            // grab the time at start of loop
            loop_start = getTimeStamp();

            if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
            {
                std::cout << "***********************************************";
                std::cout << std::endl;

                std::cout << "sequence  " << blobPositions.sequence;
                std::cout << std::endl;
            }    

            // capture images 
            capLeft.read(imageLeft);
            capRight.read(imageRight);

            if (imageLeft.empty() || imageRight.empty())
                break;

            // detect blobs
            start = getTimeStamp();
            detector->detect(imageLeft, keypointsLeft);
            detector->detect(imageRight, keypointsRight);
            end = getTimeStamp();

            if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
            {
                std::cout << "detect    " << (end - start) << " (s)";
                std::cout << std::endl;

                std::cout << "keypointsLeft.size(): " << keypointsLeft.size();
                std::cout << std::endl;
                std::cout << "keypointsRight.size(): " << keypointsRight.size();
                std::cout << std::endl;
            }    

            if ( keypointsLeft.size() > 0 )
            {
                blobPositions.data[0] = (double)keypointsLeft[0].pt.x;
                blobPositions.data[1] = (double)keypointsLeft[0].pt.y;

                if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
                {
                    std::cout << "keypointsLeft[0].pt = ";
                    std::cout << keypointsLeft[0].pt << std::endl;
                    std::cout << "keypointsLeft[0].size = ";
                    std::cout << keypointsLeft[0].size << std::endl;
                }    
            }
            else
            {
                blobPositions.data[0] = -1.0;
                blobPositions.data[1] = -1.0;

                if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
                {
                    std::cout << "keypointsLeft[0].pt = N/A" << std::endl;
                    std::cout << "keypointsLeft[0].size = N/A" << std::endl;
                }
            }  

            if ( keypointsRight.size() > 0 )
            {
                blobPositions.data[2] = (double)keypointsRight[0].pt.x;
                blobPositions.data[3] = (double)keypointsRight[0].pt.y;

                if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
                {
                    std::cout << "keypointsRight[0].pt = "; 
                    std::cout << keypointsRight[0].pt << std::endl;
                    std::cout << "keypointsRight[0].size = ";
                    std::cout << keypointsRight[0].size << std::endl;
                }    
            }    
            else
            {
                blobPositions.data[2] = -1.0;
                blobPositions.data[3] = -1.0;

                if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
                {
                    std::cout << "keypointsRight[0].pt = N/A" << std::endl;
                    std::cout << "keypointsRight[0].size = N/A" << std::endl;
                }
            }

            // update LCM timestamp and publish
            blobPositions.epoch_time =  getTimeStamp();
            lcm.publish("MBOT_BLOB_POS", &blobPositions);

#if DISPLAY_BLOBS            
            // draw detected blobs as red circles.
            // DrawMatchesFlags::DRAW_RICH_KEYPOINTS flag ensures
            // the size of the circle corresponds to the size of blob
            drawKeypoints(imageLeft, keypointsLeft, 
                          imLeftKeypoints, cv::Scalar(0,0,255), 
                          cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

            drawKeypoints(imageRight, keypointsRight, 
                          imRightKeypoints, cv::Scalar(0,0,255), 
                          cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);


            // display the blobs
            imshow("blobs left", imLeftKeypoints);
            imshow("blobs right", imRightKeypoints);

            if ( cv::waitKey(1) >= 0 )
                break;
#endif
            // grab the time at end of loop
            loop_end = getTimeStamp();

            // display loop exec time
            if ((blobPositions.sequence % STD_OUT_MODULUS) == 0)
            {
                std::cout << "loop time " << (loop_end - loop_start) << " (s)";
                std::cout << std::endl;
            }    

            // increment the LCM sequence then rinse and repeat
            ++blobPositions.sequence;

            std::cout.flush();
        }
    }

    // release the capture resources
    capLeft.release();
    capRight.release();

    return 0;
}

void testEigenLib()
{
#if PERFOM_EIGNEN_LIB_TEST
    
    Eigen::Matrix3d M;
    Eigen::Matrix3d Mt;
    Eigen::Matrix3d Mi;
    Eigen::Matrix3d A;
    Eigen::Vector3d v1;
    Eigen::Vector3d v2;

    M(0,0) = 1.0;
    M(1,0) = 0.0;
    M(2,0) = 5.0;

    M(0,1) = 2.0;
    M(1,1) = 1.0;
    M(2,1) = 6.0;

    M(0,2) = 3.0;
    M(1,2) = 4.0;
    M(2,2) = 0.0;

    v1(0) = 1.0;
    v1(1) = 2.0;
    v1(2) = 3.0;

    std::cout << "Performing Eigen lib test"  << std::endl << std::endl;

    std::cout << "Matrix M:" << std::endl << M << std::endl;
    std::cout << std::endl;

    Mt = M.transpose();

    std::cout << "Transpose Matrix M (Mt):" << std::endl << Mt << std::endl;
    std::cout << std::endl;

    Mi = M.inverse();

    std::cout << "Inverse Matrix M (Mi):" << std::endl << Mi << std::endl;
    std::cout << std::endl;

    A = M * Mi;

    std::cout << std::endl;

    std::cout << "A = M * Mi:" << std::endl << A << std::endl;
    std::cout << std::endl;

    std::cout << "v1:" << std::endl << v1 << std::endl;
    std::cout << std::endl;

    v2 = M * v1;

    std::cout << "v2 = M * v1:" << std::endl << v2 << std::endl;
    std::cout << std::endl;

#else
    std::cout << "Eigen lib test disabled" << std::endl;
#endif

    return;
}
