/* Copyright (c) 2012 Patrick Ruoff
 * Copyright (c) 2014-2016 Stanislaw Halik <sthalik@misaki.pl>
 * Copyright (c) 2019 Stephane Lenclud
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 */

#include "tracker-easy.h"
#include "video/video-widget.hpp"
#include "compat/math-imports.hpp"
#include "compat/check-visible.hpp"
#include "point-extractor.h"
#include "tracker-easy-api.h"

#include <QHBoxLayout>
#include <QDebug>
#include <QFile>
#include <QCoreApplication>

#include <opencv2/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <iostream>

using namespace options;

// Disable debug
#define dbgout if (true) {} else std::cout
// Enable debug
//#define dbgout if (false) {} else std::cout

namespace EasyTracker
{

    Tracker::Tracker() :
        s{ KModuleName },
        iPointExtractor{ std::make_unique<PointExtractor>() },
        iPreview{ preview_width, preview_height }
    {
        cv::setBreakOnError(true);
        cv::setNumThreads(1);

        connect(s.b.get(), &bundle_::saving, this, &Tracker::maybe_reopen_camera, Qt::DirectConnection);
        connect(s.b.get(), &bundle_::reloading, this, &Tracker::maybe_reopen_camera, Qt::DirectConnection);

        connect(&s.fov, value_::value_changed<int>(), this, &Tracker::set_fov, Qt::DirectConnection);
        set_fov(s.fov);
    }

    Tracker::~Tracker()
    {
        //
        cv::destroyWindow("Preview");

        requestInterruption();
        wait();

        QMutexLocker l(&camera_mtx);
        camera->stop();        
    }


