/*
Copyright 2017. All rights reserved.
Computer Vision Group, Visual Computing Institute
RWTH Aachen University, Germany

This file is part of the rwth_mot framework.
Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de)

rwth_mot framework is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include "utils_bounding_box.h"
//std
#include<cmath>
// eigen
#include <Eigen/Core>
// openCV
#include "opencv2/imgproc.hpp"

//qhull
#include"libqhullcpp/Qhull.h"
#include"libqhullcpp/RboxPoints.h"

// utils
#include "utils_filtering.h"
#include "camera.h"
#include "utils_common.h"

namespace SUN {
    namespace utils {
        namespace bbox {

            Eigen::Vector4d Intersection2d(const Eigen::Vector4d &rect1, const Eigen::Vector4d &rect2) {
                const double rect1_x = rect1[0];
                const double rect1_y = rect1[1];
                const double rect1_w = rect1[2];
                const double rect1_h = rect1[3];

                const double rect2_x = rect2[0];
                const double rect2_y = rect2[1];
                const double rect2_w = rect2[2];
                const double rect2_h = rect2[3];

                const double left = rect1_x > rect2_x ? rect1_x : rect2_x;
                const double top = rect1_y > rect2_y ? rect1_y : rect2_y;
                double lhs = rect1_x + rect1_w;
                double rhs = rect2_x + rect2_w;
                const double right = lhs < rhs ? lhs : rhs;
                lhs = rect1_y + rect1_h;
                rhs = rect2_y + rect2_h;
                const double bottom = lhs < rhs ? lhs : rhs;

                Eigen::Vector4d rect_intersection;
                rect_intersection[0] = right < left ? 0 : left;
                rect_intersection[1] = bottom < top ? 0 : top;
                rect_intersection[2] = right < left ? 0 : right - left;
                rect_intersection[3] = bottom < top ? 0 : bottom - top;

                return rect_intersection;
            }

            double IntersectionOverUnion2d(const Eigen::Vector4d &rect1, const Eigen::Vector4d &rect2) {
                Eigen::Vector4d rect_intersection = Intersection2d(rect1, rect2);
                const double intersection_area = rect_intersection[2] * rect_intersection[3]; // Surface of the intersection of the rects
                const double union_of_rects = rect1[2] * rect1[3] + rect2[2] * rect2[3] - intersection_area; // Union of the area of the rects
                return intersection_area / union_of_rects; // Intersection over union
            }

            Eigen::Vector4d BoundingBox2d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr scene_cloud,
                                          const std::vector<int> &indices, double percentage) {

                pcl::PointIndices filtered_indices = SUN::utils::filter::FilterPointCloudBasedOnRadius(scene_cloud, indices, percentage);

                int bbx_min = static_cast<int>(1e10);
                int bbx_max = static_cast<int>(-1e10);
                int bby_min = static_cast<int>(1e10);
                int bby_max = static_cast<int>(-1e10);

                for (auto ind:filtered_indices.indices) {
                    int x = -1, y = -1;
                    UnravelIndex(ind, scene_cloud->width, &x, &y);
                    if (x < bbx_min)
                        bbx_min = x;
                    if (x > bbx_max)
                        bbx_max = x;
                    if (y < bby_min)
                        bby_min = y;
                    if (y > bby_max)
                        bby_max = y;
                }

                // [min_x min_y w h]
                Eigen::Vector4d bb2d_out;
                bb2d_out[0] = static_cast<double>(bbx_min);
                bb2d_out[1] = static_cast<double>(bby_min);
                bb2d_out[2] = static_cast<double>(bbx_max - bbx_min);
                bb2d_out[3] = static_cast<double>(bby_max - bby_min);

                return bb2d_out;
            }

            Eigen::Vector4d BoundingBox2d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr object_cloud, const SUN::utils::Camera &cam, double percentage) {
                auto filtered_cloud = SUN::utils::filter::FilterPointCloudBasedOnRadius(object_cloud,  percentage);

                int bbx_min = static_cast<int>(1e10);
                int bbx_max = static_cast<int>(-1e10);
                int bby_min = static_cast<int>(1e10);
                int bby_max = static_cast<int>(-1e10);

                for (const auto &pt:filtered_cloud->points) {
                    int x, y;

                    auto proj_pt = cam.CameraToImage(pt.getVector4fMap().cast<double>());
                    x = proj_pt[0];
                    y = proj_pt[1];

                    if (x < bbx_min)
                        bbx_min = x;
                    if (x > bbx_max)
                        bbx_max = x;
                    if (y < bby_min)
                        bby_min = y;
                    if (y > bby_max)
                        bby_max = y;
                }

                // [min_x min_y w h]
                Eigen::Vector4d bb2d_out;
                bb2d_out[0] = static_cast<double>(bbx_min);
                bb2d_out[1] = static_cast<double>(bby_min);
                bb2d_out[2] = static_cast<double>(bbx_max - bbx_min);
                bb2d_out[3] = static_cast<double>(bby_max - bby_min);

                return bb2d_out;
            }

            Eigen::VectorXd BoundingBox3d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud_in, double percentage) {
                // Remove some points (radius-based cleaning)
                auto cloud_to_process = SUN::utils::filter::FilterPointCloudBasedOnRadius(cloud_in, percentage);

                // Compute mean, covariance matrix 3d
                Eigen::Matrix3d cov_mat3d;
                Eigen::Vector4d mean3d;
                pcl::computeMeanAndCovarianceMatrix(*cloud_to_process, cov_mat3d, mean3d);

                // Let's restrict ourselves to 2D ground-plane projection. More robust.
                Eigen::Matrix2d cov_mat2d;
                cov_mat2d(0, 0) = cov_mat3d(0, 0);
                cov_mat2d(1, 1) = cov_mat3d(2, 2);
                cov_mat2d(0, 1) = cov_mat3d(0, 2);
                cov_mat2d(1, 0) = cov_mat3d(2, 0);

                // Compute Eigen vectors, values.
                // Here, we get 2 eigenvectors, corresponding to dominant axes of 2D proj. of object (to the ground plane).
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigen_solver(cov_mat2d, Eigen::ComputeEigenvectors);
                Eigen::Matrix2d eigen_vectors = eigen_solver.eigenvectors();

                // Def. local coord. sys.
                Eigen::Matrix3d p2w(Eigen::Matrix3d::Identity());
                p2w.block<2, 2>(0, 0) = eigen_vectors.transpose();
                Eigen::Vector2d c_2d(mean3d[0], mean3d[2]); // Mean on gp. proj.
                p2w.block<2, 1>(0, 2) = -1.f * (p2w.block<2, 2>(0, 0) * c_2d); //centroid.head<2>());

                // Find bbox extent (width, depth)
                float bbx_min = static_cast<float>(1e4);
                float bbx_max = static_cast<float>(-1e4);
                float bbz_min = static_cast<float>(1e4);
                float bbz_max = static_cast<float>(-1e4);

                for (int i = 0; i <cloud_to_process->size(); i++) {

                    const auto &p_ref = cloud_to_process->at(i);
                    Eigen::Vector3d p_eig(p_ref.x, p_ref.z, 1.0);

                    if (std::isnan(p_ref.x))
                        continue;

                    p_eig = p2w * p_eig;

                    const float tmp_x = p_eig[0];
                    const float tmp_z = p_eig[1];

                    if (tmp_x < bbx_min)
                        bbx_min = tmp_x;
                    if (tmp_x > bbx_max)
                        bbx_max = tmp_x;
                    if (tmp_z < bbz_min)
                        bbz_min = tmp_z;
                    if (tmp_z > bbz_max)
                        bbz_max = tmp_z;
                }

                // Find out orientation
                Eigen::Matrix3d R_mat;
                R_mat.setIdentity();
                R_mat(0, 0) = eigen_vectors(0, 0);
                R_mat(0, 2) = eigen_vectors(0, 1);
                R_mat(2, 0) = eigen_vectors(1, 0);
                R_mat(2, 2) = eigen_vectors(1, 1);
                const Eigen::Vector2d mean_diag = 0.5f * (Eigen::Vector2d(bbx_min + bbx_max, bbz_min + bbz_max));
                const Eigen::Vector2d tfinal = eigen_vectors * mean_diag + Eigen::Vector2d(mean3d[0], mean3d[2]);

                // Final transform
                Eigen::Quaterniond qfinal(R_mat);
                double bb1 = bbx_max - bbx_min;
                double bb3 = bbz_max - bbz_min;

                // Get height
                Eigen::Vector4f cloud_min, cloud_max;
                pcl::getMinMax3D(*cloud_to_process, cloud_min, cloud_max);
                double min_y = cloud_min[1];
                double max_y = cloud_max[1];
                double cloud_height = std::abs(max_y - min_y); // Difference between Y-coords.

                // Resulting data structure
                Eigen::VectorXd bb3d_out;
                bb3d_out.setZero(10, 1); // center_x, center_y, center_z, width, height, depth, quaternion

                // X-Z center from 2d-gp-PCA, Y compute from 3D points
                bb3d_out(0) = tfinal[0];
                bb3d_out(1) = min_y + (max_y - min_y) / 2.0;
                bb3d_out(2) = tfinal[1];

                // Width, height, depth
                bb3d_out(3) = std::abs(bb1);
                bb3d_out(4) = std::abs(cloud_height);
                bb3d_out(5) = std::abs(bb3);

                // Quaternion, representing orientation
                //qfinal.setIdentity();
                bb3d_out(6) = qfinal.w();
                bb3d_out(7) = qfinal.x();
                bb3d_out(8) = qfinal.y();
                bb3d_out(9) = qfinal.z();

                return bb3d_out;
            }

            Eigen::VectorXd BoundingBox3d(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr scene_cloud,
                                          const std::vector<int> &indices, double percentage) {
                // Remove some points (radius-based cleaning)
                auto filtered_indices = SUN::utils::filter::FilterPointCloudBasedOnRadius(scene_cloud, indices, percentage);

                // Compute mean, covariance matrix 3d
                Eigen::Matrix3d cov_mat3d;
                Eigen::Vector4d mean3d;
                pcl::computeMeanAndCovarianceMatrix(*scene_cloud, indices, cov_mat3d, mean3d);

                // Let's restrict ourselves to 2D ground-plane projection. More robust.
                Eigen::Matrix2d cov_mat2d;
                cov_mat2d(0, 0) = cov_mat3d(0, 0);
                cov_mat2d(1, 1) = cov_mat3d(2, 2);
                cov_mat2d(0, 1) = cov_mat2d(1, 0) = cov_mat3d(0, 2);

                // Compute Eigen vectors, values..
                // Here, we get 2 eigenvectors, corresponding to dominant axes of 2D proj. of object (to the ground plane).
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigen_solver(cov_mat2d, Eigen::ComputeEigenvectors);
                Eigen::Matrix2d eigen_vectors = eigen_solver.eigenvectors();

                // Def. local coord. sys.
                Eigen::Matrix3d p2w(Eigen::Matrix3d::Identity());
                p2w.block<2, 2>(0, 0) = eigen_vectors.transpose();
                Eigen::Vector2d c_2d(mean3d[0], mean3d[2]); // Mean on gp. proj.
                p2w.block<2, 1>(0, 2) = -1.f * (p2w.block<2, 2>(0, 0) * c_2d); //centroid.head<2>());

                // Find bbox extent (width, depth)
                float bbx_min = 1e10;
                float bbx_max = -1e10;
                float bbz_min = 1e10;
                float bbz_max = -1e10;

                for (int i = 0; i < filtered_indices.indices.size(); i++) {
                    int ind = filtered_indices.indices.at(i);
                    const auto &p_ref = scene_cloud->at(ind);

                    if (std::isnan(p_ref.x))
                        continue;

                    Eigen::Vector3d p_eig(p_ref.x, p_ref.z, 1.0);
                    p_eig = p2w * p_eig;

                    const double tmp_x = p_eig[0];
                    const double tmp_z = p_eig[1];

                    if (tmp_x < bbx_min)
                        bbx_min = tmp_x;
                    if (tmp_x > bbx_max)
                        bbx_max = tmp_x;
                    if (tmp_z < bbz_min)
                        bbz_min = tmp_z;
                    if (tmp_z > bbz_max)
                        bbz_max = tmp_z;
                }

                // Find out orientation
                Eigen::Matrix3d R_mat;
                R_mat.setIdentity();
                R_mat(0, 0) = eigen_vectors(0, 0);
                R_mat(0, 2) = eigen_vectors(0, 1);
                R_mat(2, 0) = eigen_vectors(1, 0);
                R_mat(2, 2) = eigen_vectors(1, 1);
                const Eigen::Vector2d mean_diag = 0.5f * (Eigen::Vector2d(bbx_min + bbx_max, bbz_min + bbz_max));
                const Eigen::Vector2d tfinal = eigen_vectors * mean_diag + Eigen::Vector2d(mean3d[0], mean3d[2]);

                // Final transform
                const Eigen::Quaterniond qfinal(R_mat);
                double bb1 = bbx_max - bbx_min;
                double bb3 = bbz_max - bbz_min;

                // Get height
                Eigen::Vector4f cloud_min, cloud_max;
                pcl::getMinMax3D(*scene_cloud, filtered_indices, cloud_min, cloud_max);
                double min_y = cloud_min[1];
                double max_y = cloud_max[1];
                double cloud_height = std::abs(max_y - min_y); // Difference between Y-coords.

                // Resulting data structure
                Eigen::VectorXd bb3d_out;
                bb3d_out.setZero(10, 1); // center_x, center_y, center_z, width, height, depth, quaternion

                // X-Z center from 2d-gp-PCA, Y compute from 3D points
                bb3d_out(0) = tfinal[0];
                bb3d_out(1) = min_y + (max_y - min_y) / 2.0;
                bb3d_out(2) = tfinal[1];

                // Width, height, depth
                bb3d_out(3) = std::abs(bb1);
                bb3d_out(4) = std::abs(cloud_height);
                bb3d_out(5) = std::abs(bb3);

                // Quaternion, representing orientation
                bb3d_out(6) = qfinal.w();
                bb3d_out(7) = qfinal.x();
                bb3d_out(8) = qfinal.y();
                bb3d_out(9) = qfinal.z();

                return bb3d_out;
            }

            Eigen::Vector4d ReparametrizeBoundingBoxCenterMidToLeftTopPoint(const Eigen::Vector4d &bounding_box_2d) {
                auto cx = bounding_box_2d[0];
                auto cy = bounding_box_2d[1];
                auto w = bounding_box_2d[2];
                auto h = bounding_box_2d[3];
                return Eigen::Vector4d(cx - (w / 2.0), cy - (h / 2.0), w, h);
            }

            Eigen::Vector4d ReparametrizeBoundingBoxCenterTopToLeftTopPoint(const Eigen::Vector4d &bounding_box_2d) {
                auto cx = bounding_box_2d[0];
                auto cy = bounding_box_2d[1];
                auto w = bounding_box_2d[2];
                auto h = bounding_box_2d[3];
                return Eigen::Vector4d(cx - (w / 2.0), cy, w, h);
            }
            /**
               * @brief converts 3D bounding box with centriod, height, width, length to 8 corner bounding box.
               *        Works only for box parallel to ground plane. Also, with assume no rotation except around y-axis.
               * @param point: the one centriod of the bounding box.
               * @param h: hieght of the bounding box.
               * @param w: width of the boudning box.
               * @param l: length of the bounding box.
               * @param rotationY: rotation around the y axis.
               * @return box3d: matrix with shape=[3,8], each column represents a corner
               * @author Omran kaddah.omran@gmail.com.
               */
            Eigen::MatrixXd Convert3DBoxTo8Corner(const Eigen::Vector3d &point, const double &h, const double &w, const double &l,const double rotationY){
                double c = std::cos(rotationY);
                double s = std::sin(rotationY);
                Eigen::Matrix3d R;
                R.setZero();
                R << c, 0, s,
                     0, 1, 0,
                    -s, 0, c;
                Eigen::MatrixXd box3d(3,8);
                box3d.row(0) << l/2,  l/2, - l/2, -l/2, l/2,  l/2,  -l/2,  -l/2;
                box3d.row(1) << 0  ,  0   ,  0  ,  0,   -h,   -h,   -h,      -h;
                box3d.row(2) << w/2, -w/2, -w/2,  w/2  ,w/2  ,-w/2,  -w/2,   w/2;
                box3d = R * box3d;
                box3d.row(0) = box3d.row(0) + Eigen::RowVectorXd::Constant(1,8,point[0]);
                box3d.row(1) = box3d.row(1) + Eigen::RowVectorXd::Constant(1,8,point[1]);
                box3d.row(2) = box3d.row(2) + Eigen::RowVectorXd::Constant(1,8,point[2]);
                return box3d;

            }

            /**
               * @brief converts 3D bounding box with centriod, height, width, length to 8 corner bounding box.
               * @ref: https://rosettacode.org/wiki/Sutherland-Hodgman_polygon_clipping
               *        Works only for box parallel to ground plane. Also, with assume no rotation except around y-axis.
               * @param point: the one centriod of the bounding box.
               * @param rotationY: rotation around the y axis.
               * @return box3d: matrix with shape=[3,8], each column represents a corner
               * @author Omran kaddah.omran@gmail.com.
               */
            double ConvexHullIntersection (std::vector<cv::Point2f> subjectPolygon, std::vector<cv::Point2f> clipPolygon) {
                auto inside = [](cv::Point2f p, cv::Point2f p1, cv::Point2f p2){
                    return (p2.y - p1.y) * p.x + (p1.x - p2.x) * p.y + (p2.x * p1.y - p1.x * p2.y) < 0;
                    };
                auto intersection = [](cv::Point2f cp1, cv::Point2f cp2, cv::Point2f s, cv::Point2f e){
                    cv::Point2f dc;
                    dc.x = cp1.x - cp2.x;
                    dc.y = cp1.y - cp2.y;
                    cv::Point2f dp;
                    dp.x = s.x - e.x;
                    dp.y =  s.y - e.y;
                    float n1 = cp1.x * cp2.y - cp1.y * cp2.x;
                    float n2 = s.x * e.y - s.y * e.x;
                    float n3 = 1.0 / (dc.x * dp.y - dc.y * dp.x);
                    cv::Point2f returned;
                    returned.x = (n1 * dp.x - n2 * dc.x) * n3;
                    returned.y = (n1 * dp.y - n2 * dc.y) * n3;
                    return returned;
                    };
                // ****************************
                // Sutherland-Hodgman clipping
                // ****************************
                int subjectPolygonSize = subjectPolygon.size();
                int clipPolygonSize = clipPolygon.size();
                std::vector<cv::Point2f> newPolygon = std::vector<cv::Point2f>(8);
                int newPolygonSize;
                cv::Point2f cp1, cp2, s, e;
                std::vector<cv::Point2f> inputPolygon = std::vector<cv::Point2f>(8);

                // copy subject polygon to new polygon and set its size
                for(int i = 0; i < subjectPolygonSize; i++)
                    newPolygon[i] = subjectPolygon[i];
                newPolygonSize = subjectPolygonSize;

                for(int j = 0; j < clipPolygonSize; j++)
                {
                    // copy new polygon to input polygon & set counter to 0
                    for(int k = 0; k < newPolygonSize; k++){ inputPolygon[k] = newPolygon[k]; }

                    int counter = 0;

                    // get clipping polygon edge
                    cp1 = clipPolygon[j];
                    cp2 = clipPolygon[(j + 1) % clipPolygonSize];

                    for(int i = 0; i < newPolygonSize; i++)
                    {
                        // get subject polygon edge
                        s = inputPolygon[i];
                        e = inputPolygon[(i + 1) % newPolygonSize];

                        // Case 1: Both vertices are inside:
                        // Only the second vertex is added to the output list
                        if(inside(s, cp1, cp2) && inside(e, cp1, cp2))
                            newPolygon[counter++] = e;

                        // Case 2: First vertex is outside while second one is inside:
                        // Both the point of intersection of the edge with the clip boundary
                        // and the second vertex are added to the output list
                        else if(!inside(s, cp1, cp2) && inside(e, cp1, cp2))
                        {
                            newPolygon[counter++] = intersection(cp1, cp2, s, e);
                            newPolygon[counter++] = e;
                        }

                        // Case 3: First vertex is inside while second one is outside:
                        // Only the point of intersection of the edge with the clip boundary
                        // is added to the output list
                        else if(inside(s, cp1, cp2) && !inside(e, cp1, cp2))
                            newPolygon[counter++] = intersection(cp1, cp2, s, e);

                        // Case 4: Both vertices are outside
                        else if(!inside(s, cp1, cp2) && !inside(e, cp1, cp2))
                        {
                            // No vertices are added to the output list
                        }
                    }
                    // set new polygon size
                    newPolygonSize = counter;
                }
                // ***********************************
                // End of Sutherland-Hodgman clipping
                // ***********************************
                if (newPolygonSize!= 0)
                {
                    for( int remaining = 8 - newPolygonSize; remaining>0; --remaining )
                        newPolygon.pop_back();
                    std::vector<cv::Point2f> hull;
                    cv::convexHull(cv::Mat(newPolygon), hull, true);
                    double area = 0;
                    for (int i = 0; i < hull.size(); i++){
                        int next_i = (i+1)%(hull.size());
                        double dX   = hull[next_i].x - hull[i].x;
                        double avgY = (hull[next_i].y + hull[i].y)/2;
                        area += dX*avgY;  // This is the integration step.
                    }
                    // std::vector<cv::Point2f> contour;
                    // cv::approxPolyDP(cv::Mat(hull), contour, 0.001, true);
                    // double area2 = std::fabs(cv::contourArea(cv::Mat(contour)));
                    return std::fabs(area);
                }

                return 0;
            }

            /**
               * @brief Computes Intersection Over Union IoU in 3D.
               *        Works only for box parallel to ground plane. Also, with assume no rotation except around y-axis.
               * @param corners2: matrix with shape=[3,8], each column represents a corner.
               * @param corners2: matrix with shape=[3,8], each column represents a corner.
               * @return IoU: double
               * @author Omran kaddah.omran@gmail.com.
               */
            double IntersectionOverUnion3d(const Eigen::MatrixXd &corners1, const Eigen::MatrixXd &corners2) {
                std::vector<cv::Point2f> rect1;
                std::vector<cv::Point2f> rect2;
                cv::Point2f pt;
                for( int i =3; i>=0; i--){

                    pt.x = corners1(0,i);
                    pt.y = corners1(2,i);
                    rect1.push_back(pt);
                    pt.x = corners2(0,i);
                    pt.y = corners2(2,i);
                    rect2.push_back(pt);
                }
                double interArea = ConvexHullIntersection(rect1, rect2);
                double ymax = std::min(corners1(1,0), corners2(1,0));
                double ymin = std::max(corners1(1,4), corners2(1,4));
                double inter_vol = interArea * std::max(0.0, ymax-ymin);
                auto boxVol = [](const Eigen::MatrixXd & corners){
                    double volum = std::sqrt((corners.col(0) - corners.col(1)).array().pow(2).sum());
                    double volum1 = std::sqrt((corners.col(1) - corners.col(2)).array().pow(2).sum());
                    double volum2 = std::sqrt((corners.col(0) - corners.col(4)).array().pow(2).sum());
                    return volum * volum1 * volum2;
                };

                return inter_vol / (boxVol(corners1) + boxVol(corners2) - inter_vol);
            }
        }
    }
}


