#include "rcvio/updater.hpp"

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <visualization_msgs/Marker.h>

#include "rcvio/numerics.h"

namespace rcvio
{
    static int cloud_id = 0;
    std_msgs::ColorRGBA color_landmark;
    geometry_msgs::Vector3 scale_landmark;

    Updater::Updater(const cv::FileStorage &fs_settings)
    {
        cam_rate_ = fs_settings["Camera.fps"];

        const float image_noise_sigma_x = fs_settings["Camera.sigma_px"];
        const float image_noise_sigma_y = fs_settings["Camera.sigma_py"];

        image_noise_sigma_ = std::max(image_noise_sigma_x, image_noise_sigma_y);

        cv::Mat T(4, 4, CV_32F);
        fs_settings["Camera.T_BC0"] >> T;
        Eigen::Matrix4d Tic;
        cv::cv2eigen(T, Tic);
        Ric_ = Tic.block<3, 3>(0, 0);
        tic_ = Tic.block<3, 1>(0, 3);
        Rci_ = Ric_.transpose();
        tci_ = -Rci_ * tic_;

        xk1k1.setZero(26, 1);
        Pk1k1.setZero(24, 24);

        feature_publish_ = updater_node_.advertise<visualization_msgs::Marker>("/rcvio/landmarks", 1);
        publish_rate_ = fs_settings["Landmark.nPubRate"];

        scale_landmark.x = fs_settings["Landmark.nScale"];
        scale_landmark.y = fs_settings["Landmark.nScale"];
        scale_landmark.z = fs_settings["Landmark.nScale"];

        color_landmark.a = 1;
        color_landmark.r = 0;
        color_landmark.b = 1;
        color_landmark.g = 0;
    }

    void Updater::update(Eigen::VectorXd &xk1k,
                         Eigen::MatrixXd &Pk1k,
                         std::vector<unsigned char> &feature_types_for_update,
                         std::vector<std::list<cv::Point2f>> &feature_measurements_for_update)
    {
        visualization_msgs::Marker cloud;
        cloud.header.frame_id = "imu";
        cloud.ns = "points";
        cloud.id = ++cloud_id;
        cloud.color = color_landmark;
        cloud.scale = scale_landmark;
        cloud.pose.orientation.w = 1.0;
        cloud.lifetime = ros::Duration(1 / publish_rate_);
        cloud.action = visualization_msgs::Marker::ADD;
        cloud.type = visualization_msgs::Marker::POINTS;

        int num_feature = static_cast<int>(feature_types_for_update.size());

        int rows = 0;
        for (int i = 0; i < num_feature; ++i)
        {
            rows += 2 * static_cast<int>(feature_measurements_for_update.at(i).size());
        }

        int clone_states = (xk1k.rows() - 26) / 7;

        Eigen::VectorXd r(rows, 1);
        Eigen::MatrixXd Hx(rows, 24 + 6 * clone_states);
        r.setZero();
        Hx.setZero();

        int row_count = 0;
        int good_feature_count = 0;

        for (int feature_idx = 0; feature_idx < num_feature; ++feature_idx)
        {
            char feature_type = feature_types_for_update.at(feature_idx);
            std::list<cv::Point2f> feature_measurements = feature_measurements_for_update.at(feature_idx);

            int track_length = static_cast<int>(feature_measurements.size());
            int track_phases = track_length - 1;
            int relative_poses_dimension = 7 * track_phases;

            Eigen::VectorXd relative_poses;
            if (feature_type == '1')
            {
                relative_poses = xk1k.tail(relative_poses_dimension);
            }
            else
            {
                relative_poses = xk1k.block(26, 0, relative_poses_dimension, 1);
            }

            Eigen::VectorXd relative_poses_to_first(relative_poses_dimension, 1);
            relative_poses_to_first.block(0, 0, 7, 1) << relative_poses.block(0, 0, 4, 1),
                -quatToRot(relative_poses.block(0, 0, 4, 1)) * relative_poses.block(4, 0, 3, 1);

            for (int i = 1; i < track_phases; ++i)
            {
                Eigen::Vector4d qI = quatMul(relative_poses.block(7 * i, 0, 4, 1),
                                             relative_poses_to_first.block(7 * (i - 1), 0, 4, 1));
                Eigen::Vector3d tI = quatToRot(relative_poses.block(7 * i, 0, 4, 1)) *
                                     (relative_poses_to_first.block(7 * (i - 1) + 4, 0, 3, 1) -
                                      relative_poses.block(7 * i + 4, 0, 3, 1));
                relative_poses_to_first.block(7 * i, 0, 7, 1) << qI, tI;
            }

            Eigen::VectorXd cam_relative_poses_to_first(relative_poses_dimension, 1);
            for (int i = 0; i < track_phases; ++i)
            {
                Eigen::Vector4d qC = rotToQuat(Rci_ * quatToRot(relative_poses_to_first.block(7 * i, 0, 4, 1)) * Ric_);
                Eigen::Vector3d tC = Rci_ * quatToRot(relative_poses_to_first.block(7 * i, 0, 4, 1)) * tic_ + Rci_ * relative_poses_to_first.block(7 * i + 4, 0, 3, 1) + tci_;
                cam_relative_poses_to_first.block(7 * i, 0, 7, 1) << qC, tC;
            }

            cv::Point2f pt_first = feature_measurements.front();
            feature_measurements.pop_front();

            double phi = std::atan2(pt_first.y, std::sqrt(std::pow(pt_first.x, 2) + 1));
            double psi = std::atan2(pt_first.x, 1);
            double rho = 0.0;

            if (std::fabs(phi) > 0.5 * 3.14 || std::fabs(psi) > 0.5 * 3.14)
            {
                ROS_DEBUG("Invalid inverse-depth feature estimate (0)!");
                continue;
            }

            Eigen::Vector3d epfinv;
            epfinv << std::cos(phi) * std::sin(psi), std::sin(phi), std::cos(phi) * std::cos(psi);

            Eigen::Matrix<double, 3, 2> Jang;
            Jang << -std::sin(phi) * std::sin(psi), std::cos(phi) * std::cos(psi),
                std::cos(phi), 0,
                -std::sin(phi) * std::cos(psi), -std::cos(phi) * std::sin(psi);

            Eigen::Matrix2d Rinv;
            Rinv << 1.0 / std::pow(image_noise_sigma_, 2), 0,
                0, 1.0 / std::pow(image_noise_sigma_, 2);

            int max_iter = 10;
            double lambda = 0.01;
            double last_cost = std::numeric_limits<double>::infinity();

            for (int iter = 0; iter < max_iter; ++iter)
            {
                Eigen::Matrix3d HTRinvH = Eigen::Matrix3d::Zero();
                Eigen::Vector3d HTRinve = Eigen::Vector3d::Zero();
                double cost = 0;

                Eigen::Vector3d h1 = epfinv;

                Eigen::Matrix<double, 2, 3> Hproj1;
                Hproj1 << 1 / h1(2), 0, -h1(0) / std::pow(h1(2), 2),
                    0, 1 / h1(2), -h1(1) / std::pow(h1(2), 2);

                Eigen::Matrix<double, 2, 3> H1;
                H1 << Hproj1 * Jang, Eigen::Vector2d::Zero();

                cv::Point2f pt1;
                pt1.x = h1(0) / h1(2);
                pt1.y = h1(1) / h1(2);

                Eigen::Vector2d e1;
                e1 << (pt_first - pt1).x, (pt_first - pt1).y;

                cost += e1.transpose() * Rinv * e1;

                HTRinvH.noalias() += H1.transpose() * Rinv * H1;
                HTRinve.noalias() += H1.transpose() * Rinv * e1;

                std::list<cv::Point2f>::const_iterator it = feature_measurements.begin();
                for (int i = 0; i < track_phases; ++i, ++it)
                {
                    Eigen::Matrix3d Rc = quatToRot(cam_relative_poses_to_first.block(7 * i, 0, 4, 1));
                    Eigen::Vector3d tc = cam_relative_poses_to_first.block(7 * i + 4, 0, 3, 1);
                    Eigen::Vector3d h = Rc * epfinv + rho * tc;

                    Eigen::Matrix<double, 2, 3> Hproj;
                    Hproj << 1 / h(2), 0, -h(0) / std::pow(h(2), 2),
                        0, 1 / h(2), -h(1) / std::pow(h(2), 2);

                    Eigen::Matrix<double, 2, 3> H;
                    H << Hproj * Rc * Jang, Hproj * tc;

                    cv::Point2f pt;
                    pt.x = h(0) / h(2);
                    pt.y = h(1) / h(2);

                    Eigen::Vector2d e;
                    e << ((*it) - pt).x, ((*it) - pt).y;

                    cost += e.transpose() * Rinv * e;

                    HTRinvH.noalias() += H.transpose() * Rinv * H;
                    HTRinve.noalias() += H.transpose() * Rinv * e;
                }

                if (cost <= last_cost)
                {
                    HTRinvH.diagonal() += lambda * HTRinvH.diagonal();
                    Eigen::Vector3d dpfinv = HTRinvH.colPivHouseholderQr().solve(HTRinve);

                    phi += dpfinv(0);
                    psi += dpfinv(1);
                    rho += dpfinv(2);

                    epfinv << std::cos(phi) * std::sin(psi), std::sin(phi), std::cos(phi) * std::cos(psi);

                    Jang << -std::sin(phi) * std::sin(psi), std::cos(phi) * std::cos(psi),
                        std::cos(phi), 0,
                        -std::sin(phi) * std::cos(psi), -std::cos(phi) * std::sin(psi);

                    if (std::fabs(last_cost - cost) < 1e-6 && dpfinv(2) < 1e-6)
                        break;

                    lambda *= 0.1;
                    last_cost = cost;
                }
                else
                {
                    lambda *= 10;
                    last_cost = cost;
                }
            }

            if (std::fabs(phi) > 0.5 * 3.14 || std::fabs(psi) > 0.5 * 3.14 || std::isinf(rho) || rho < 0)
            {
                ROS_DEBUG("Invalid inverse-depth feature estimate (1)!");
                continue;
            }

            if (feature_type == '2')
            {
                track_length = std::ceil(0.5 * track_length);
                track_phases = track_length - 1;
            }

            Eigen::VectorXd tempr(2 * track_length, 1);
            Eigen::MatrixXd temp_Hx(2 * track_length, 6 * clone_states);
            Eigen::MatrixXd temp_Hf(2 * track_length, 3);
            tempr.setZero();
            temp_Hx.setZero();
            temp_Hf.setZero();

            int start_row = 0;
            int start_col;
            if (feature_type == '1')
            {
                start_col = 6 * (clone_states - track_phases);
            }
            else
            {
                start_col = 0;
            }

            Eigen::Vector3d h1 = epfinv;

            cv::Point2f pt1;
            pt1.x = h1(0) / h1(2);
            pt1.y = h1(1) / h1(2);

            Eigen::Matrix<double, 2, 3> Hproj1;
            Hproj1 << 1 / h1(2), 0, -h1(0) / std::pow(h1(2), 2),
                0, 1 / h1(2), -h1(1) / std::pow(h1(2), 2);

            cv::Point2f e1 = pt_first - pt1;
            tempr.block(0, 0, 2, 1) << e1.x, e1.y;

            Eigen::Matrix3d tempm = Eigen::Matrix3d::Zero();
            tempm.block<3, 2>(0, 0) = Jang;
            temp_Hf.block<2, 3>(0, 0) = Hproj1 * tempm;

            start_row += 2;

            std::list<cv::Point2f>::const_iterator it = feature_measurements.begin();
            for (int i = 1; i < track_length; ++i, ++it)
            {
                Eigen::Matrix3d R = quatToRot(relative_poses_to_first.block(7 * (i - 1), 0, 4, 1));

                Eigen::Matrix3d Rc = quatToRot(cam_relative_poses_to_first.block(7 * (i - 1), 0, 4, 1));
                Eigen::Vector3d tc = cam_relative_poses_to_first.block(7 * (i - 1) + 4, 0, 3, 1);
                Eigen::Vector3d h = Rc * epfinv + rho * tc;

                cv::Point2f pt;
                pt.x = h(0) / h(2);
                pt.y = h(1) / h(2);

                Eigen::Matrix<double, 2, 3> Hproj;
                Hproj << 1 / h(2), 0, -h(0) / std::pow(h(2), 2),
                    0, 1 / h(2), -h(1) / std::pow(h(2), 2);

                cv::Point2f e = (*it) - pt;
                tempr.block(2 * i, 0, 2, 1) << e.x, e.y;

                Eigen::Matrix3d R0T = quatToRot(relative_poses_to_first.block(0, 0, 4, 1)).transpose();
                Eigen::Vector3d t0 = relative_poses_to_first.block(4, 0, 3, 1);

                Eigen::Matrix3d dpx0 = skew(Ric_ * epfinv + rho * tic_ + rho * R0T * t0);
                Eigen::Matrix<double, 3, 6> subH;
                subH << dpx0 * R0T, -rho * Eigen::Matrix3d::Identity();

                temp_Hx.block(start_row, start_col, 2, 6) = Hproj * Rci_ * R * subH;

                for (int j = 1; j < i; ++j)
                {
                    Eigen::Matrix3d R1T = quatToRot(relative_poses_to_first.block(7 * j, 0, 4, 1)).transpose();
                    Eigen::Vector3d t1 = relative_poses_to_first.block(7 * j + 4, 0, 3, 1);
                    Eigen::Matrix3d R2T = quatToRot(relative_poses_to_first.block(7 * (j - 1), 0, 4, 1)).transpose();

                    Eigen::Matrix3d dpx = skew(Ric_ * epfinv + rho * tic_ + rho * R1T * t1);
                    subH << dpx * R1T, -rho * R2T;

                    temp_Hx.block(start_row, start_col + 6 * j, 2, 6) = Hproj * Rci_ * R * subH;
                }

                temp_Hf.block(start_row, 0, 2, 3) << Hproj * Rc * Jang, Hproj * tc;

                start_row += 2;
            }

            int M = start_row;
            int N = temp_Hf.cols();

            if (temp_Hf.col(N - 1).norm() < 1e-4)
            {
                ROS_DEBUG("Hf is rank deficient!");
                N--;
            }

            Eigen::JacobiRotation<double> temp_Hf_GR;

            for (int n = 0; n < N; ++n)
            {
                for (int m = M - 1; m > n; m--)
                {
                    temp_Hf_GR.makeGivens(temp_Hf(m - 1, n), temp_Hf(m, n));

                    (temp_Hf.block(m - 1, n, 2, N - n)).applyOnTheLeft(0, 1, temp_Hf_GR.adjoint());

                    (temp_Hx.block(m - 1, 0, 2, temp_Hx.cols())).applyOnTheLeft(0, 1, temp_Hf_GR.adjoint());

                    (tempr.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, temp_Hf_GR.adjoint());
                }
            }

            int DOF = M - N;
            Eigen::VectorXd _tempr = tempr.block(N, 0, DOF, 1);
            Eigen::MatrixXd _temp_Hx = temp_Hx.block(N, 0, DOF, temp_Hx.cols());

            Eigen::VectorXd temp_R;
            temp_R.setOnes(DOF, 1);
            temp_R *= std::pow(image_noise_sigma_, 2);

            Eigen::MatrixXd temp_S;
            temp_S = _temp_Hx * Pk1k.block(24, 24, 6 * clone_states, 6 * clone_states) * (_temp_Hx.transpose());
            temp_S.diagonal() += temp_R;
            temp_S = 0.5 * (temp_S + temp_S.transpose());

            double mahalanobis_dist = (_tempr.transpose() * (temp_S.colPivHouseholderQr().solve(_tempr))).norm();

            if (mahalanobis_dist < CHI_THRESHOLD[DOF - 1])
            {
                r.block(row_count, 0, DOF, 1) = _tempr;
                Hx.block(row_count, 24, DOF, 6 * clone_states) = _temp_Hx;

                row_count += DOF;
                good_feature_count++;

                if (rho > 0)
                {
                    Eigen::VectorXd posek = relative_poses_to_first.tail(7);
                    Eigen::MatrixXd Rk = quatToRot(posek.head(4));
                    Eigen::Vector3d tk = posek.tail(3);

                    Eigen::Vector3d pfc = 1 / rho * epfinv;
                    Eigen::Vector3d pf1 = Ric_ * pfc + tic_;
                    Eigen::Vector3d pfk = Rk * pf1 + tk;

                    geometry_msgs::Point feature;
                    feature.x = pfk(0);
                    feature.y = pfk(1);
                    feature.z = pfk(2);
                    cloud.points.push_back(feature);
                }
            }
            else
            {
                ROS_DEBUG("Failed in Mahalanobis distance test!");
                continue;
            }
        }

        feature_publish_.publish(cloud);

        if (good_feature_count > 2)
        {
            Eigen::VectorXd ro = r.block(0, 0, row_count, 1);
            Eigen::MatrixXd Ho = Hx.block(0, 0, row_count, Hx.cols());

            Eigen::VectorXd Ro;
            Ro.setOnes(row_count, 1);
            Ro *= std::pow(image_noise_sigma_, 2);

            Eigen::VectorXd rn;
            Eigen::MatrixXd Hn;
            Eigen::VectorXd Rn;

            if (Ho.rows() > Ho.cols() - 24)
            {
                int M = Ho.rows();
                int N = Ho.cols() - 24;

                Eigen::MatrixXd temp_Hw = Ho.block(0, 24, M, N);

                for (int i = N; i > 0; i--)
                {
                    if (temp_Hw.col(i - 1).norm() == 0)
                    {
                        ROS_DEBUG("Hw is rank deficient!");
                        N--;
                    }
                    else
                    {
                        break;
                    }
                }

                Eigen::JacobiRotation<double> temp_Hw_GR;

                for (int n = 0; n < N; ++n)
                {
                    for (int m = M - 1; m > n; m--)
                    {
                        temp_Hw_GR.makeGivens(temp_Hw(m - 1, n), temp_Hw(m, n));

                        (temp_Hw.block(m - 1, n, 2, N - n)).applyOnTheLeft(0, 1, temp_Hw_GR.adjoint());

                        (ro.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, temp_Hw_GR.adjoint());
                    }
                }

                Ho.block(0, 24, M, N) = temp_Hw.block(0, 0, M, N);

                int rank = 0;
                for (int i = 0; i < M; ++i)
                {
                    if (Ho.row(i).norm() < 1e-4)
                    {
                        break;
                    }
                    else
                    {
                        rank++;
                    }
                }

                rn = ro.block(0, 0, rank, 1);
                Hn = Ho.block(0, 0, rank, Ho.cols());
                Rn.setOnes(rank, 1);
                Rn *= std::pow(image_noise_sigma_, 2);
            }
            else
            {
                rn = ro;
                Hn = Ho;
                Rn = Ro;
            }

            Eigen::MatrixXd S = Hn * Pk1k * (Hn.transpose());
            S.diagonal() += Rn;
            S = 0.5 * (S + S.transpose());
            Eigen::MatrixXd K = Pk1k * (Hn.transpose()) * (S.inverse());
            Eigen::VectorXd dx = K * rn;

            xk1k1.resize(xk1k.rows(), 1);

            Eigen::Vector4d dqG;
            dqG(0) = 0.5 * dx(0);
            dqG(1) = 0.5 * dx(1);
            dqG(2) = 0.5 * dx(2);

            double dqGvn = (dqG.head(3)).norm();
            if (dqGvn < 1)
            {
                dqG(3) = std::sqrt(1 - std::pow(dqGvn, 2));
            }
            else
            {
                dqG.head(3) *= (1 / std::sqrt(1 + std::pow(dqGvn, 2)));
                dqG(3) = 1 / std::sqrt(1 + std::pow(dqGvn, 2));
            }

            xk1k1.block(0, 0, 4, 1) = quatMul(dqG, xk1k.block(0, 0, 4, 1));
            xk1k1.block(4, 0, 6, 1) = dx.block(3, 0, 6, 1) + xk1k.block(4, 0, 6, 1);

            Eigen::Vector3d g = xk1k1.block(7, 0, 3, 1);
            g.normalize();
            xk1k1.block(7, 0, 3, 1) = g;

            Eigen::Vector4d dqR;
            dqR(0) = 0.5 * dx(9);
            dqR(1) = 0.5 * dx(10);
            dqR(2) = 0.5 * dx(11);

            double dqRvn = (dqR.head(3)).norm();
            if (dqRvn < 1)
            {
                dqR(3) = std::sqrt(1 - std::pow(dqRvn, 2));
            }
            else
            {
                dqR.head(3) *= (1 / std::sqrt(1 + std::pow(dqRvn, 2)));
                dqR(3) = 1 / std::sqrt(1 + std::pow(dqRvn, 2));
            }

            xk1k1.block(10, 0, 4, 1) = quatMul(dqR, xk1k.block(10, 0, 4, 1));
            xk1k1.block(14, 0, 12, 1) = dx.block(12, 0, 12, 1) + xk1k.block(14, 0, 12, 1);

            for (int pose_idx = 0; pose_idx < clone_states; ++pose_idx)
            {
                Eigen::Vector4d dqc;
                dqc(0) = 0.5 * dx(24 + 6 * pose_idx);
                dqc(1) = 0.5 * dx(24 + 6 * pose_idx + 1);
                dqc(2) = 0.5 * dx(24 + 6 * pose_idx + 2);

                double dqcvn = (dqc.head(3)).norm();
                if (dqcvn < 1)
                {
                    dqc(3) = std::sqrt(1 - std::pow(dqcvn, 2));
                }
                else
                {
                    dqc.head(3) *= (1 / std::sqrt(1 + std::pow(dqcvn, 2)));
                    dqc(3) = 1 / std::sqrt(1 + std::pow(dqcvn, 2));
                }

                xk1k1.block(26 + 7 * pose_idx, 0, 4, 1) = quatMul(dqc, xk1k.block(26 + 7 * pose_idx, 0, 4, 1));
                xk1k1.block(26 + 7 * pose_idx + 4, 0, 3, 1) = dx.block(24 + 6 * pose_idx + 3, 0, 3, 1) + xk1k.block(26 + 7 * pose_idx + 4, 0, 3, 1);
            }

            Eigen::MatrixXd _I = Eigen::MatrixXd::Identity(Pk1k.rows(), Pk1k.cols());
            Eigen::MatrixXd I_KH = _I - K * Hn;

            Pk1k1 = I_KH * Pk1k * (I_KH.transpose());
            Pk1k1 += Rn(0) * K * (K.transpose());
            Pk1k1 = 0.5 * (Pk1k1 + Pk1k1.transpose());
        }
        else
        {
            ROS_DEBUG("Too few measurements for update!");

            xk1k1 = xk1k;
            Pk1k1 = Pk1k;
        }
    }
}