    // Compute Euler angles from ratation matrix
    cv::Vec3f EulerAngles(cv::Mat &R)
    {

        float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));

        bool singular = sy < 1e-6; // If

        float x, y, z;
        if (!singular)
        {
            x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
            y = atan2(-R.at<double>(2, 0), sy);
            z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
        }
        else
        {
            x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
            y = atan2(-R.at<double>(2, 0), sy);
            z = 0;
        }

        // Convert to degrees
        return cv::Vec3f(x * 180 / CV_PI, y * 180 / CV_PI, z * 180 / CV_PI);
    }


    void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles)
    {

        cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ;
        double* _r = rotCamerMatrix.ptr<double>();
        double projMatrix[12] = { _r[0],_r[1],_r[2],0,
                              _r[3],_r[4],_r[5],0,
                              _r[6],_r[7],_r[8],0 };

        cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix),
            cameraMatrix,
            rotMatrix,
            transVect,
            rotMatrixX,
            rotMatrixY,
            rotMatrixZ,
            eulerAngles);
    }


    void Tracker::run()
    {
        maybe_reopen_camera();

        while (!isInterruptionRequested())
        {
            bool new_frame = false;

            {
                QMutexLocker l(&camera_mtx);

                if (camera)
                {
                    std::tie(iFrame, new_frame) = camera->get_frame();
                }
                    
            }

            if (new_frame)
            {
                // Create OpenCV matrix from our frame
                // TODO: Assert channel size is one or two
                iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE((iFrame.channelSize == 2 ? CV_16U : CV_8U), iFrame.channels), iFrame.data, iFrame.stride);


                const bool preview_visible = check_is_visible();
                if (preview_visible)
                {
                    iPreview = iMatFrame;
                }

                iPoints.clear();
                iPointExtractor->extract_points(iMatFrame, (preview_visible ? &iPreview.iFrameRgb : nullptr), iPoints);
                point_count.store(iPoints.size(), std::memory_order_relaxed);

                const bool success = iPoints.size() >= KPointCount;

                int topPointIndex = -1;

                {
                    QMutexLocker l(&center_lock);

                    if (success)
                    {
                        ever_success.store(true, std::memory_order_relaxed);

                        // Solve P3P problem with OpenCV

                        // Construct the points defining the object we want to detect based on settings.
                        // We are converting them from millimeters to centimeters.
                        // TODO: Need to support clip too. That's cap only for now.
                        // s.active_model_panel != PointModel::Clip

                        std::vector<cv::Point3f> objectPoints;
                        objectPoints.push_back(cv::Point3f(s.cap_x / 10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right
                        objectPoints.push_back(cv::Point3f(-s.cap_x / 10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left
                        objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top

                        //Bitmap origin is top left
                        std::vector<cv::Point2f> trackedPoints;
                        // Stuff bitmap point in there making sure they match the order of the object point  
                        // Find top most point, that's the one with min Y as we assume our guy's head is not up side down

                        int minY = std::numeric_limits<int>::max();
                        for (int i = 0; i < 3; i++)
                        {
                            if (iPoints[i][1] < minY)
                            {
                                minY = iPoints[i][1];
                                topPointIndex = i;
                            }
                        }

                        int rightPointIndex = -1;
                        int maxX = 0;

                        // Find right most point 
                        for (int i = 0; i < 3; i++)
                        {
                            // Excluding top most point
                            if (i != topPointIndex && iPoints[i][0] > maxX)
                            {
                                maxX = iPoints[i][0];
                                rightPointIndex = i;
                            }
                        }

                        // Find left most point
                        int leftPointIndex = -1;
                        for (int i = 0; i < 3; i++)
                        {
                            // Excluding top most point
                            if (i != topPointIndex && i != rightPointIndex)
                            {
                                leftPointIndex = i;
                                break;
                            }
                        }

                        //
                        trackedPoints.push_back(cv::Point2f(iPoints[rightPointIndex][0], iPoints[rightPointIndex][1]));
                        trackedPoints.push_back(cv::Point2f(iPoints[leftPointIndex][0], iPoints[leftPointIndex][1]));
                        trackedPoints.push_back(cv::Point2f(iPoints[topPointIndex][0], iPoints[topPointIndex][1]));

                        dbgout << "Object: " << objectPoints << "\n";
                        dbgout << "Points: " << trackedPoints << "\n";


                        // Create our camera matrix
                        // TODO: Just do that once, use data member instead
                        // Double or Float?
                        cv::Mat cameraMatrix;
                        cameraMatrix.create(3, 3, CV_64FC1);
                        cameraMatrix.setTo(cv::Scalar(0));
                        cameraMatrix.at<double>(0, 0) = iCameraInfo.focalLengthX;
                        cameraMatrix.at<double>(1, 1) = iCameraInfo.focalLengthY;
                        cameraMatrix.at<double>(0, 2) = iCameraInfo.principalPointX;
                        cameraMatrix.at<double>(1, 2) = iCameraInfo.principalPointY;
                        cameraMatrix.at<double>(2, 2) = 1;

                        // Create distortion cooefficients
                        cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1);
                        // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6
                        distCoeffs.at<double>(0, 0) = 0; // Radial first order
                        distCoeffs.at<double>(1, 0) = iCameraInfo.radialDistortionSecondOrder; // Radial second order
                        distCoeffs.at<double>(2, 0) = 0; // Tangential first order
                        distCoeffs.at<double>(3, 0) = 0; // Tangential second order
                        distCoeffs.at<double>(4, 0) = 0; // Radial third order
                        distCoeffs.at<double>(5, 0) = iCameraInfo.radialDistortionFourthOrder; // Radial fourth order
                        distCoeffs.at<double>(6, 0) = 0; // Radial fith order
                        distCoeffs.at<double>(7, 0) = iCameraInfo.radialDistortionSixthOrder; // Radial sixth order

                        // Define our solution arrays
                        // They will receive up to 4 solutions for our P3P problem


                        // TODO: try SOLVEPNP_AP3P too
                        iAngles.clear();
                        iBestSolutionIndex = -1;
                        int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P);

                        if (solutionCount > 0)
                        {
                            dbgout << "Solution count: " << solutionCount << "\n";
                            int minPitch = std::numeric_limits<int>::max();
                            // Find the solution we want
                            for (int i = 0; i < solutionCount; i++)
                            {
                                dbgout << "Translation:\n";
                                dbgout << iTranslations.at(i);
                                dbgout << "\n";
                                dbgout << "Rotation:\n";
                                //dbgout << rvecs.at(i);
                                cv::Mat rotationCameraMatrix;
                                cv::Rodrigues(iRotations[i], rotationCameraMatrix);
                                cv::Vec3d angles;
                                getEulerAngles(rotationCameraMatrix, angles);
                                iAngles.push_back(angles);

                                // Check if pitch is closest to zero
                                int absolutePitch = std::abs(angles[0]);
                                if (minPitch > absolutePitch)
                                {
                                    minPitch = absolutePitch;
                                    iBestSolutionIndex = i;
                                }

                                //cv::Vec3f angles=EulerAngles(quaternion);
                                dbgout << angles;
                                dbgout << "\n";
                            }

                            dbgout << "\n";

                        }

                    }

                    // Send solution data back to main thread
                    QMutexLocker l2(&data_lock);
                    if (iBestSolutionIndex != -1)
                    {
                        iBestAngles = iAngles[iBestSolutionIndex];
                        iBestTranslation = iTranslations[iBestSolutionIndex];
                    }

                }

                if (preview_visible)
                {                    
                    //
                    if (topPointIndex != -1)
                    {
                        // Render a cross to indicate which point is the head
                        iPreview.draw_head_center(iPoints[topPointIndex][0], iPoints[topPointIndex][1]);
                    }

                    // Show full size preview pop-up
                    if (s.debug)
                    {
                        cv::imshow("Preview", iPreview.iFrameRgb);
                        cv::waitKey(1);
                    }

                    // Update preview widget
                    widget->update_image(iPreview.get_bitmap());

                    auto[w, h] = widget->preview_size();
                    if (w != preview_width || h != preview_height)
                    {
                        // Resize preivew if widget size has changed
                        preview_width = w; preview_height = h;
                        iPreview = Preview(w, h);
                    }
                }
                else
                {
                    // No preview, destroy preview pop-up
                    if (s.debug)
                    {
                        cv::destroyWindow("Preview");
                    }                    
                }
            }
        }
    }

    bool Tracker::maybe_reopen_camera()
    {
        QMutexLocker l(&camera_mtx);

        if (camera->is_open())
        {
            return true;
        }

        iCameraInfo.fps = s.cam_fps;
        iCameraInfo.width = s.cam_res_x;
        iCameraInfo.height = s.cam_res_y;

        return camera->start(iCameraInfo);
    }

    void Tracker::set_fov(int value)
    {
        QMutexLocker l(&camera_mtx);

    }

    module_status Tracker::start_tracker(QFrame* video_frame)
    {
        //video_frame->setAttribute(Qt::WA_NativeWindow);

        widget = std::make_unique<video_widget>(video_frame);
        layout = std::make_unique<QHBoxLayout>(video_frame);
        layout->setContentsMargins(0, 0, 0, 0);
        layout->addWidget(widget.get());
        video_frame->setLayout(layout.get());
        //video_widget->resize(video_frame->width(), video_frame->height());
        video_frame->show();

        // Create our camera
        camera = video::make_camera(s.camera_name);

        start(QThread::HighPriority);

        return {};
    }

    void Tracker::data(double *data)
    {
        if (ever_success.load(std::memory_order_relaxed))
        {
            // Get data back from tracker thread
            QMutexLocker l(&data_lock);
            data[Yaw] = iBestAngles[1];
            data[Pitch] = iBestAngles[0];
            data[Roll] = iBestAngles[2];
            data[TX] = iBestTranslation[0];
            data[TY] = iBestTranslation[1];
            data[TZ] = iBestTranslation[2];
        }
    }

    bool Tracker::center()
    {
        QMutexLocker l(&center_lock);
        //TODO: Do we need to do anything there?
        return false;
    }

    int Tracker::get_n_points()
    {
        return (int)point_count.load(std::memory_order_relaxed);
    }

}