#pragma once
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/opencv.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>

#include <vitis/ai/demo.hpp>
#include <vitis/ai/facedetect.hpp>
#include <iostream>

using namespace std;
namespace vitis {
namespace ai {

struct FaceSwap {
  static std::unique_ptr<FaceSwap> create();
  FaceSwap();
  std::vector<int> run(std::vector<cv::Mat> &input_image);
  int run(cv::Mat &input_image);

  int getInputWidth();
  int getInputHeight();
  size_t get_input_batch();

private:
  std::unique_ptr<vitis::ai::FaceDetect> face_detect_;
  dlib::shape_predictor sp;
  int frame_number = 0;
  bool debug;
  FaceDetectResult faces;
  bool faulty_face = false;
  std::vector<dlib::rectangle> dlibRects;

	std::vector<std::vector<cv::Point2f>> points;

  // Apply affine transform calculated using srcTri and dstTri to src
  void applyAffineTransform(cv::Mat &warpImage, cv::Mat &src, std::vector<cv::Point2f> &srcTri, std::vector<cv::Point2f> &dstTri)
  {
      // Given a pair of triangles, find the affine transform.
      cv::Mat warpMat = getAffineTransform( srcTri, dstTri );

      // Apply the Affine Transform just found to the src image
      warpAffine( src, warpImage, warpMat, warpImage.size(), cv::INTER_LINEAR, cv::BORDER_REFLECT_101);
  }