/**
 * @file pfasst/encap/encapsulation.hpp
 * @since v0.1.0
 */
#ifndef _PFASST_ENCAPSULATED_HPP_
#define _PFASST_ENCAPSULATED_HPP_

#include <memory>
#include <vector>
using namespace std;

#include <Eigen/Dense>

template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

#include "pfasst/interfaces.hpp"

namespace pfasst
{
  /**
   * Encapsulations (short _encaps_) are the central data type for all PFASST++ algorithms.
   * Encaps can represent the unknown variable or the result of evaluating the right hand side of
   * the problem equation(s).
   */
  namespace encap
  {
    typedef enum EncapType { solution, function } EncapType;

    /**
     * Data/solution encapsulation.
     *
     * An Encapsulation provides basic mathematical functionality for the user's data.
     *
     * @tparam time time precision; defaults to pfasst::time_precision
     */
    template<typename time = time_precision>
    class Encapsulation
    {
      public:
        //! @{
        virtual ~Encapsulation();
        //! @}

        //! @{
        /**
         * Zeroes out all values of this data structure.
         */
        virtual void zero();

        /**
         * Copies values from @p other into this data structure.
         *
         * @param[in] other other data structure to copy data from
         */
        virtual void copy(shared_ptr<const Encapsulation<time>> other);
        //! @}

        //! @{
        /**
         * Computes the \\( 0 \\)-norm of the data structure's values.
         *
         * @returns \\( 0 \\)-norm of this data structure
         */
        virtual time norm0() const;
        //! @}

        //! @{
        /**
         * Provides basic mathematical operation \\( y = ax + y \\).
         *
         * This is the main mathematical operation applied by PFASST on the data structures.
         * Here, \\( a \\) is a constant and \\( x \\) another data structure (usually of the same
         * type) and \\( y \\) is this data structure.
         *
         * @param[in] a time point to multiply
         * @param[in] x another data structure to scale-add onto this one
         */
        virtual void saxpy(time a, shared_ptr<const Encapsulation<time>> x);

        /**
         * Defines matrix-vector multiplication for this data type.
         *
         * This implements the matrix-vector multiplication of the form
         * \\( \\vec{y}=a M \\vec{x} \\).
         * Here, \\( a \\) is a time point to scale the matrix-vector multiplication with, \\( M \\)
         * is a matrix and \\( x \\) another data structure usually of the same type as this one.
         *
         * If @p zero is `true`, then @p dst is zeroed out before applying the matrix-vector
         * multiplication.
         * Elsewise the result of the matrix-vector multiplication is added onto @p dst.
         *
         * @param[in,out] dst  data structure to store the result in
         * @param[in]     a
         * @param[in]     mat
         * @param[in]     src
         * @param[in]     zero
         */
        virtual void mat_apply(vector<shared_ptr<Encapsulation<time>>> dst,
                               time a, Matrix<time> mat,
                               vector<shared_ptr<Encapsulation<time>>> src,
                               bool zero = true);
        //! @}

        //! @{
        /**
         * Prepare to receive a solution (MPI_IRecv).
         *
         * @param[in] comm     communicator managing the processes to post ??? to/from ???
         * @param[in] tag      tag to distinguish overlapping communication
         */
        virtual void post(ICommunicator* comm, int tag);

        /**
         * Send solution (MPI_Send).
         *
         * @param[in] comm     communicator managing the processes to send to
         * @param[in] tag      tag to distinguish overlapping communication
         * @param[in] blocking whether to use blocking or non-blocking send
         */
        virtual void send(ICommunicator* comm, int tag, bool blocking);

        /**
         * Receive solution (MPI_Recv).
         *
         * @param[in] comm     communicator managing the processes to receive from
         * @param[in] tag      tag to distinguish overlapping communication
         * @param[in] blocking whether to use blocking or non-blocking receive
         */
        virtual void recv(ICommunicator* comm, int tag, bool blocking);

        /**
         * Broadcast this data structure to all processes in @p comm.
         *
         * @param[in] comm communicator managing the processes to send this data structure to
         */
        virtual void broadcast(ICommunicator* comm);
        //! @}
    };

    /**
     * Abstract interface of factory for creating Encapsulation objects.
     *
     * This factory is intended to be instantiated once to create multiple Encapsulation objects
     * of the same type and with the same parameters later on through calls to
     * EncapFactory::create().
     *
     * @tparam time time precision; defaults to pfasst::time_precision
     */
    template<typename time = time_precision>
    class EncapFactory
    {
      public:
        /**
         * Actual method to create Encapsulation object of specific type.
         *
         * @param[in] type encapsulation type of the requested Encapsulation object
         */
        virtual shared_ptr<Encapsulation<time>> create(const EncapType type) = 0;
    };
  }  // ::pfasst::encap
} // ::pfasst

#include "pfasst/encap/encapsulation_impl.hpp"

#endif


#include <boost/test/unit_test.hpp>
#include "../src/quadratic.hpp"

using namespace planner;

BOOST_AUTO_TEST_SUITE(quadratic)

BOOST_AUTO_TEST_CASE(test_constructors) {
    {
        Quadratic<int> q1;
        BOOST_CHECK_EQUAL(q1.real, 0);
        BOOST_CHECK_EQUAL(q1.imaginary, 0);
    }
    {
        Quadratic<int> q2{1, 2};
        BOOST_CHECK_EQUAL(q2.real, 1);
        BOOST_CHECK_EQUAL(q2.imaginary, 2);
    }
}

BOOST_AUTO_TEST_CASE(test_arithmetic) {
    Quadratic<int> q1{ 1, 2 };
    Quadratic<int> q2{ 3, -5};
    q1 += q2;
    BOOST_CHECK_EQUAL(q1,  Quadratic<int>(4, -3));
    auto q3 = q1 + q2;
    BOOST_CHECK_EQUAL(q3, Quadratic<int>(7, -8));
    q3 -= q2;
    BOOST_CHECK_EQUAL(q3,  Quadratic<int>(4, -3));
    auto q4 = q3 - q2;
    BOOST_CHECK_EQUAL(q4,  Quadratic<int>(1, 2));
}

BOOST_AUTO_TEST_CASE(test_evaluation) {
    {
        Quadratic<int> q{ 1, 1 };
        BOOST_CHECK_CLOSE(evaluate(q), 1 + sqrt(2), 1e-9);
    }
    {
        Quadratic<int> q{ 0, 1 };
        BOOST_CHECK_CLOSE(evaluate(q), sqrt(2), 1e-9);
    }
    {
        Quadratic<int> q{ 1, 0 };
        BOOST_CHECK_CLOSE(evaluate(q), 1, 1e-9);
    }
    {
        Quadratic<int> q{ 5, -4 };
        BOOST_CHECK_CLOSE(evaluate(q), 5 - 4 * sqrt(2), 1e-9);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/axis.hpp>

#include <Eigen/Dense>

namespace orient {

// \brief Construct rotation matrix from scalar angle and a rotation axis
// \param angle Scalar angle
// \template-param axis Rotation axis
// \return Rotation matrix
template<Axis axis, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Scalar angle);

// \brief Construct rotation matrix and partial derivatives from scalar angle and a rotation axis
// \param angle Scalar angle
// \template-param axis Rotation axis
// \return A pair containing first the rotation matrix and then secondly the Jacobian matrix
template<Axis axis, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar,3,3>> rotationMatrixFromEulerWD(Scalar angle);

// \brief Calculate rotation matrix from Euler angles and a rotation order
//        The rotation order is expressed in instrisic rotations.
//        For example, the rotation matrix
//        R = Rz(yaw) * Ry(pitch) * Rx(roll)
//        is constructed with
//        auto R = rotationMatrixFromEuler<Axis::z, Axis::y, Axis::x>([yaw, pitch, roll]);
// \param anlges Source Euler angles
// \template-params A1,A2,A3 The intrinsic rotation order
// \return Rotation matrix
template<Axis A1, Axis A2, Axis A3, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Eigen::Matrix<Scalar,3,1> const& angles);

// \brief Calculate rotation matrix and partial derivatves from Euler angles and a rotation order
//        See above for detail
// \param anlges Source Euler angles
// \template-params A1,A2,A3 The intrinsic rotation order
// \return A pair containing first the rotation matrix and then secondly the Jacobian matrix
template<Axis A1, Axis A2, Axis A3, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar, 9, 3>> rotationMatrixFromEulerWD(Eigen::Matrix<Scalar,3,1> const& angles);

}

#include <orient/impl/from_euler.hpp>


/**	\file TestPerpIntercept.cpp
*	\brief
*/

/****************************************************************************/
/*	TestPerpIntercept.cpp													*/
/****************************************************************************/
/*                                                                          */
/*  Copyright 2008 - 2010 Paul Kohut                                        */
/*  Licensed under the Apache License, Version 2.0 (the "License"); you may */
/*  not use this file except in compliance with the License. You may obtain */
/*  a copy of the License at                                                */
/*                                                                          */
/*  http://www.apache.org/licenses/LICENSE-2.0                              */
/*                                                                          */
/*  Unless required by applicable law or agreed to in writing, software     */
/*  distributed under the License is distributed on an "AS IS" BASIS,       */
/*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or         */
/*  implied. See the License for the specific language governing            */
/*  permissions and limitations under the License.                          */
/*                                                                          */
/****************************************************************************/

//#include "stdafx.h"
#include <string>
#include <fstream>
#include "LatLongConversions.h"
#include "..\GeoFormulas\Conversions.h"
#include "..\GeoFormulas\GeoFormulas.h"
#include <boost/regex.hpp>

using namespace boost;
using namespace GeoCalcs;
using namespace std;

bool ParseTestPerpIntercept(string sString)
{
	bool bPassed = true;
	TrimWhitespace(sString);
	string soTestId, soStartLat, soStartLong, soAz, soTestPtLat, soTestPtLong, soAzFromPt, soDistFromPt;
	string soInterceptLat, soInterceptLong;
	try
	{
		regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;

		string sRxPat = "([a-z]+|[A-Z]+\\d+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[].?[0-9]+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[].?[0-9]+)[,]([-+]?[0-9]*[.]?[0-9]+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])";
		regex pat(sRxPat, flags);

		int const sub_matches[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, };
		sregex_token_iterator it(sString.begin(), sString.end(), pat, sub_matches);
		if(it != sregex_token_iterator())
		{
			soTestId = *it++;
			soStartLat = *it++;
			soStartLong = *it++;
			soAz = *it++;
			soTestPtLat = *it++;
			soTestPtLong = *it++;
			soAzFromPt = *it++;
			soDistFromPt = *it++;
			soInterceptLat = *it++;
			soInterceptLong = *it++;
		}
	}
	catch(regex_error & e)
	{
		cout << "\n" << e.what();
		return false;
	}

	double dCalcedCrsFromPt, dCalcedDistFromPt;
	LLPoint pt3;

	pt3 = PerpIntercept(LLPoint(Deg2Rad(ParseLatitude(soStartLat)), Deg2Rad(ParseLongitude(soStartLong))),
		Deg2Rad(atof(soAz.c_str())),
		LLPoint(Deg2Rad(ParseLatitude(soTestPtLat)), Deg2Rad(ParseLongitude(soTestPtLong))),
		dCalcedCrsFromPt, dCalcedDistFromPt, Tol());

	string sLat = ConvertLatitudeDdToDms(Rad2Deg(pt3.latitude));
	string sLon = ConvertLongitudeDdToDms(Rad2Deg(pt3.longitude));

	if(sLat.compare(soInterceptLat) != 0)
	{
		cout << "\n" << soTestId << " failed: Input intercept latitude: " << soInterceptLat << "  calced: " << sLat;
		bPassed = false;
	}

	if(sLon.compare(soInterceptLong) != 0)
	{
		cout << "\n" << soTestId << " failed: Input intercept longitude: " << soInterceptLong << "  calced: " << sLon;
		bPassed = false;
	}
	return bPassed;
}

int TestPerpIntercept(const string & sFilePath)
{
	ifstream infile;
	infile.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit);
	int nCount = 0;
	int nCommentCount = 0;
	bool bPassed = true;
	try
	{
		string sLine;
		infile.open(sFilePath.c_str(), ifstream::in);

		while(!infile.eof())
		{
			getline(infile, sLine);
			if(sLine.at(0) == '#')
			{
				nCommentCount++;
			}
			else
			{
				if(!ParseTestPerpIntercept(sLine))
					bPassed = false;
				nCount++;
			}
		}
		infile.close();
		return bPassed;
	}

	catch(ifstream::failure e)
	{
		int nError = -99;
		// Per C++ standards for ifstream::failbit with global function getline
		// No characters were extracted because the end was prematurely found.Notice
		// that some eofbit cases will also set failbit.
		// In this case the end of the file is read and causes both flags to be raised,
		// so this presumably means all the data has been read correctly.
		if((infile.rdstate() & ifstream::failbit) && (infile.rdstate() & ifstream::eofbit) != 0)
			nError = bPassed;
		else if((infile.rdstate() & ifstream::failbit) != 0)
			nError = -1;
		else if((infile.rdstate() & ifstream::badbit) != 0)
			nError = -2;
		else if((infile.rdstate() & ifstream::eofbit) != 0)
			nError = -3;
		if(infile.is_open())
			infile.close();
		return nError;
	}
}


#include <iostream>
#include <vector>
#include <string>
#include <memory>
#include <Eigen/Dense>
#include "../include/layer.h"

using namespace Eigen;