  // Calculate Delaunay triangles for set of points
  // Returns the vector of indices of 3 points for each triangle
  static void calculateDelaunayTriangles(cv::Rect rect, std::vector<cv::Point2f> &points, std::vector< std::vector<int> > &delaunayTri){

    // Create an instance of Subdiv2D
      cv::Subdiv2D subdiv(rect);

    // Insert points into subdiv
      for( std::vector<cv::Point2f>::iterator it = points.begin(); it != points.end(); it++)
          subdiv.insert(*it);

    std::vector<cv::Vec6f> triangleList;
    subdiv.getTriangleList(triangleList);
    std::vector<cv::Point2f> pt(3);
    std::vector<int> ind(3);

    for( size_t i = 0; i < triangleList.size(); i++ )
    {
      cv::Vec6f t = triangleList[i];
      pt[0] = cv::Point2f(t[0], t[1]);
      pt[1] = cv::Point2f(t[2], t[3]);
      pt[2] = cv::Point2f(t[4], t[5]);

      if ( rect.contains(pt[0]) && rect.contains(pt[1]) && rect.contains(pt[2])){
        for(int j = 0; j < 3; j++)
          for(size_t k = 0; k < points.size(); k++)
            if(abs(pt[j].x - points[k].x) < 1.0 && abs(pt[j].y - points[k].y) < 1)
              ind[j] = k;

        delaunayTri.push_back(ind);
      }
    }
  }