int main()
{
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using std::shared_ptr;
    using std::make_shared;
    using namespace MyDL;

    int dim = 2;
    int batch_size = 3;

    // auto pgamma = make_shared<MatrixXd>(1, dim);
    // auto pbeta  = make_shared<MatrixXd>(1, dim);

    // *pgamma = MatrixXd::Random(1, dim);
    // *pbeta  = MatrixXd::Random(1, dim);

    // MyDL::BatchNorm batch_norm(pgamma, pbeta);

    BatchNorm batch_norm(dim);

    vector<MatrixXd> inputs, outputs, douts;
    MatrixXd X = MatrixXd::Random(batch_size, dim);
    inputs.push_back(X);

    MatrixXd dout = MatrixXd::Random(batch_size, dim);
    douts.push_back(dout);

    bool train_flg = Config::getInstance().get_flag();

    cout << "Train Flag: " << train_flg << endl;
    cout << "--- Mode Train ---" << endl;

    outputs = batch_norm.forward(inputs);

    cout << "Forward Result: " << outputs[0] << endl;

    outputs = batch_norm.forward(inputs); // _avg_mean, _avg_varをもう一段階変化させる

    Config::getInstance().set_flag(false);
    train_flg = Config::getInstance().get_flag();

    cout << "Train Flag: " << train_flg << endl;
    cout << "--- Mode Inference ---" << endl;

    outputs = batch_norm.forward(inputs);

    cout << "Forward Result: " << endl;
    cout << outputs[0] << endl;

    Config::getInstance().set_flag(true);
    train_flg = Config::getInstance().get_flag();

    cout << "Train Flag: " << train_flg << endl;
    cout << "--- Mode Train ---" << endl;

    vector<MatrixXd> grads;

    grads = batch_norm.backward(douts);
    cout << "Backward Result: " << endl;;
    cout << grads[0] << endl;

    return 0;
}


// Copyright (C) 2016 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <glog/logging.h>
#include "gtest/gtest.h"

#include "theia/sfm/gps_converter.h"
#include "theia/util/random.h"

namespace theia {

TEST(GPSConverter, ECEFToLLA) {
  static const double kTolerance = 1e-8;
  const Eigen::Vector3d taj_mahal_lla(27.173891, 78.042068, 168.0);
  const Eigen::Vector3d ecef(1176498.769459714,
                             5555043.905503586,
                             2895446.8901510699);
  const Eigen::Vector3d lla = GPSConverter::ECEFToLLA(ecef);
  EXPECT_NEAR(taj_mahal_lla[0], lla[0], kTolerance);
  EXPECT_NEAR(taj_mahal_lla[1], lla[1], kTolerance);
  EXPECT_NEAR(taj_mahal_lla[2], lla[2], kTolerance);
}

TEST(GPSConverter, LLAToECEF) {
  static const double kTolerance = 1e-8;
  const Eigen::Vector3d taj_mahal_lla(27.173891, 78.042068, 168.0);
  const Eigen::Vector3d gt_ecef(1176498.769459714,
                                5555043.905503586,
                                2895446.8901510699);
  const Eigen::Vector3d ecef = GPSConverter::LLAToECEF(taj_mahal_lla);
  EXPECT_NEAR(gt_ecef[0], ecef[0], kTolerance);
  EXPECT_NEAR(gt_ecef[1], ecef[1], kTolerance);
  EXPECT_NEAR(gt_ecef[2], ecef[2], kTolerance);
}

TEST(GPSConverter, RoundTrip) {
  RandomNumberGenerator rng(69);

  static const double kTolerance = 1e-8;
  static const int kNumTrials = 1000;
  for (int i = 0; i < kNumTrials; i++) {
    // Use the same random configuration as in the original paper: Olson,
    // D.K. "Converting earth-Centered, Earth-Fixed Coordinates to Geodetic
    // Coordinates," IEEE Transactions on Aerospace and Electronic Systems,
    // Vol. 32, No. 1, January 1996, pp. 473-476.
    const Eigen::Vector3d gt_lla(rng.RandDouble(-90.0, 90.0),
                                 rng.RandDouble(-180.0, 180.0),
                                 rng.RandDouble(-10000, 100000));

    const Eigen::Vector3d ecef = GPSConverter::LLAToECEF(gt_lla);
    const Eigen::Vector3d lla = GPSConverter::ECEFToLLA(ecef);

    EXPECT_NEAR(gt_lla[0], lla[0], kTolerance);
    EXPECT_NEAR(gt_lla[1], lla[1], kTolerance);
    EXPECT_NEAR(gt_lla[2], lla[2], kTolerance);
  }
}

}  // namespace theia


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_as_scalar_1")
  {
  mat A(1,1); A.fill(2.0);
  mat B(2,2); B.fill(2.0);

  REQUIRE( as_scalar(A) == Approx(2.0) );

  REQUIRE( as_scalar(2+A) == Approx(4.0) );

  REQUIRE( as_scalar(B(span(0,0), span(0,0))) == Approx(2.0) );

  REQUIRE_THROWS( as_scalar(B) );
  }

TEST_CASE("fn_as_scalar_2")
  {
  rowvec r = linspace<rowvec>(1,5,6);
  colvec q = linspace<colvec>(1,5,6);
  mat    X = 0.5*toeplitz(q);

  REQUIRE( as_scalar(r*q) == Approx(65.2) );

  REQUIRE( as_scalar(r*X*q) == Approx(380.848) );

  REQUIRE( as_scalar(r*diagmat(X)*q) == Approx(32.6) );
  REQUIRE( as_scalar(r*inv(diagmat(X))*q) == Approx(130.4) );
  }

TEST_CASE("fn_as_scalar_3")
  {
  cube A(1,1,1); A.fill(2.0);
  cube B(2,2,2); B.fill(2.0);

  REQUIRE( as_scalar(A) == Approx(2.0) );

  REQUIRE( as_scalar(2+A) == Approx(4.0) );

  REQUIRE( as_scalar(B(span(0,0), span(0,0), span(0,0))) == Approx(2.0) );

  REQUIRE_THROWS( as_scalar(B) );
  }


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

Distributed under the Boost Software License, Version 1.0.
See accompanying file LICENSE or copy at
http://www.boost.org/LICENSE_1_0.txt
*******/

#include "edit_distance_common.hpp"

#include <iostream>
#include <boost/algorithm/sequence/edit_distance.hpp>
using namespace boost::algorithm::sequence::parameter;
using boost::algorithm::sequence::edit_distance;
using boost::algorithm::sequence::unit_cost;

int main(int argc, char** argv) {
    char const* str1 = "abc";
    char const* str2 = "axc";

    // Compare two null-terminated strings that differ by one substitution
    // (distance should be 2)
    stringstream_tuple_output<unit_cost, char const*> out;
    unsigned dist = edit_distance(str1, str2, _script = out);
    std::cout << "dist= " << dist << "   edit operations= " << out.ss.str() << "\n";

    // Any type of sequences or range adaptors can be
    // applied as sequence arguments (here distance should be 4: "abc" -> "cxa" (-a, -b, =c, +x, +a)
    out.ss.str("");
    dist = edit_distance(as_vector(str1), as_list(str2) | boost::adaptors::reversed, _script = out);
    std::cout << "dist= " << dist << "   edit operations= " << out.ss.str() << "\n";

    return 0;
}


/*
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
 */

#ifndef __eigenpy_quaternion_hpp__
#define __eigenpy_quaternion_hpp__

#include "eigenpy/fwd.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "eigenpy/exception.hpp"
#include "eigenpy/registration.hpp"

namespace eigenpy
{

  class ExceptionIndex : public Exception
  {
  public:
    ExceptionIndex(int index,int imin,int imax) : Exception("")
    {
      std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
      message = oss.str();
    }
  };

  namespace bp = boost::python;

  template<typename QuaternionDerived> class QuaternionVisitor;

  template<typename Scalar, int Options>
  struct call< Eigen::Quaternion<Scalar,Options> >
  {
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    static inline void expose()
    {
      QuaternionVisitor<Quaternion>::expose();
    }

    static inline bool isApprox(const Quaternion & self, const Quaternion & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
  };

  BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call<Eigen::Quaterniond>::isApprox,2,3)

  template<typename Quaternion>
  class QuaternionVisitor
  :  public bp::def_visitor< QuaternionVisitor<Quaternion> >
  {
    typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;

    typedef typename QuaternionBase::Scalar Scalar;
    typedef typename Quaternion::Coefficients Coefficients;
    typedef typename QuaternionBase::Vector3 Vector3;
    typedef typename Eigen::Matrix<Scalar,4,1> Vector4;
    typedef typename QuaternionBase::Matrix3 Matrix3;

    typedef typename QuaternionBase::AngleAxisType AngleAxis;

  public:

    template<class PyClass>
    void visit(PyClass& cl) const
    {
      cl
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Vector4>((bp::arg("vec4")),
                             "Initialize from a vector 4D.\n"
                             "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
      .def(bp::init<Matrix3>((bp::arg("R")),
                             "Initialize from rotation matrix.\n"
                             "\tR : a rotation matrix 3x3."))
      .def(bp::init<AngleAxis>((bp::arg("aa")),
                               "Initialize from an angle axis.\n"
                               "\taa: angle axis object."))
      .def(bp::init<Quaternion>((bp::arg("quat")),
                                "Copy constructor.\n"
                                "\tquat: a quaternion."))
      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
                                           bp::default_call_policies(),
                                           (bp::arg("u: a 3D vector"),bp::arg("v: a 3D vector"))),
           "Initialize from two vectors u and v")
      .def(bp::init<Scalar,Scalar,Scalar,Scalar>
           ((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
            "Initialize from coefficients.\n\n"
            "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
            "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))

      .add_property("x",
                    &QuaternionVisitor::getCoeff<0>,
                    &QuaternionVisitor::setCoeff<0>,"The x coefficient.")
      .add_property("y",
                    &QuaternionVisitor::getCoeff<1>,
                    &QuaternionVisitor::setCoeff<1>,"The y coefficient.")
      .add_property("z",
                    &QuaternionVisitor::getCoeff<2>,
                    &QuaternionVisitor::setCoeff<2>,"The z coefficient.")
      .add_property("w",
                    &QuaternionVisitor::getCoeff<3>,
                    &QuaternionVisitor::setCoeff<3>,"The w coefficient.")

      .def("isApprox",
           &call<Quaternion>::isApprox,
           isApproxQuaternion_overload(bp::args("other","prec"),
                                       "Returns true if *this is approximately equal to other, within the precision determined by prec."))

      /* --- Methods --- */
      .def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
           bp::return_value_policy<bp::copy_const_reference>())
      .def("matrix",&Quaternion::matrix,"Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
      .def("toRotationMatrix",&Quaternion::toRotationMatrix,"Returns an equivalent 3x3 rotation matrix.")

      .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transforms a into b through a rotation."
           ,bp::return_self<>())
      .def("conjugate",&Quaternion::conjugate,"Returns the conjugated quaternion. The conjugate of a quaternion represents the opposite rotation.")
      .def("inverse",&Quaternion::inverse,"Returns the quaternion describing the inverse rotation.")
      .def("setIdentity",&Quaternion::setIdentity,bp::return_self<>(),"Set *this to the idendity rotation.")
      .def("norm",&Quaternion::norm,"Returns the norm of the quaternion's coefficients.")
      .def("normalize",&Quaternion::normalize,"Normalizes the quaternion *this.")
      .def("normalized",&Quaternion::normalized,"Returns a normalized copy of *this.")
      .def("squaredNorm",&Quaternion::squaredNorm,"Returns the squared norm of the quaternion's coefficients.")
      .def("dot",&Quaternion::template dot<Quaternion>,bp::arg("other"),"Returns the dot product of *this with other"
           "Geometrically speaking, the dot product of two unit quaternions corresponds to the cosine of half the angle between the two rotations.")
      .def("_transformVector",&Quaternion::_transformVector,bp::arg("vector"),"Rotation of a vector by a quaternion.")
      .def("vec",&vec,"Returns a vector expression of the imaginary part (x,y,z).")
      .def("angularDistance",&Quaternion::template angularDistance<Quaternion>,"Returns the angle (in radian) between two rotations.")
      .def("slerp",&slerp,bp::args("t","other"),
           "Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")

      /* --- Operators --- */
      .def(bp::self * bp::self)
      .def(bp::self *= bp::self)
      .def(bp::self * bp::other<Vector3>())
      .def("__eq__",&QuaternionVisitor::__eq__)
      .def("__ne__",&QuaternionVisitor::__ne__)
      .def("__abs__",&Quaternion::norm)
      .def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
      .def("__setitem__",&QuaternionVisitor::__setitem__)
      .def("__getitem__",&QuaternionVisitor::__getitem__)
      .def("assign",&assign<Quaternion>,
           bp::arg("quat"),"Set *this from an quaternion quat and returns a reference to *this.",bp::return_self<>())
      .def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
           bp::arg("aa"),"Set *this from an angle-axis aa and returns a reference to *this.",bp::return_self<>())
      .def("__str__",&print)
      .def("__repr__",&print)

//      .def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
//           bp::args("a","b"),
//           "Returns the quaternion which transform a into b through a rotation.")
      .def("FromTwoVectors",&FromTwoVectors,
           bp::args("a","b"),
           "Returns the quaternion which transforms a into b through a rotation.",
           bp::return_value_policy<bp::manage_new_object>())
      .staticmethod("FromTwoVectors")
      .def("Identity",&Quaternion::Identity,"Returns a quaternion representing an identity rotation.")
      .staticmethod("Identity")
      ;
    }
  private:

    template<int i>
    static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }

    template<int i>
    static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }

    static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
    { return self.setFromTwoVectors(a,b); }

    template<typename OtherQuat>
    static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
    { return self = quat; }

    static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
    {
      Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
      return q;
    }

    static bool __eq__(const Quaternion & u, const Quaternion & v)
    {
      return u.coeffs() == v.coeffs();
    }

    static bool __ne__(const Quaternion& u, const Quaternion& v)
    {
      return !__eq__(u,v);
    }

    static Scalar __getitem__(const Quaternion & self, int idx)
    {
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      return self.coeffs()[idx];
    }

    static void __setitem__(Quaternion& self, int idx, const Scalar value)
    {
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      self.coeffs()[idx] = value;
    }

    static int __len__() {  return 4;  }
    static Vector3 vec(const Quaternion & self) { return self.vec(); }

    static std::string print(const Quaternion & self)
    {
      std::stringstream ss;
      ss << "(x,y,z,w) = " << self.coeffs().transpose() << std::endl;

      return ss.str();
    }

    static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
    { return self.slerp(t,other); }

  public:

    static void expose()
    {
      bp::class_<Quaternion>("Quaternion",
                             "Quaternion representing rotation.\n\n"
                             "Supported operations "
                             "('q is a Quaternion, 'v' is a Vector3): "
                             "'q*q' (rotation composition), "
                             "'q*=q', "
                             "'q*v' (rotating 'v' by 'q'), "
                             "'q==q', 'q!=q', 'q[0..3]'.",
                             bp::no_init)
      .def(QuaternionVisitor<Quaternion>())
      ;

    }

  };

} // namespace eigenpy

#endif // ifndef __eigenpy_quaternion_hpp__


/*
 * Copyright 2015 Alex Millane, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <math.h>
#include <fstream>
#include <iostream>

#include <gtest/gtest.h>
#include <Eigen/Geometry>

#include "test_helper_library.h"
#include "vicon_estimator.h"

// Translational trajectory defines
#define TRANS_TRAJECTORY_PERIOD 10.0
#define TRANS_TRAJECTORY_AMPLITUDE 1.0
#define TRANS_TRAJECTORY_DT 0.01
#define TRANS_TRAJECTORY_FREQ 0.5
#define TRANS_TRAJECTORY_PHASE_OFFSET_X 0.0
#define TRANS_TRAJECTORY_PHASE_OFFSET_Y M_PI / 2.0
#define TRANS_TRAJECTORY_PHASE_OFFSET_Z M_PI / 1.0

// Rotational trajectory defines
#define ROT_TRAJECTORY_PERIOD 10.0
#define ROT_TRAJECTORY_AMPLITUDE 1.0 * M_PI
#define ROT_TRAJECTORY_DT 0.01
#define ROT_TRAJECTORY_FREQ 0.5
#define ROT_TRAJECTORY_PHASE_OFFSET_X 0.0
#define ROT_TRAJECTORY_PHASE_OFFSET_Y M_PI / 2.0
#define ROT_TRAJECTORY_PHASE_OFFSET_Z M_PI / 1.0

#define POS_ERROR_THRESHOLD 0.01
#define VEL_ERROR_THRESHOLD 0.1

#define QUAT_ERROR_THRESHOLD 0.005
#define OMEGA_ERROR_THRESHOLD 1.5

/*
 *  Helper Function Tests
 */

/*TEST(helperFunctions, euler2Quat)
{
  //
  double roll = 1.0/2.0*M_PI ;
  double pitch = 0.0 ;
  double yaw = 0.0 ;
  void euler2quat(double roll, double pitch, double yaw, double* q1, double* q2,
double* q3, double* q4);

}*/

/*
 *  Translational Estimator Tests
 */

void generateTranslationalTrajectorySinusoidal(const int trajectory_length,
                                               double position_trajectory[][3],
                                               double velocity_trajectory[][3],
                                               double timestamps[]) {
  // Generating position trajectories
  for (int i = 0; i < trajectory_length; i++) {
    position_trajectory[i][0] =
        TRANS_TRAJECTORY_AMPLITUDE *
        sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_X);
    position_trajectory[i][1] =
        TRANS_TRAJECTORY_AMPLITUDE *
        sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_Y);
    position_trajectory[i][2] =
        TRANS_TRAJECTORY_AMPLITUDE *
        sin(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_Z);
  }
  // Generating velocity trajectories from algebraic differentiation
  for (int i = 0; i < trajectory_length; i++) {
    velocity_trajectory[i][0] =
        2 * M_PI * TRANS_TRAJECTORY_FREQ *
        cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_X);
    velocity_trajectory[i][1] =
        2 * M_PI * TRANS_TRAJECTORY_FREQ *
        cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_Y);
    velocity_trajectory[i][2] =
        2 * M_PI * TRANS_TRAJECTORY_FREQ *
        cos(2 * M_PI * i * TRANS_TRAJECTORY_DT * TRANS_TRAJECTORY_FREQ +
            TRANS_TRAJECTORY_PHASE_OFFSET_Z);
  }
  // Generating the timestamps
  timestamps[0] = 0.0;
  for (int i = 1; i < trajectory_length; i++) {
    timestamps[i] = timestamps[i - 1] + TRANS_TRAJECTORY_DT;
  }
}

TEST(translationalEstimator, sinusoidal_clean) {
  // Creating the estimator
  vicon_estimator::TranslationalEstimator translational_estimator;

  // Setting the estimator gains
  vicon_estimator::TranslationalEstimatorParameters
      translational_estimator_parameters;
  translational_estimator_parameters.kp_ = 1.0;
  translational_estimator_parameters.kv_ = 10 * 10.0;
  translational_estimator.setParameters(translational_estimator_parameters);
  translational_estimator.reset();

  // Generating the trajectory over which to test the estimator
  const int trajectory_length =
      static_cast<int>(TRANS_TRAJECTORY_PERIOD / TRANS_TRAJECTORY_DT) + 1;
  double position_trajectory[trajectory_length][3];
  double velocity_trajectory[trajectory_length][3];
  double timestamps[trajectory_length];
  generateTranslationalTrajectorySinusoidal(
      trajectory_length, position_trajectory, velocity_trajectory, timestamps);

  // Looping over trajectory and retrieving estimates
  double position_trajectory_estimate[trajectory_length][3];
  double velocity_trajectory_estimate[trajectory_length][3];
  for (int i = 0; i < trajectory_length; i++) {
    // Constructing input
    Eigen::Vector3d input(position_trajectory[i][0], position_trajectory[i][1],
                          position_trajectory[i][2]);
    double timestamp = timestamps[i];
    // Updating the estimate with the measurement
    translational_estimator.updateEstimate(input, timestamp);
    // Getting the position and velocity estimates
    Eigen::Vector3d estimated_position =
        translational_estimator.getEstimatedPosition();
    Eigen::Vector3d estimated_velocity =
        translational_estimator.getEstimatedVelocity();
    // Moving values to arrays
    position_trajectory_estimate[i][0] = estimated_position.x();
    position_trajectory_estimate[i][1] = estimated_position.y();
    position_trajectory_estimate[i][2] = estimated_position.z();
    velocity_trajectory_estimate[i][0] = estimated_velocity.x();
    velocity_trajectory_estimate[i][1] = estimated_velocity.y();
    velocity_trajectory_estimate[i][2] = estimated_velocity.z();
  }

  // Start index for error calculation
  const int start_index = trajectory_length / 2;

  // Calculating position estimate errors
  double position_error[3];
  calculate3dRmsError(position_trajectory, position_trajectory_estimate,
                      trajectory_length, start_index, position_error);

  // Performing test
  EXPECT_NEAR(position_error[0], 0, POS_ERROR_THRESHOLD)
      << "X position estimate error too great";
  EXPECT_NEAR(position_error[1], 0, POS_ERROR_THRESHOLD)
      << "Y position estimate error too great";
  EXPECT_NEAR(position_error[2], 0, POS_ERROR_THRESHOLD)
      << "Z position estimate error too great";

  // Calculating velocity estimate errors
  double velocity_error[3];
  calculate3dRmsError(velocity_trajectory, velocity_trajectory_estimate,
                      trajectory_length, start_index, velocity_error);

  // Performing test
  EXPECT_NEAR(velocity_error[0], 0, VEL_ERROR_THRESHOLD)
      << "X velocity estimate error too great";
  EXPECT_NEAR(velocity_error[1], 0, VEL_ERROR_THRESHOLD)
      << "Y velocity estimate error too great";
  EXPECT_NEAR(velocity_error[2], 0, VEL_ERROR_THRESHOLD)
      << "Z velocity estimate error too great";

  /*
   *  For matlab debug
   */

  /*  // Opening a file for trajectories debug data
    std::ofstream trajectoriesFile;
    trajectoriesFile.open("translationalTrajectories.txt");
    // Writing the data to file
    for (int i = 0; i < trajectoryLength; i++)
    {
      trajectoriesFile << posTrajectory[i][0] << ", " << posTrajectory[i][1] <<
    ", " << posTrajectory[i][2] << ", ";
      trajectoriesFile << posEstTrajectory[i][0] << ", " <<
    posEstTrajectory[i][1] << ", " << posEstTrajectory[i][2] << ", ";
      trajectoriesFile << velTrajectory[i][0] << ", " << velTrajectory[i][1] <<
    ", " << velTrajectory[i][2] << ", ";
      trajectoriesFile << velEstTrajectory[i][0] << ", " <<
    velEstTrajectory[i][1] << ", " << velEstTrajectory[i][2];
      trajectoriesFile << std::endl;
    }
    // Closing file
    trajectoriesFile.close();

    // Opening a file for errors debug data
    std::ofstream errorsFile;
    errorsFile.open("translationalErrors.txt");
    // Writing the data to file
    errorsFile << posError[0] << ", " << posError[1] << ", " << posError[2] <<
    std::endl;
    errorsFile << velError[0] << ", " << velError[1] << ", " << velError[2] <<
    std::endl;
    // Closing file
    errorsFile.close();*/
}

/*
 *  Rotational Estimator Tests
 */

void generateRotationalTrajectorySinusoidal(const int trajectory_length,
                                            double orientation_trajectory[][4],
                                            double rollrate_trajectory[][3],
                                            double timestamps[]) {
  // Generating quaternion trajectories
  double roll, pitch, yaw;
  for (int i = 0; i < trajectory_length; i++) {
    // Generating euler angles
    roll = ROT_TRAJECTORY_AMPLITUDE *
           sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ +
               ROT_TRAJECTORY_PHASE_OFFSET_X);
    pitch = ROT_TRAJECTORY_AMPLITUDE *
            sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ +
                ROT_TRAJECTORY_PHASE_OFFSET_Y);
    yaw = ROT_TRAJECTORY_AMPLITUDE *
          sin(2 * M_PI * i * ROT_TRAJECTORY_DT * ROT_TRAJECTORY_FREQ +
              ROT_TRAJECTORY_PHASE_OFFSET_Z);
    // Converting to quaternions
    euler2quat(roll, pitch, yaw, orientation_trajectory[i]);
  }

  // Generating the omega trajectories through numeric differentiation
  Eigen::Quaterniond q_k, q_k_1;
  for (int i = 0; i < trajectory_length - 1; i++) {
    // Generating quaternions
    q_k = Eigen::Quaterniond(
        orientation_trajectory[i + 1][0], orientation_trajectory[i + 1][1],
        orientation_trajectory[i + 1][2], orientation_trajectory[i + 1][3]);
    q_k_1 = Eigen::Quaterniond(
        orientation_trajectory[i][0], orientation_trajectory[i][1],
        orientation_trajectory[i][2], orientation_trajectory[i][3]);
    // Calculating quaternion derivative
    Eigen::Quaterniond diff =
        Eigen::Quaterniond((q_k.coeffs() - q_k_1.coeffs()) / ROT_TRAJECTORY_DT);
    Eigen::Quaterniond omega =
        Eigen::Quaterniond(2 * (q_k_1.inverse() * diff).coeffs());
    // Writing to the trajectory
    rollrate_trajectory[i][0] = omega.x();
    rollrate_trajectory[i][1] = omega.y();
    rollrate_trajectory[i][2] = omega.z();
  }

  // Generating the timestamps
  timestamps[0] = 0.0;
  for (int i = 1; i < trajectory_length; i++) {
    timestamps[i] = timestamps[i - 1] + TRANS_TRAJECTORY_DT;
  }
}