  // Warps and alpha blends triangular regions from img1 and img2 to img
  void warpTriangle(cv::Mat &img1, cv::Mat &img2, std::vector<cv::Point2f> &t1, std::vector<cv::Point2f> &t2)
  {
      cv::Rect r1 = boundingRect(t1);
      cv::Rect r2 = boundingRect(t2);

      // Offset points by left top corner of the respective rectangles
      std::vector<cv::Point2f> t1Rect, t2Rect;
      std::vector<cv::Point> t2RectInt;
      for(int i = 0; i < 3; i++)
      {
          t1Rect.push_back( cv::Point2f( t1[i].x - r1.x, t1[i].y -  r1.y) );
          t2Rect.push_back( cv::Point2f( t2[i].x - r2.x, t2[i].y - r2.y) );
          t2RectInt.push_back( cv::Point(t2[i].x - r2.x, t2[i].y - r2.y) ); // for fillConvexPoly
      }

      // Get mask by filling triangle
      cv::Mat mask = cv::Mat::zeros(r2.height, r2.width, CV_32FC3);
      fillConvexPoly(mask, t2RectInt, cv::Scalar(1.0, 1.0, 1.0), 16, 0);

      // Apply warpImage to small rectangular patches
      cv::Mat img1Rect;
      img1(r1).copyTo(img1Rect);

      cv::Mat img2Rect = cv::Mat::zeros(r2.height, r2.width, img1Rect.type());

      applyAffineTransform(img2Rect, img1Rect, t1Rect, t2Rect);

      cv::multiply(img2Rect,mask, img2Rect);
      cv::multiply(img2(r2), cv::Scalar(1.0,1.0,1.0) - mask, img2(r2));
      img2(r2) = img2(r2) + img2Rect;
  }

void swap(cv::Mat img1, cv::Mat& img2, std::vector<cv::Point2f> points1, std::vector<cv::Point2f> points2) {
      cv::Mat img1Warped = img2.clone();

      //convert Mat to float data type
      img1.convertTo(img1, CV_32F);
      img1Warped.convertTo(img1Warped, CV_32F);

      // Find convex hull
      std::vector<cv::Point2f> hull1;
      std::vector<cv::Point2f> hull2;
      std::vector<int> hullIndex;

      cv::convexHull(points2, hullIndex, false, false);

      for(unsigned int i = 0; i < hullIndex.size(); i++)
      {
          hull1.push_back(points1[hullIndex[i]]);
          hull2.push_back(points2[hullIndex[i]]);
      }

      // Find delaunay triangulation for points on the convex hull
      std::vector< std::vector<int> > dt;
      cv::Rect rect(0, 0, img1Warped.cols, img1Warped.rows);
      calculateDelaunayTriangles(rect, hull2, dt);

      // Apply affine transformation to Delaunay triangles
      for(size_t i = 0; i < dt.size(); i++)
      {
          std::vector<cv::Point2f> t1, t2;
          // Get points for img1, img2 corresponding to the triangles
          for(size_t j = 0; j < 3; j++)
          {
              t1.push_back(hull1[dt[i][j]]);
              t2.push_back(hull2[dt[i][j]]);
          }
          warpTriangle(img1, img1Warped, t1, t2);
      }

      // Calculate mask
      std::vector<cv::Point> hull8U;
      for(unsigned int i = 0; i < hull2.size(); i++)
      {
          cv::Point pt(hull2[i].x, hull2[i].y);
          hull8U.push_back(pt);
      }

      cv::Mat mask = cv::Mat::zeros(img2.rows, img2.cols, img2.depth());
      cv::fillConvexPoly(mask,&hull8U[0], hull8U.size(), cv::Scalar(255,255,255));

      // Clone seamlessly.
      cv::Rect r = boundingRect(hull2);
      cv::Point center = (r.tl() + r.br()) / 2;

      img1Warped.convertTo(img1Warped, CV_8UC3);
      seamlessClone(img1Warped,img2, mask, center, img2, cv::NORMAL_CLONE);
      return;
  }