TEST(rotationalEstimator, sinusoidal_clean) {
  // Creating the estimator
  vicon_estimator::RotationalEstimator rotational_estimator;

  // Setting the estimator gains
  vicon_estimator::RotationalEstimatorParameters
      rotational_estimator_parameters;
  rotational_estimator_parameters.dorientation_estimate_initial_covariance_ = 1;
  rotational_estimator_parameters.drate_estimate_initial_covariance_ = 1;
  rotational_estimator_parameters.dorientation_process_covariance_ = 0.01;
  rotational_estimator_parameters.drate_process_covariance_ = 1000 * 1;
  rotational_estimator_parameters.orientation_measurement_covariance_ = 0.0005;
  rotational_estimator.setParameters(rotational_estimator_parameters);
  rotational_estimator.reset();

  // Generating the trajectory over which to test the estimator
  const int trajectory_length =
      static_cast<int>(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1;
  double orientation_trajectory[trajectory_length][4];
  double omega_trajectory[trajectory_length][3];
  double timestamps[trajectory_length];
  generateRotationalTrajectorySinusoidal(
      trajectory_length, orientation_trajectory, omega_trajectory, timestamps);

  // Looping over trajectory and retrieving estimates
  double orientation_estimate_trajectory[trajectory_length][4];
  double rollrate_estimate_trajectory[trajectory_length][3];
  for (int i = 0; i < trajectory_length; i++) {
    // Constructing input
    Eigen::Quaterniond input(
        orientation_trajectory[i][0], orientation_trajectory[i][1],
        orientation_trajectory[i][2], orientation_trajectory[i][3]);
    // Updating the estimate with the measurement
    double timestamp = timestamps[i];
    rotational_estimator.updateEstimate(input, timestamp);
    // Getting the position and velocity estimates
    Eigen::Quaterniond estimated_orientation =
        rotational_estimator.getEstimatedOrientation();
    Eigen::Vector3d estimated_rollrate =
        rotational_estimator.getEstimatedRate();
    // Moving values to arrays
    orientation_estimate_trajectory[i][0] = estimated_orientation.w();
    orientation_estimate_trajectory[i][1] = estimated_orientation.x();
    orientation_estimate_trajectory[i][2] = estimated_orientation.y();
    orientation_estimate_trajectory[i][3] = estimated_orientation.z();
    rollrate_estimate_trajectory[i][0] = estimated_rollrate.x();
    rollrate_estimate_trajectory[i][1] = estimated_rollrate.y();
    rollrate_estimate_trajectory[i][2] = estimated_rollrate.z();
  }

  // Start index for error calculation
  const int start_index = trajectory_length / 2;

  // Calculating position estimate errors
  double orientation_error[3];
  calculateQuaternionRmsError(
      orientation_trajectory, orientation_estimate_trajectory,
      trajectory_length, start_index, orientation_error);

  // Performing test
  EXPECT_NEAR(orientation_error[0], 0, QUAT_ERROR_THRESHOLD)
      << "X errorquaternion estimate error too great";
  EXPECT_NEAR(orientation_error[1], 0, QUAT_ERROR_THRESHOLD)
      << "Y errorquaternion estimate error too great";
  EXPECT_NEAR(orientation_error[2], 0, QUAT_ERROR_THRESHOLD)
      << "Z errorquaternion estimate error too great";

  // Calculating velocity estimate errors
  double omegaError[3];
  calculate3dRmsError(omega_trajectory, rollrate_estimate_trajectory,
                      trajectory_length, start_index, omegaError);

  // Performing test
  EXPECT_NEAR(omegaError[0], 0, OMEGA_ERROR_THRESHOLD)
      << "X rotational velocity estimate error too great";
  EXPECT_NEAR(omegaError[1], 0, OMEGA_ERROR_THRESHOLD)
      << "Y rotational velocity estimate error too great";
  EXPECT_NEAR(omegaError[2], 0, OMEGA_ERROR_THRESHOLD)
      << "Z rotational velocity estimate error too great";

  /*
   *  For matlab debug
   */

  /*  // Opening a file for trajectories debug data
    std::ofstream trajectoriesFile;
    trajectoriesFile.open("rotationalTrajectories.txt");
    // Writing the data to file
    for (int i = 0; i < trajectoryLength; i++)
    {
      trajectoriesFile << quatTrajectory[i][0] << ", " << quatTrajectory[i][1]
    << ", " << quatTrajectory[i][2] << ", " << quatTrajectory[i][3] << ", ";
      trajectoriesFile << quatEstTrajectory[i][0] << ", " <<
    quatEstTrajectory[i][1] << ", " << quatEstTrajectory[i][2] << ", " <<
    quatEstTrajectory[i][3] << ", ";
      trajectoriesFile << omegaTrajectory[i][0] << ", " << omegaTrajectory[i][1]
    << ", " << omegaTrajectory[i][2] << ", ";
      trajectoriesFile << omegaEstTrajectory[i][0] << ", " <<
    omegaEstTrajectory[i][1] << ", " << omegaEstTrajectory[i][2];
      trajectoriesFile << std::endl;
    }
    // Closing file
    trajectoriesFile.close();

    // Opening a file for errors debug data
    std::ofstream errorsFile;
    errorsFile.open("rotationalErrors.txt");
    // Writing the data to file
    errorsFile << quatError[0] << ", " << quatError[1] << ", " << quatError[2]
    << std::endl;
    errorsFile << omegaError[0] << ", " << omegaError[1] << ", " <<
    omegaError[2] << std::endl;
    // Closing file
    errorsFile.close();*/
}

// Quaternions representing flips in various dimensions
const int kNumberOfInversions = 7;
const Eigen::Quaterniond rotation_inversion_x =
    Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
const Eigen::Quaterniond rotation_inversion_y =
    Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
const Eigen::Quaterniond rotation_inversion_z =
    Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
const Eigen::Quaterniond rotation_inversion_xy =
    rotation_inversion_x * rotation_inversion_y;
const Eigen::Quaterniond rotation_inversion_yz =
    rotation_inversion_y * rotation_inversion_z;
const Eigen::Quaterniond rotation_inversion_xz =
    rotation_inversion_x * rotation_inversion_z;
const Eigen::Quaterniond rotation_inversion_xyz =
    rotation_inversion_x * rotation_inversion_y * rotation_inversion_z;
const Eigen::Quaterniond rotation_inversions[kNumberOfInversions] = {
    rotation_inversion_x,  rotation_inversion_y,  rotation_inversion_z,
    rotation_inversion_xy, rotation_inversion_yz, rotation_inversion_xz,
    rotation_inversion_xyz};

void generateCorruptedInputTrajectory(
    const Eigen::Quaterniond clean_trajectory[], const int trajectory_length,
    const int corruption_rate, Eigen::Quaterniond corrupted_trajectory[]) {
  // Looping over the clean input trajecty and corruputing measurements at a
  // rate determined by corruption_rate
  // Note that corruption starts only after corruption_rate samples.
  int inversion_index = 0;
  for (int i = corruption_rate; i <= trajectory_length; i += corruption_rate) {
    // Corrupting the measurement with an inversion from the inversion list
    corrupted_trajectory[i] =
        rotation_inversions[inversion_index] * clean_trajectory[i];
    // corrupted_trajectory[i] = clean_trajectory[i];
    // Incrementing the inversion
    inversion_index = (inversion_index + 1) % kNumberOfInversions;
  }
}

TEST(rotationalEstimator, sinusoidal_corrupted) {
  // Creating the estimator
  vicon_estimator::RotationalEstimator rotational_estimator;

  // Setting the estimator gains
  vicon_estimator::RotationalEstimatorParameters
      rotational_estimator_parameters;
  rotational_estimator_parameters.dorientation_estimate_initial_covariance_ = 1;
  rotational_estimator_parameters.drate_estimate_initial_covariance_ = 1;
  rotational_estimator_parameters.dorientation_process_covariance_ = 0.01;
  rotational_estimator_parameters.drate_process_covariance_ = 1000 * 1;
  rotational_estimator_parameters.orientation_measurement_covariance_ = 0.0005;
  rotational_estimator.setParameters(rotational_estimator_parameters);
  rotational_estimator.reset();

  // Generating the trajectory over which to test the estimator
  const int trajectory_length =
      static_cast<int>(ROT_TRAJECTORY_PERIOD / ROT_TRAJECTORY_DT) + 1;
  double orientation_trajectory[trajectory_length][4];
  double omega_trajectory[trajectory_length][3];
  double timestamps[trajectory_length];
  generateRotationalTrajectorySinusoidal(
      trajectory_length, orientation_trajectory, omega_trajectory, timestamps);

  // Constructing the measurement vector
  Eigen::Quaterniond clean_input_trajectory[trajectory_length];
  Eigen::Quaterniond corrupted_input_trajectory[trajectory_length];
  // Creating the clean measurement vector
  for (int i = 0; i < trajectory_length; i++) {
    clean_input_trajectory[i] = Eigen::Quaterniond(
        orientation_trajectory[i][0], orientation_trajectory[i][1],
        orientation_trajectory[i][2], orientation_trajectory[i][3]);
    corrupted_input_trajectory[i] = Eigen::Quaterniond(
        orientation_trajectory[i][0], orientation_trajectory[i][1],
        orientation_trajectory[i][2], orientation_trajectory[i][3]);
  }
  // Corrupting the measurement vector
  int corruption_rate = 100;  // TODO(millanea): Parameter above.
  generateCorruptedInputTrajectory(clean_input_trajectory, trajectory_length,
                                   corruption_rate, corrupted_input_trajectory);

  // Looping over trajectory and retrieving estimates
  double orientation_estimate_trajectory[trajectory_length][4];
  double rollrate_estimate_trajectory[trajectory_length][3];
  for (int i = 0; i < trajectory_length; i++) {
    // Updating the estimate with the measurement
    rotational_estimator.updateEstimate(corrupted_input_trajectory[i],
                                        timestamps[i]);
    // Getting the position and velocity estimates
    Eigen::Quaterniond estimated_orientation =
        rotational_estimator.getEstimatedOrientation();
    Eigen::Vector3d estimated_rollrate =
        rotational_estimator.getEstimatedRate();
    // Moving values to arrays
    orientation_estimate_trajectory[i][0] = estimated_orientation.w();
    orientation_estimate_trajectory[i][1] = estimated_orientation.x();
    orientation_estimate_trajectory[i][2] = estimated_orientation.y();
    orientation_estimate_trajectory[i][3] = estimated_orientation.z();
    rollrate_estimate_trajectory[i][0] = estimated_rollrate.x();
    rollrate_estimate_trajectory[i][1] = estimated_rollrate.y();
    rollrate_estimate_trajectory[i][2] = estimated_rollrate.z();
  }

  // Start index for error calculation
  const int start_index = trajectory_length / 2;

  // Calculating position estimate errors
  double orientation_error[3];
  calculateQuaternionRmsError(
      orientation_trajectory, orientation_estimate_trajectory,
      trajectory_length, start_index, orientation_error);

  // Performing test
  EXPECT_NEAR(orientation_error[0], 0, QUAT_ERROR_THRESHOLD)
      << "X errorquaternion estimate error too great";
  EXPECT_NEAR(orientation_error[1], 0, QUAT_ERROR_THRESHOLD)
      << "Y errorquaternion estimate error too great";
  EXPECT_NEAR(orientation_error[2], 0, QUAT_ERROR_THRESHOLD)
      << "Z errorquaternion estimate error too great";

  // Calculating velocity estimate errors
  double omegaError[3];
  calculate3dRmsError(omega_trajectory, rollrate_estimate_trajectory,
                      trajectory_length, start_index, omegaError);

  // Performing test
  EXPECT_NEAR(omegaError[0], 0, OMEGA_ERROR_THRESHOLD)
      << "X rotational velocity estimate error too great";
  EXPECT_NEAR(omegaError[1], 0, OMEGA_ERROR_THRESHOLD)
      << "Y rotational velocity estimate error too great";
  EXPECT_NEAR(omegaError[2], 0, OMEGA_ERROR_THRESHOLD)
      << "Z rotational velocity estimate error too great";
}

/*
 *  GTests Main
 */

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <cmath>
#include <boost/foreach.hpp>
#include <boost/unordered_map.hpp>

#include "fastmath.hpp"
#include "nlopt/nlopt.h"
#include "tpbias.hpp"

typedef std::pair<pos_t, pos_t> TPDistPair;
typedef boost::unordered_map<pos_t, double> DistNormMap;

static double geometric_cdf_upper(double p, unsigned int k)
{
    return std::pow(1 - p, k);
}

static double geometric_logcdf_upper(double p, unsigned int k)
{
    return k * fastlog(1 - p);
}

static double tpbias_loglikelihood(double lp,
                                   pos_t maxtlen,
                                   DistNormMap& distnorm,
                                   const std::vector<TPDistPair>& xs)
{
    double p = exp(lp);
    double norm = 0.0;
    double ppower = 1.0;
    for (pos_t tlen = 0; tlen <= maxtlen; ++tlen) {
        ppower *= 1 - p;
        norm += ppower;

        DistNormMap::iterator i = distnorm.find(tlen);
        if (i != distnorm.end()) {
            i->second = fastlog(norm);
        }
    }

    double ll = 0.0;
    BOOST_FOREACH (const TPDistPair& x, xs)  {
        ll += geometric_logcdf_upper(p, x.first) - distnorm[x.second];
    }

    return ll;
}

struct TPBiasTrainData
{
    TPBiasTrainData(pos_t maxtlen,
                    DistNormMap& distnorm,
                    const std::vector<TPDistPair>& xs)
        : maxtlen(maxtlen)
        , distnorm(distnorm)
        , xs(xs)
    {
    }

    pos_t maxtlen;
    DistNormMap& distnorm;
    const std::vector<TPDistPair>& xs;
};

static double tpbias_nlopt_objective(unsigned int _n, const double* _x,
                                     double* _grad, void* data)
{
    UNUSED(_n);
    UNUSED(_grad);

    TPBiasTrainData* traindata =
        reinterpret_cast<TPBiasTrainData*>(data);

    return tpbias_loglikelihood(_x[0], traindata->maxtlen,
                                traindata->distnorm, traindata->xs);
}

TPBias::TPBias(const std::vector<std::pair<pos_t, pos_t> >& tpdists)
{
    // normalizing constants for fragment disributions given tlen
    boost::unordered_map<pos_t, double> distnorm;

    pos_t maxtlen = 0;
    BOOST_FOREACH (const TPDistPair& tpdist_tlen, tpdists) {
        distnorm[tpdist_tlen.second] = 0.0;
        maxtlen = std::max<pos_t>(maxtlen, tpdist_tlen.second);
    }

    TPBiasTrainData traindata(maxtlen, distnorm, tpdists);

#if 0
    FILE* out = fopen("tpdists.tsv", "w");
    fprintf(out, "pos\ttlen\n");
    BOOST_FOREACH (const TPDistPair& x, tpdists)  {
        fprintf(out, "%ld\t%ld\n", x.first, x.second);
    }
    fclose(out);
#endif

    nlopt_opt opt = nlopt_create(NLOPT_LN_SBPLX, 1);
    double lower_limit = -100;
    double upper_limit = -10.0;
    nlopt_set_lower_bounds(opt, &lower_limit);
    nlopt_set_upper_bounds(opt, &upper_limit);
    nlopt_set_max_objective(opt, tpbias_nlopt_objective,
                            reinterpret_cast<void*>(&traindata));
    //nlopt_set_ftol_abs(opt, 1e-4);

    double xtol_abs = 1e-3;
    nlopt_set_xtol_abs(opt, &xtol_abs);

    p = -12;
    double maxf;
    nlopt_result result = nlopt_optimize(opt, &p, &maxf);
    if (result < 0) {
        Logger::warn("Failed to fit 3' bias model.");
        p = 0.0;
    }

    p = exp(p);

    nlopt_destroy(opt);
}

double TPBias::get_bias(pos_t k)
{
    return geometric_cdf_upper(p, k);
}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <boost/math/tools/roots.hpp>

#include "NOpenMeshType.hpp"
//#include "NOpenMeshDesc.hpp"
#include "NOpenMeshBisect.hpp"

template <typename T>
NOpenMeshBisect<T>::NOpenMeshBisect( const SDF& sdf, const P& a, const P& b , float tolerance )
    :
    func(sdf, a, b ),
    tol(tolerance),
    invalid( func(0)*func(1) > 0 ),  // <-- sdf must have different signs at a and b : ie bracket zero
    degenerate( fabs(func(0)) < tolerance && fabs(func(1)) <  tolerance ),  // <-- no root to find, already there
    iterations(15)
{
}

template <typename T>
void NOpenMeshBisect<T>::bisect( P& frontier, float& t )
{
    std::pair<float, float> root = boost::math::tools::bisect(func, 0.f, 1.f, tol, iterations );

    t = (root.first + root.second)/2. ;

    func.position(frontier, t );
}

template <typename T>
NOpenMeshBisectFunc<T>::NOpenMeshBisectFunc( const SDF& sdf, const P& a, const P& b )
    :
    sdf(sdf),
    a(a),
    b(b)
{
}
template <typename T>
void NOpenMeshBisectFunc<T>::position(P& tp, const float t) const
{
    // parameterized position along a -> b line segment
    //    a(1-t)+t*b   t=0 -> a,   t=1 -> b

    const float s = 1.f - t ;
    tp[0] = a[0]*s + b[0]*t ;
    tp[1] = a[1]*s + b[1]*t ;
    tp[2] = a[2]*s + b[2]*t ;
}

template <typename T>
float NOpenMeshBisectFunc<T>::operator()(const float t) const
{
    // signed distance to CSG left/right/composite object from position on line segement
    P tp ;
    position(tp, t );

    float d = sdf( tp[0], tp[1], tp[2] ) ;

/*
    std::cout << "NOpenMeshBisectFunc<T>"
              << " t " << t
              << " tp " << NOpenMeshDesc<T>::desc_point(tp,8,2)
              << " d " << d
              << std::endl ;
*/

    return d ;
}

template struct NOpenMeshBisectFunc<NOpenMeshType> ;
template struct NOpenMeshBisect<NOpenMeshType> ;


#include "shendk/files/image/pvr/vector_quantizer.h"

#include <cstring>
#include <algorithm>
#include <limits>

#include <Eigen/Dense>

#include "shendk/utils/math.h"
#include "shendk/files/image/pvr/twiddle.h"

namespace shendk {
namespace pvr {

struct VQBlock {

    VQBlock()
        : pixels(Eigen::MatrixXf(4, 4))
    {}

    VQBlock(uint8_t blockSize, uint8_t pixelSize)
        : pixels(Eigen::MatrixXf(blockSize, pixelSize))
    {}

    inline void zero() {
        pixels = pixels.Zero(pixels.rows(), pixels.cols());
    }

    inline void add(VQBlock& block) {
        pixels += block.pixels;
    }

    inline void div(int group) {
        pixels /= group;
    }

    inline float distance(const VQBlock& block) const {
        return std::abs((pixels - block.pixels).sum());
    }

    uint8_t* toArrayTwiddled() {
        // TODO: only works with RGBA blocks
        uint8_t* array = new uint8_t[pixels.rows() * 4];
        uint64_t destinationIndex = 0;
        int64_t sourceIndex = 0;
        uint64_t* twiddleMap = createTwiddleMap(pixels.rows());
        uint32_t blockHeight, blockWidth;
        blockHeight = blockWidth = pixels.rows() / 2;
        for (uint32_t y = 0; y < blockHeight; y++) {
            for (uint32_t x = 0; x < blockWidth; x++) {
                destinationIndex = (twiddleMap[x] << 1) | twiddleMap[y];
                array[destinationIndex * 4]     = static_cast<uint8_t>(pixels(sourceIndex));
                array[destinationIndex * 4 + 1] = static_cast<uint8_t>(pixels(sourceIndex + 1));
                array[destinationIndex * 4 + 2] = static_cast<uint8_t>(pixels(sourceIndex + 2));
                array[destinationIndex * 4 + 3] = static_cast<uint8_t>(pixels(sourceIndex + 3));
                sourceIndex += 4;
            }
        }
        delete[] twiddleMap;
        return array;
    }

    uint8_t* toArray() {
        Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic> matrix = pixels.cast<uint8_t>();
        uint8_t* array = new uint8_t(pixels.size());
        memcpy(array, matrix.data(), pixels.size());
        return array;
        return nullptr;
    }

    uint32_t group = 0;
    Eigen::MatrixXf pixels;
};

int nearest(const VQBlock& block, VQBlock* cluster, int clusterSize, double& nearestDistance) {
    int i = 0;
    int nearestIndex = 0;
    double distance = 0;
    double minDistance = 0;
    for (i = 0; i < clusterSize; i++) {
        minDistance = std::numeric_limits<double>::infinity();
        nearestIndex = block.group;
        for (i = 0; i < clusterSize; i++) {
            if (minDistance > (distance = cluster[i].distance(block))) {
                minDistance = distance;
                nearestIndex = i;
            }
        }
    }
    nearestDistance = minDistance;
    return nearestIndex;
}

std::vector<VQBlock> createBlocks(Image& image, int blockWidth, int blockHeight) {
    //Convert bitmap to blocks
    int width = image.width() / blockWidth;
    int height = image.height() / blockHeight;
    int blockSize = blockWidth * blockHeight * 4;
    int blockCount = width * height;
    std::vector<VQBlock> blocks(blockCount);

    for (int y = 0; y < height - 1; y += blockHeight) {
        for (int x = 0; x < width - 1; x += blockWidth) {
            int blockIndex = (y / blockHeight) * width + (x / blockWidth);
            blocks[blockIndex] = VQBlock(blockWidth * blockHeight, 4);
            for (int k = 0; k < blockHeight; k++) {
                for (int l = 0; l < blockWidth; l++) {
                    int imageIndex = ((y + k) * width + (x + l)) * 4;
                    int subVectorIndex = k * blockWidth + l;
                    blocks[blockIndex].pixels(subVectorIndex * 4)     = image[imageIndex].b;
                    blocks[blockIndex].pixels(subVectorIndex * 4 + 1) = image[imageIndex].g;
                    blocks[blockIndex].pixels(subVectorIndex * 4 + 2) = image[imageIndex].r;
                    blocks[blockIndex].pixels(subVectorIndex * 4 + 3) = image[imageIndex].a;
                }
            }
        }
    }
    return blocks;
}

std::vector<VQBlock> initializeCodeBook(std::vector<VQBlock> imageBlocks, int codeBookSize, bool fastInitialization) {
    std::vector<VQBlock> codeBook(codeBookSize);
    if (fastInitialization) {
        int i = 0;
        for (i = 0; i < codeBookSize; i++)
        {
            codeBook[i % codeBookSize] = VQBlock(imageBlocks[i]);
        }
        for (i = 0; i < imageBlocks.size(); i++)
        {
            imageBlocks[i].group = i % codeBookSize;
        }
        return codeBook;
    } else {
        int i = 0;
        int imageLength = imageBlocks.size();
        double sum = 0.0;
        double* distances = new double[imageBlocks.size()];
        codeBook[0] = VQBlock(imageBlocks[std::rand() % imageBlocks.size()]);
        for (int cluster = 1; cluster < codeBookSize; cluster++) {
            sum = 0;
            for (i = 0; i < imageLength; i++) {
                nearest(imageBlocks[i], codeBook.data(), cluster, distances[i]);
                sum += distances[i];
            }
            sum = sum * std::clamp(std::rand(), 0, 0x7fff) / (0x7fff - 1.0);
            for (i = 0; i < imageLength; i++) {
                if ((sum -= distances[i]) > 0) continue;
                codeBook[cluster] = VQBlock(imageBlocks[i]);
                break;
            }
        }
        double dump = 0.0;
        for (i = 0; i < imageLength; i++) {
            imageBlocks[i].group = nearest(imageBlocks[i], codeBook.data(), codeBookSize, dump);
        }
        return codeBook;
    }
}

std::vector<VQBlock> createCodebook(Image& image, int codeBookSize, int blockWidth, int blockHeight)
{
    if (image.width() % blockWidth != 0 || image.height() % blockHeight != 0) {
        throw std::runtime_error("The image can't be devided by the given block size!");
    }

    std::vector<VQBlock> imageBlocks = createBlocks(image, blockWidth, blockHeight);
    std::vector<VQBlock> codeBookBlocks = initializeCodeBook(imageBlocks, codeBookSize);

    int i = 0;
    int j = 0;
    int changed = 0;
    int nearestIndex = 0;
    VQBlock imageBlock;
    VQBlock codeBookBlock;
    double dump = 0.0;
    int runs = 0;

    do {
        for (i = 0; i < codeBookSize; i++) {
            codeBookBlock = codeBookBlocks[i];
            codeBookBlock.group = 0;
            codeBookBlock.zero();
        }
        for (j = 0; j < imageBlocks.size(); j++) {
            imageBlock = imageBlocks[j];
            codeBookBlock = codeBookBlocks[imageBlock.group];
            codeBookBlock.group += 1;
            codeBookBlock.add(imageBlock);
        }
        for (i = 0; i < codeBookSize; i++) {
            codeBookBlock = codeBookBlocks[i];
            codeBookBlock.div(codeBookBlock.group);
        }
        changed = 0;
        for (j = 0; j < imageBlocks.size(); j++) {
            imageBlock = imageBlocks[j];
            nearestIndex = nearest(imageBlock, codeBookBlocks.data(), codeBookSize, dump);
            if (nearestIndex != imageBlock.group)
            {
                changed++;
                imageBlock.group = nearestIndex;
            }
        }
        runs++;
    } while (changed > (imageBlocks.size() >> 10));

    for (i = 0; i < codeBookSize; i++) {
        codeBookBlock = codeBookBlocks[i];
        codeBookBlock.group = i;
    }
    return codeBookBlocks;
}

uint8_t* quantizeImage(Image& image, std::vector<VQBlock> codeBook, uint8_t blockSize) {
    std::vector<VQBlock> blocks = createBlocks(image, blockSize, blockSize);
    uint8_t* result = new uint8_t[blocks.size()];
    double dump = 0.0;
    for (int i = 0; i < blocks.size(); i++) {
        VQBlock block = blocks[i];
        result[i] = nearest(block, codeBook.data(), codeBook.size(), dump);
    }
    return result;
}

uint8_t* quantizeImage(Image& image, uint32_t codeBookSize, uint8_t blockSize, uint8_t pixelSize) {
    std::vector<VQBlock> codeBook = createCodebook(image, codeBookSize);
    return quantizeImage(image, codeBook, blockSize);
}

}
}


#include <algorithm>
#include <cmath>
#include <iostream>

#include <minbase/crossplat.h>
#include <minimgapi/minimgapi-helpers.hpp>
#include <minimgapi/imgguard.hpp>
#include <minimgio/minimgio.h>
#include <mximg/image.h>
#include <mximg/ocv.h>
#include <vi_cvt/std/exception_macros.hpp>
#include <vi_cvt/ocv/image.hpp>

#include <colorseg/color_distance_func.h>
#include <colorseg/colorspace_homography.hpp>
#include <colorseg/color_vertex.h>
#include <remseg/segmentator.hpp>
#include <remseg/utils.h>

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

THIRDPARTY_INCLUDES_BEGIN
#include <boost/filesystem.hpp>
#include <tclap/CmdLine.h>
THIRDPARTY_INCLUDES_END

namespace bfs = boost::filesystem;

using namespace vi::remseg;
using namespace vi::colorseg;

const int    BILATERAL_D = 15;
const double BILATERAL_SIGMA_COLOR = 50;
const double BILATERAL_SIGMA_SPACE = 50;

double KL(Eigen::Vector3d const & mean1, Eigen::Vector3d const & mean2,
          Eigen::Matrix3d const & cov1, Eigen::Matrix3d const & cov2)
{
  auto s1 = (cov2.inverse() * cov1).trace();
  auto s2 = (mean2 - mean1).transpose() * cov2.inverse() * (mean2 - mean1);
  auto s3 = std::log(cov2.determinant() / cov1.determinant());
  return (s1 + s2 + s3 - 3)/ 2;
}

void obtainBlockList(std::set<std::pair<int, int> > & blockList,
                      Segmentator<ColorVertex> const & segmentator,
                      double threshold)
{
  const ImageMap &imageMap = segmentator.getImageMap();
  auto stats = imageMap.getSegmentStats();
  for (auto const & stat : stats)
  {
    SegmentID id = stat.first;
    ColorVertex * v = segmentator.vertexById(id);
    ColorVertex::HelperStats const & hs = v->getHelperStats();
    std::vector<EdgeValue> dists;
    for (auto const& n : stat.second.neighbours)
    {
      ColorVertex * v_n = segmentator.vertexById(n);
      ColorVertex::HelperStats const & hs_n = v_n->getHelperStats();
      dists.push_back(KL(hs.mean, hs_n.mean, hs.covariance, hs_n.covariance));
    }

    if (!dists.empty() && *std::min_element(dists.begin(), dists.end()) > threshold)
      blockList.insert(stat.second.leftTopPoint);
  }
}

void offscaleFix(Segmentator<ColorVertex> & segmentator, double threshold)
{
  const ImageMap &imageMap = segmentator.getImageMap();
  auto const stats = imageMap.getSegmentStats();
  std::set<SegmentID> merged;

  for (auto const & stat : stats)
  {
    if (merged.find(stat.first) != merged.end())
      continue;

    ColorVertex * v = segmentator.vertexById(stat.first);
    ColorVertex::HelperStats const & hs = v->getHelperStats();

    if (homographyInv(hs.mean, ColorVertex::getHomographyA(),
                      ColorVertex::getHomographyK()).mean() < threshold)
      continue;

    if (v->size() == 1)
    {
      merged.insert(segmentator.getId(v->begin()->vertex));
      segmentator.merge(v, dynamic_cast<ColorVertex*>(v->begin()->vertex));
    }
    else
    {
      int i = 0, j = 0;

      std::vector<int> ids;
      std::set<int> merged_ids;

      for (ConstJoint it = v->begin(); it != v->end(); it++)
        ids.push_back(segmentator.getId(it->vertex));

      for (auto it1 = ids.begin(); it1 != ids.end(); it1++)
      {
        if (merged_ids.find(*it1) != merged_ids.end())
          continue;

        for (auto it2 = v->begin(); it2 != v->end(); it2++)
        {
          ColorVertex * v1 = segmentator.vertexById(*it1);
          ColorVertex * v2 = dynamic_cast<ColorVertex*>(it2->vertex);

          if (v1 == v2)
            continue;

          Edge * edge = 0;
          for (auto it3 = v2->begin(); it3 != v2->end(); it3++)
          if (dynamic_cast<ColorVertex*>(it3->vertex) == v1)
            edge = it3->edge;

          if (edge == 0)
            continue;

          if (!isLTCluster(v1, v2))
            continue;

          merged.insert(segmentator.getId(v1));
          merged.insert(segmentator.getId(v2));

          segmentator.merge(v, v1);
          segmentator.merge(v, v2);

          merged_ids.insert(*it1);
          merged_ids.insert(segmentator.getId(v2));
          break;
        }
      }
    }
    segmentator.updateMapping();
  }
}

int main(int argc, const char *argv[])
{
  i8r::AutoShutdown i8r_shutdown;

  TCLAP::CmdLine cmd("Run Range-Based Region Merge Segmentation on a Single image");
  TCLAP::ValueArg<double> errorLimit("e", "error_limit", "average error limit", false, -1, "double", cmd);
  TCLAP::ValueArg<int> segmentsLimit("n", "segm_limit", "segments limit", false, -1, "int", cmd);
  TCLAP::UnlabeledValueArg<std::string> imagePath("image", "path to source RGB-image in tif-convertible format", true, "", "string", cmd);
  TCLAP::ValueArg<std::string> output("o", "output", "path to output dir", false, ".", "string", cmd);
  TCLAP::SwitchArg debug("d", "debug", "debug mode", cmd, false);
  TCLAP::ValueArg<int> debugIter("i", "debug_iter", "debug iterations", false, 1, "int", cmd);
  TCLAP::ValueArg<int> maxSegments("s", "max_segments", "max segments for debug output", false, -1, "int", cmd);
  TCLAP::ValueArg<double> blockingThresh("g", "blocking_thresh", "blocking threshold value", false, 1, "double", cmd);
  TCLAP::ValueArg<double> maxModelDistance("", "model_distance", "model distance", false, 20, "double", cmd);
  TCLAP::ValueArg<double> glareThresh("", "glare_thresh", "glare threshold", false, 230, "double", cmd);
  TCLAP::SwitchArg prefilter("p", "prefilter", "use image pre-filtering", cmd, false);

  cmd.parse(argc, argv);

  ColorVertex::setMaxModelDistance(maxModelDistance.getValue());

  if (!bfs::is_directory(output.getValue()))
    throw std::runtime_error("Failed to find output directory " + output.getValue());

  if (debug.getValue())
     i8r::configure(output.getValue());

  std::string const basename = bfs::path(imagePath.getValue()).stem().string();
  std::string const imgres_filename = bfs::absolute(basename + ".png", output.getValue()).string();
  std::string const filtered_filename = bfs::absolute(basename + ".filtered.png", output.getValue()).string();

  try
  {
    mximg::PImage image = mximg::Image::imread(imagePath.getValue().c_str());
    if ((*image)->channels != 3)
      throw std::runtime_error("Image should have exact 3 channels for color segmentation");

    cv::Mat cv_image_filtered;
    if (prefilter.getValue())
    {
      cv::Mat cv_image = vi::cvt::ocv::as_cvmat(*image);
      cv::bilateralFilter(cv_image, cv_image_filtered, BILATERAL_D, BILATERAL_SIGMA_COLOR, BILATERAL_SIGMA_SPACE);
      cv::imwrite(filtered_filename, cv_image_filtered);
      //MinImg min_image_filtered = vi::cvt::ocv::as_minimg(cv_image_filtered);
      image = mximg::createByCopy(cv_image_filtered);
    }

    auto dbg = i8r::logger("debug." + basename + ".pointlike");
    Segmentator<ColorVertex> segmentatorPointlike(*image, shouldnotcall, criteria_r0, true);
    segmentatorPointlike.mergeToLimit(-1, errorLimit.getValue(), segmentsLimit.getValue(),
                                      dbg, debugIter.getValue(), maxSegments.getValue());

    std::set<std::pair<int, int> > blockList;
    obtainBlockList(blockList, segmentatorPointlike, blockingThresh.getValue());

    Segmentator<ColorVertex> segmentatorLinear(*image, &segmentatorPointlike.getImageMap(),
                                               error_r1, criteria_r1,
                                               blockList, BLOCK_SEGMENTS, true);
    segmentatorLinear.mergeToLimit(-1, errorLimit.getValue() * std::sqrt(2./3), segmentsLimit.getValue(),
                                   dbg, debugIter.getValue(), maxSegments.getValue());

    Segmentator<ColorVertex> segmentatorPlanar(*image, &segmentatorLinear.getImageMap(),
                                                error_r2, criteria_r2,
                                                {}, BLOCK_SEGMENTS, true);
    segmentatorPlanar.mergeToLimit(-1, errorLimit.getValue() * std::sqrt(1./3), segmentsLimit.getValue(),
                                   dbg, debugIter.getValue(), maxSegments.getValue());

    offscaleFix(segmentatorPlanar, glareThresh.getValue());
    const ImageMap &imageMap = segmentatorPlanar.getImageMap();

    DECLARE_GUARDED_MINIMG(imgres);
    visualize(&imgres, imageMap);
    THROW_ON_MINERR(SaveMinImage(imgres_filename.c_str(), &imgres));
  }
  catch (std::exception const& e)
  {
    std::cerr << "Exception caught: " << e.what() << "\n";
    return 1;
  }
  catch (...)
  {
    std::cerr << "UNTYPED exception\n";
    return 2;
  }

  return 0;
}