  std::vector<std::vector<cv::Point2f>> landmark_detection(dlib::cv_image<dlib::rgb_pixel> dlibIm, std::vector<dlib::rectangle> rects, dlib::shape_predictor sp) {
      std::vector<std::vector<cv::Point2f>> both_points;
      for (unsigned long j = 0; j < 2; ++j)
      {
          dlib::full_object_detection shape = sp(dlibIm, rects[j]);
          if (shape.num_parts() == 68){
              std::vector<cv::Point2f> points;
              for(int i = 0; i < 68; ++i){
                  points.push_back(cv::Point2f((float)shape.part(i)(0),(float)shape.part(i)(1)));
              }
              both_points.push_back(points);
          }
          else{
        	  faulty_face = true;
        	  return both_points;
          }
      }
      faulty_face = false;
      return both_points;
  }
};

std::unique_ptr<FaceSwap> FaceSwap::create() {
  return std::unique_ptr<FaceSwap>(new FaceSwap());
}
int FaceSwap::getInputWidth() { return face_detect_->getInputWidth(); }
int FaceSwap::getInputHeight() { return face_detect_->getInputHeight(); }
size_t FaceSwap::get_input_batch() { return face_detect_->get_input_batch(); }

FaceSwap::FaceSwap()
    : face_detect_{vitis::ai::FaceDetect::create("densebox_640_360")}
{
  dlib::deserialize("shape_predictor_68_face_landmarks.dat") >> sp;
  const char * val = std::getenv( "FACESWAP_DEBUG" );
  if ( val == nullptr ) {
    debug = false;
  }
  else {
    cout << "[INFO] FACESWAP_DEBUG" << endl;
    debug = true;
  }
}

std::vector<int>
FaceSwap::run(std::vector<cv::Mat> &input_image) {
  if ( debug == true ) {
    cout << "[INFO] batch mode" << endl;
  }
  auto batch_size = get_input_batch();
  std::vector<cv::Mat> input_tmp;
  std::vector<int> faceswap_results;
  size_t ind = 0;
  while(ind < input_image.size()) {
    size_t next = std::min(ind + batch_size, input_image.size());
    for(size_t i = ind; i < next; i++) {
      input_tmp.push_back(input_image[i]);
    }
    auto face_detect_results = face_detect_->run(input_tmp);
    input_tmp.clear();
    for (size_t i = 0; i < face_detect_results.size(); i++){
		if(face_detect_results[i].rects.size() >= 2){
			int good_faces = 0;
			for (const auto &r : face_detect_results[i].rects) {
				if ( r.score < 0.90 ) continue;
				good_faces++;
			}
			if (good_faces >= 2){
				cv::Mat image;
				image = input_image[ind+i];
				std::vector<dlib::rectangle> dlibRects;
				for (const auto &r : face_detect_results[i].rects) {
					std::vector<long> tl = {(long)(r.x * image.cols), (long)(r.y * image.rows)};
					std::vector<long> br = {tl[0] + (long)(r.width * image.cols), tl[1] + (long)(r.height * image.rows)};
					dlibRects.push_back(dlib::rectangle(tl[0],tl[1],br[0],br[1]));
				}
				std::vector<std::vector<cv::Point2f>> points;
				dlib::cv_image<dlib::rgb_pixel> dlibIm(image);
				points = landmark_detection(dlibIm,dlibRects,sp);
				cv::Mat imgClone = image.clone();
				swap(image,image, points[1], points[0]);
				swap(imgClone,image, points[0], points[1]);
				faceswap_results.push_back(0);
			}
			else{
				faceswap_results.push_back(1);
			}
		}
		else{
			faceswap_results.push_back(2);
		}
    }
	ind = ind + batch_size;
  }
  return faceswap_results;
}

int FaceSwap::run(cv::Mat &image) {
	frame_number++;
  // only process 110 frames
	if (frame_number > 110){
		std::cout << "EXIT" << std::endl;
		exit(2);
	}
	if ( debug == true ) {
		std::cout << "[INFO] single file mode" << endl;
		std::cout << "Frame " << frame_number << endl;
	}
	faces = face_detect_->run(image);
	if(faces.rects.size() >= 2){
		int good_faces = 0;
		for (const auto &r : faces.rects) {
			if ( r.score < 0.90 ) continue;
			good_faces++;
		}
		if (good_faces >= 2){
			dlibRects.clear();
			for (const auto &r : faces.rects) {
				std::vector<long> tl = {(long)(r.x * image.cols), (long)(r.y * image.rows)};
				std::vector<long> br = {tl[0] + (long)(r.width * image.cols), tl[1] + (long)(r.height * image.rows)};
				dlibRects.push_back(dlib::rectangle(tl[0],tl[1],br[0],br[1]));
			}
			dlib::cv_image<dlib::rgb_pixel> dlibIm(image);
			points = landmark_detection(dlibIm,dlibRects,sp);
			if (faulty_face == false){
				cv::Mat imageClone = image.clone();
				swap(image,image, points[1], points[0]);
				swap(imageClone,image, points[0], points[1]);
				return 0;
			}
		}
	}
	return 1;
}

} // namespace ai
} // namespace vitis


#include "stdafx.h"
#include "ObjectModel.h"
#include "NetworkCase.h"
#include "Exceptions.h"
#include "Utility.h"
#include "DCFlowSolver.h"
#include <algorithm>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

using namespace std;
using namespace PowerSolutions::ObjectModel;
using namespace Eigen;

namespace PowerSolutions
{
	namespace PowerFlow
	{
		DCFlowSolver::~DCFlowSolver()
		{
		}

		shared_ptr<Solution> DCFlowSolver::Solve(const ObjectModel::NetworkCase& network)
		{
			auto pn = network.ToPrimitive(PrimitiveNetworkOptions::NodeReorder | PrimitiveNetworkOptions::IgnoreShuntAdmittance);
			return this->Solve(*pn);
		}

		shared_ptr<Solution> DCFlowSolver::Solve(const ObjectModel::PrimitiveNetwork& network)
		{
			auto s = make_shared<Solution>();
			s->Status(SolutionStatus::Success);
			s->IterationCount(0);
			s->MaxDeviation(0);
			s->NodeCount(network.Nodes().size());
			s->PQNodeCount(network.PQNodes().size());
			s->PVNodeCount(network.PVNodes().size());
			s->SlackNode(network.SlackNode()->Bus());
			// 如果在结果生成完毕之前，直接使用 return s;
			// 就会返回一个表示求解失败的结果。
			s->Status(SolutionStatus::IterationFailed);
			// 计算直流潮流
			// P = Bθ
			auto& YMatrix = network.Admittance;
			// TODO: 考虑此处为何取负。
			auto BMatrix = -YMatrix.imag();
			//生成注入功率向量
			auto Nodes = network.Nodes().size();
			assert(Nodes > 1);
			auto IndependentNodes = Nodes - 1;
			SparseLU<SparseMatrix<double>> solver;
			//求解直流潮流。
			VectorXd PVector;
			PVector.resize(IndependentNodes);
			for (auto& node : network.Nodes())
			{
				if (node->Type() != NodeType::SlackNode) PVector[node->Index()] = node->ActivePowerInjection();
			}
			solver.compute(BMatrix.topLeftCorner(IndependentNodes, IndependentNodes));
			if (solver.info() != Eigen::Success) return s;
			//tcout << _T("  B矩阵秩：") << Solver.rank() << endl;
			// 除平衡节点外其它节点的 theta 。
			VectorXd theta = solver.solve(PVector);
			auto ps = make_shared<PrimitiveSolution>(network);
			//cout << ThetaVector << endl;
			unordered_map<Component*, ComponentFlowSolution> componentPowerInjections{};
			//初始化节点功率
			for (auto& ns : ps->NodeStatus())
			{
				//直流潮流，电压幅值为1。
				if (ns.Type() == NodeType::SlackNode)
					ns.SetVoltage(1, 0);
				else
					ns.SetVoltage(1, theta[ns.Index()]);
			}
			//计算并写入支路功率。
			for (auto& obj : network.SourceNetwork()->Objects())
			{
				auto c = dynamic_cast<Component*>(obj);
				if (c != nullptr)
				{
					//计算元件的潮流。
					//注意，此处计算元件潮流时，还是按照交流情况进行计算的。只不过仅保留计算结果的实数部分。
					//可能会造成结果偏差。
					auto cflow = c->EvalComponentFlow(*ps);
					auto injections = cflow.PowerInjections();
					for (size_t i = 0; i < cflow.PowerInjections().size(); i++)
					{
						complexd inj(cflow.PowerInjections(i).real(), 0);
						//修正潮流结果。
						cflow.PowerInjections(i, inj);
						//记录潮流结果。
						//并累加节点注入功率。
						ps->NodeStatus(network.Nodes(c->Buses(i)).Index()).AddPowerInjections(inj.real(), inj.imag());
					}
					cflow.PowerShunt(0);
					//注意，此处不能直接 AddComponentFlow
					//因为 AddComponentFlow 会尝试修改 Solution.NodeFlow 的内容。
					//而此时 NodeFlow 为空。
					componentPowerInjections.emplace(c, move(cflow));
				}
			}
			//写入节点功率。
			for (auto& node : network.Nodes()) s->AddNodeFlow(node->Bus(), ps->NodeStatus(node->Index()));
			//写入元件注入功率。
			for (auto& p : componentPowerInjections) s->AddComponentFlow(p.first, move(p.second));
			s->Status(SolutionStatus::Success);
			return s;
		}
	}
}


#include "extractChessboard.hpp"
#include "extractImageFeature.hpp"
#include "extractLidarFeature.hpp"
#include <pcl/common/geometry.h>
#include <sophus/so3.hpp>
#include <boost/program_options.hpp>
#include <opencv2/core/eigen.hpp>
#include "optimization.hpp"
#include "utils.hpp"
#include "config.hpp"
#include "evaluation.hpp"
using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{
    string config_file;
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("config,c", boost::program_options::value<std::string>(&config_file)->default_value("config.yaml"), "config file.");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }
    Config::loadFromFile(config_file); //
    std::string images_dir = Config::leftImageDatasetPath();
    std::string clouds_dir = Config::lidarCloudDatasetPath();
    std::vector<std::string> images;
    get_data_by_path(images, images_dir, Config::imageFormat());
    std::vector<std::string> clouds;
    get_data_by_path(clouds, clouds_dir, Config::cloudFormat());
    if(!check_data_by_index(images, clouds)){
        cerr <<"图像和点云文件应存在且文件名相同！！"<<endl;
        exit(-1);
    }

    ImageResults images_features;
    processImage(images, images_features, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
    cout<< "有效的图像帧有： "<<endl;
    for(auto& id:images_features.valid_index){
        cout<< id<<" ";
    }
    cout<<endl;

    CloudResults cloud_features;
    processCloud(clouds, cloud_features);
    cout<< "有效的点云帧有： "<<endl;
    for(auto& id:cloud_features.valid_index){
        cout<< id<<" ";
    }
    cout<<endl;
    getValidDataSet(images_features, cloud_features);
    auto& image_3d_corners = images_features.corners_3d; // 图像3d角点
    auto& image_planes_3d = images_features.planes_3d;
    auto& cloud_3d_corners = cloud_features.corners_3d; // 点云3d角点
    auto& plane_clouds_3d  = cloud_features.plane_points_3d;

    Eigen::VectorXd R_t(6);
    calculateInitialRt(cloud_3d_corners, image_3d_corners, R_t);
    cout<< "initial Rt \n"<< R_t<<endl;

    // debug
    // {
    //     for(auto& corners: image_3d_corners){
    //         for(auto& corner: corners)
    //         std::cout<< corner.transpose()<<std::endl;
    //     }
    //     exit(17);
    // }

    Optimizer optimizer_lc;
    ceres::Problem problem;
    std::vector<Vector3d> p1, p2;
    assert(image_3d_corners.size()==cloud_3d_corners.size());
    for(int i=0;i<image_3d_corners.size();++i){
        auto& image_corners = image_3d_corners[i];
        auto& cloud_corners = cloud_3d_corners[i];
        optimizer_lc.addPointToPointConstriants(problem,cloud_corners,image_corners,R_t);
    }

    assert(image_planes_3d.size()==plane_clouds_3d.size());
    cout<< "use " << image_planes_3d.size() << " planes"<<endl;
    for(int i=0;i<plane_clouds_3d.size();++i){
        auto& image_plane = image_planes_3d[i];
        auto& plane_cloud = plane_clouds_3d[i];
        // cout<< image_plane <<endl;
        // cout<< plane_cloud.size() <<endl;
        // display_colored_by_depth(plane_cloud.makeShared());
        optimizer_lc.addPointToPlaneConstriants(problem,plane_cloud, image_plane,R_t );
    }

    ceres::Solver::Options options;
    options.max_num_iterations = 500;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";
    Eigen::Matrix4d Rt = Matrix<double,4,4>::Identity();
    cout<< "Rt \n"<< R_t<<endl;
    Eigen::Matrix3d R = Sophus::SO3d::exp(R_t.head(3)).matrix();
    Rt.block(0,0,3,3) = R;
    Rt.block(0,3,3,1) = R_t.tail(3);
    std::cout << Rt << std::endl;
    std::cout<< Config::tformL1C1()<<std::endl;
    std::cout << Rt-Config::tformL1C1()<<std::endl;

    bool eval =  false;
    if(eval){
        // debug
        // {
        //     auto mat = Config::tformL1C1();
        //     Eigen::Matrix3d mat_R = mat.block(0,0,3,3);
        //     Eigen::AngleAxisd aa;
        //     aa.fromRotationMatrix(mat_R);
        //     R_t.head(3) = aa.angle() * aa.axis();
        //     R_t.tail(3) = mat.block(0,3,3,1);
        // }

        cv::Mat rvec = cv::Mat::zeros(cv::Size(1,3), CV_64FC1);
        rvec.at<double>(0,0) = R_t(0);
        rvec.at<double>(1,0) = R_t(1);
        rvec.at<double>(2,0) = R_t(2);
        cv::Mat tvec = cv::Mat::zeros(cv::Size(1,3), CV_64FC1);
        tvec.at<double>(0,0) = R_t(3);
        tvec.at<double>(1,0) = R_t(4);
        tvec.at<double>(2,0) = R_t(5);

        auto& valid_index = cloud_features.valid_index;

        for(int i = 0;i<valid_index.size();++i)
        {
            int k = valid_index[i];
            cv::Mat img = cv::imread(images[k], cv::IMREAD_COLOR);

            // projectLidarOnImage(img, plane_clouds_3d[i] , rvec, tvec, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
            // cv::imshow("eval", img);
            // cv::waitKey(0);

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            if (!loadPointXYZ(clouds[k], cloud)) continue;
            // debug
            // {
            //     std::vector<pcl::PointCloud<pcl::PointXYZ>> show_cloud;
            //     show_cloud.push_back(*cloud);
            //     show_cloud.push_back(plane_clouds_3d[i]);
            //     display_multi_clouds(show_cloud);
            // }

            auto color_pcd = colorizeCloud(img, *cloud , rvec, tvec, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
            boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
            // viewer->addPointCloud<pcl::PointXYZRGB>(*color_pcd , "sample cloud");
            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(color_pcd);
            viewer->addPointCloud<pcl::PointXYZRGB> (color_pcd, "sample cloud");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); // 设置点云大小
            viewer->addCoordinateSystem (1.0);
            viewer->initCameraParameters ();
            while (!viewer->wasStopped())
            {
                viewer->spinOnce(100);
                sleep(0.1);
            }
        }
    }

    bool compute_error = true;
    if(compute_error){
        auto translation_error = computeTranslationError(image_3d_corners, cloud_3d_corners,Rt );
        std::cout<< " translation_error:\n ";
        for(auto& error:translation_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
        auto rotation_error = computeRotationError(image_3d_corners, cloud_3d_corners,Rt );
        std::cout<< " rotation_error:\n ";
        for(auto& error:rotation_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
        auto reprojection_error = computeReprojectionError(image_3d_corners, cloud_3d_corners,Rt );
        std::cout<< " reprojection_error:\n ";
        for(auto& error:reprojection_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
    }

    return 0;
}


/*    Copyright (c) 2010-2016, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>

#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{

namespace reference_frames
{

//! Function to update the orientation angles to the current state.
void AerodynamicAngleCalculator::update( )
{
    // Clear all current rotation matrices.
    currentRotationMatrices_.clear( );

    // Get current body-fixed state.
    currentBodyFixedState_ = bodyFixedStateFunction_( );
    Eigen::Vector3d sphericalCoordinates = coordinate_conversions::convertCartesianToSpherical(
                currentBodyFixedState_.segment( 0, 3 ) );

    // Calculate latitude and longitude.
    currentAerodynamicAngles_[ latitude_angle ] =
            mathematical_constants::PI / 2.0 - sphericalCoordinates( 1 );
    currentAerodynamicAngles_[ longitude_angle ] = sphericalCoordinates( 2 );

    // Calculate vertical <-> aerodynamic <-> body-fixed angles if neede.
    if( calculateVerticalToAerodynamicFrame_ )
    {
        Eigen::Vector3d verticalFrameVelocity =
                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                    currentAerodynamicAngles_.at( longitude_angle ),
                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                currentBodyFixedState_.segment( 3, 3 );

        currentAerodynamicAngles_[ heading_angle ] = calculateHeadingAngle( verticalFrameVelocity );
        currentAerodynamicAngles_[ flight_path_angle ] =
                calculateFlightPathAngle( verticalFrameVelocity );

        currentAerodynamicAngles_[ angle_of_attack ] = angleOfAttackFunction_( );
        currentAerodynamicAngles_[ angle_of_sideslip ] = angleOfSideslipFunction_( );
        currentAerodynamicAngles_[ bank_angle ] = bankAngleFunction_( );
    }
}

//! Function to get the rotation quaternion between two frames
Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames(
        const AerodynamicsReferenceFrames originalFrame,
        const AerodynamicsReferenceFrames targetFrame )
{
    // Initialize rotation to identity matrix.
    Eigen::Quaterniond rotationToFrame = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );

    // Inertial frame is not specificied in current object.
    if( originalFrame == inertial_frame || targetFrame == inertial_frame)
    {
        throw( "Error in AerodynamicAngleCalculator, cannot calculate to/from inertial frame" );
    }

    // Check if update settings are consistent with requested frames.
    if( !calculateVerticalToAerodynamicFrame_ &&
            ( originalFrame > vertical_frame || targetFrame > vertical_frame ) )
    {
        throw( "Error in AerodynamicAngleCalculator, instance ends at vertical frame" );
    }

    // Set current frame pair.
    std::pair< AerodynamicsReferenceFrames, AerodynamicsReferenceFrames > currentRotationPair =
            std::make_pair( originalFrame, targetFrame );

    // Calculate rotation matrix if current rotation is not yet calculated.
    if( currentRotationMatrices_.count( currentRotationPair ) == 0 )
    {
        // Get indices of required frames.
        int currentFrameIndex = static_cast< int >( originalFrame );
        int targetFrameIndex = static_cast< int >( targetFrame );

        // Check if any rotation is needed.
        if( currentFrameIndex != targetFrameIndex )
        {
            // Check 'direction' of transformation through AerodynamicsReferenceFrames list.
            bool isTargetFrameUp;
            if( targetFrameIndex > currentFrameIndex )
            {
                isTargetFrameUp = 1;
            }
            else if( targetFrameIndex < currentFrameIndex )
            {
                isTargetFrameUp = 0;
            }
            else
            {
                throw std::runtime_error(
                            "Error when identifying target frame direction in AerodynamicAngleCalculator." );

            }

            // Add rotation sequence until final frame is reached.
            while( currentFrameIndex != targetFrameIndex )
            {
                switch( currentFrameIndex )
                {
                case static_cast< int >( corotating_frame ):

                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        throw std::runtime_error(
                                    "Error, corotating_frame is end frame in AerodynamicAngleCalculator" );
                    }
                    break;
                case static_cast< int >( vertical_frame ):
                    if( isTargetFrameUp )
                    {

                        rotationToFrame =
                                getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) * rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( longitude_angle ),
                                    currentAerodynamicAngles_.at( latitude_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( trajectory_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getTrajectoryToAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;

                    }
                    else
                    {
                        rotationToFrame =
                                getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( flight_path_angle ),
                                    currentAerodynamicAngles_.at( heading_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( aerodynamic_frame ):
                    if( isTargetFrameUp )
                    {
                        rotationToFrame =
                                getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    else
                    {
                        rotationToFrame =
                                getAerodynamicToTrajectoryFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( bank_angle ) ) *
                                rotationToFrame;
                    }
                    break;
                case static_cast< int >( body_frame ):
                    if( isTargetFrameUp )
                    {

                        throw std::runtime_error(
                                    "Error, body frame is end frame in AerodynamicAngleCalculator." );
                    }
                    else
                    {
                        rotationToFrame =
                                getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                                    currentAerodynamicAngles_.at( angle_of_attack ),
                                    currentAerodynamicAngles_.at( angle_of_sideslip ) ) *
                                rotationToFrame;
                    }
                    break;
                default:
                    throw std::runtime_error(
                                "Error, index " +
                                boost::lexical_cast< std::string>( currentFrameIndex ) +
                                "not found in AerodynamicAngleCalculator" );

                }

                // Increment/decrement current frame.
                if( isTargetFrameUp )
                {
                    currentFrameIndex++;
                }
                else
                {
                    currentFrameIndex--;
                }
            }
        }

        // Set current rotation (as well as inverse).
        currentRotationMatrices_[ currentRotationPair ] = rotationToFrame;
        currentRotationMatrices_[ std::make_pair( targetFrame, originalFrame ) ] =
                rotationToFrame.inverse( );
    }
    else
    {
        rotationToFrame = currentRotationMatrices_.at( currentRotationPair );
    }
    return rotationToFrame;
}

//! Function to get a single orientation angle.
double AerodynamicAngleCalculator::getAerodynamicAngle(
        const AerodynamicsReferenceFrameAngles angleId )
{
    double angleValue = TUDAT_NAN;
    if( currentAerodynamicAngles_.count( angleId ) == 0 )
    {
        throw std::runtime_error( "Error in AerodynamicAngleCalculator, angleId " +
                                  boost::lexical_cast< std::string >( angleId ) + "not found" );
    }
    else
    {
        angleValue = currentAerodynamicAngles_.at( angleId );
    }
    return angleValue;
}

//! Get a function to transform aerodynamic force from local to propagation frame.
boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) >
getAerodynamicForceTransformationFunction(
        const boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator,
        const AerodynamicsReferenceFrames accelerationFrame,
        const boost::function< Eigen::Quaterniond( ) > bodyFixedToInertialFrameFunction,
        const AerodynamicsReferenceFrames propagationFrame )
{
    boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > transformationFunction;

    // If propagation frame is the inertial frame, use bodyFixedToInertialFrameFunction.
    if( propagationFrame == inertial_frame )
    {
        std::vector< boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > > rotationsList;

        // Get accelerationFrame to corotating frame transformation.
        boost::function< Eigen::Quaterniond( ) > firstRotation =
                boost::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, corotating_frame );
        rotationsList.push_back(
                    boost::bind(
                        static_cast< Eigen::Vector3d(&)(
                            const Eigen::Vector3d&,
                            const boost::function< Eigen::Quaterniond( ) > ) >( &transformVector ),
                        _1, firstRotation ) );

        // Add corotating to inertial frame.
        rotationsList.push_back(
                    boost::bind(
                        static_cast< Eigen::Vector3d(&)(
                            const Eigen::Vector3d&,
                            const boost::function< Eigen::Quaterniond( ) > ) >( &transformVector ),
                        _1, bodyFixedToInertialFrameFunction ) );

        // Create transformation function.
        transformationFunction = boost::bind(
                    static_cast< Eigen::Vector3d(&)(
                        const Eigen::Vector3d&,
                        const std::vector< boost::function<
                        Eigen::Vector3d( const Eigen::Vector3d& ) > >& ) >( &transformVector ),
                    _1, rotationsList );
    }
    else
    {
        // Get accelerationFrame to propagationFrame frame transformation directly.
        boost::function< Eigen::Quaterniond( ) > rotationFunction =
                boost::bind( &AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames,
                             aerodynamicAngleCalculator, accelerationFrame, propagationFrame );

        // Create transformation function.
        transformationFunction = boost::bind(
                    static_cast< Eigen::Vector3d(&)(
                        const Eigen::Vector3d&,
                        const boost::function< Eigen::Quaterniond( ) > ) >( &transformVector ), _1,
                    rotationFunction );
    }

    return transformationFunction;
}

} // namespace reference_frames

} // namespace tudat


#ifndef __T2__
#define __T2__

#include <Eigen/Eigen>
#include "scene/Node.hpp"
#include "scene/Component.hpp"

namespace mb
{
	class Transform2: public Component
	{
	public:
		Transform2()
		: Component()
		{
			position = Eigen::Vector3f::Zero();
			scale = Eigen::Vector3f(1.0f, 1.0f, 1.0f);

			positionWorld = Eigen::Vector3f::Zero();
			scaleWorld = Eigen::Vector3f(1.0f, 1.0f, 1.0f);

			forward = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
			left = Eigen::Vector3f(1.0f, 0.0f, 0.0f);
			up = Eigen::Vector3f(0.0f, 1.0f, 0.0f);

			modelMatrix = Eigen::Matrix4f::Identity();
			modelWorldMatrix = Eigen::Matrix4f::Identity();
		}
		Eigen::Vector3f& getPosition()
		{
			return position;
		}
		Eigen::Vector3f& getScale()
		{
			return scale;
		}
		Eigen::Vector3f getPositionWorld()
		{
			return positionWorld;
		}
		Eigen::Vector3f getScaleWorld()
		{
			return scaleWorld;
		}
		void setPosition(float x, float y, float z)
		{
			setPosition(Eigen::Vector3f(x, y, z));
		}
		void setPosition(Eigen::Vector3f newPos)
		{
			position = newPos;
			_updateModelMatrix();
		}
		void translate(float x, float y, float z)
		{
			translate(Eigen::Vector3f(x, y, z));
		}
		void translate(Eigen::Vector3f diff)
		{
			setPosition(position + diff);
		}
		void translate(float dist, Eigen::Vector3f along)
		{
			setPosition(position + (dist * along.normalized()));
		}
		void lookAt(float x, float y, float z)
		{
			lookAt(Eigen::Vector3f(x, y, z));
		}
		void lookAt(Eigen::Vector3f point)
		{
			Eigen::Vector3f oldUp = getUp();
			{

			}
		}
		Eigen::Vector3f getForward() const
		{
			return forward;
		}
		Eigen::Vector3f getLeft() const
		{
			return left;
		}
		Eigen::Vector3f getUp() const
		{
			return up;
		}
	private:
		void _setPosition(const Eigen::Vector3f newPos)
		{
			position = newPos;
			_updateModelMatrix();
		}
		void _setPositionWorld(const Eigen::Vector3f newPos)
		{
			Eigen::Vector3f translation = newPos - positionWorld;

			Node* parent = this->_node->parent();
			if (parent != nullptr)
			{
				//translation = translation * Eigen::Matrix4f::inverse()
			}
			position += translation;
			_updateModelMatrix();
		}
		void _setScale(const Eigen::Vector3f newScale)
		{

		}

		void _updateModelMatrix()
		{
		}

		Eigen::Vector3f position;
		Eigen::Vector3f scale;

		Eigen::Vector3f positionWorld;
		Eigen::Vector3f scaleWorld;

		Eigen::Vector3f forward;
		Eigen::Vector3f left;
		Eigen::Vector3f up;

		Eigen::Matrix4f modelMatrix;
		Eigen::Matrix4f modelWorldMatrix;

		friend class Node;
	};
}

#endif