// Copyright (C) 2014 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 "theia/sfm/estimators/estimate_essential_matrix.h"

#include <Eigen/Core>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/solvers/estimator.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/five_point_relative_pose.h"
#include "theia/sfm/pose/util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

// An estimator for computing the essential matrix from 5 feature
// correspondences. The feature correspondences should be normalized
// by the focal length with the principal point at (0, 0).
class EssentialMatrixEstimator
    : public Estimator<FeatureCorrespondence, Eigen::Matrix3d> {
 public:
  EssentialMatrixEstimator() {}

  // 5 correspondences are needed to determine an essential matrix.
  double SampleSize() const { return 5; }

  // Estimates candidate essential matrices from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<Eigen::Matrix3d>* essential_matrices) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    image1_points.reserve(correspondences.size());
    image2_points.reserve(correspondences.size());
    for (int i = 0; i < correspondences.size(); i++) {
      image1_points.emplace_back(correspondences[i].feature1);
      image2_points.emplace_back(correspondences[i].feature2);
    }

    return FivePointRelativePose(image1_points,
                                 image2_points,
                                 essential_matrices);
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& correspondence,
               const Eigen::Matrix3d& essential_matrix) const {
    return SquaredSampsonDistance(essential_matrix,
                                  correspondence.feature1,
                                  correspondence.feature2);
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(EssentialMatrixEstimator);
};

}  // namespace

bool EstimateEssentialMatrix(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    Eigen::Matrix3d* essential_matrix,
    RansacSummary* ransac_summary) {
  EssentialMatrixEstimator essential_matrix_estimator;
  std::unique_ptr<SampleConsensusEstimator<EssentialMatrixEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type,
                                                ransac_params,
                                                essential_matrix_estimator);

  // Estimate essential matrix.
  return ransac->Estimate(normalized_correspondences,
                          essential_matrix,
                          ransac_summary);
}

}  // namespace theia


#pragma once
#include <Eigen/Dense>
#include <memory>

typedef std::pair<Eigen::Vector3d, double> PlaneParam; //normal, offset
typedef std::pair<std::pair<Eigen::Vector3d, Eigen::Vector3d>, double> CylinderParam; //(point, direction), radius
typedef std::pair<Eigen::Vector3d, Eigen::Vector3d> Edge3d;
typedef std::pair<Eigen::Vector2d, Eigen::Vector2d> Edge2d;

struct PointCloud3 {
    Eigen::MatrixX3d P;
    Eigen::MatrixX3d N;
    typedef std::shared_ptr<PointCloud3> Handle;
};

struct PointCloud2 {
    Eigen::MatrixX2d P;
    typedef std::shared_ptr<PointCloud2> Handle;
};


#include <ros/ros.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <sophus/se3.hpp>
#include <math.h>
#include <mutex>
#include <atomic>

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

// visp
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>

class PBVS {
public:
    PBVS():s_t_(vpFeatureTranslation::cMo), s_tu_(vpFeatureThetaU::cdRc),
                              s_star_t_(vpFeatureTranslation::cMo), s_star_tu_(vpFeatureThetaU::cdRc),
                              v_c_(6) {}
    ~PBVS() {task_.kill();}
    void Init() {
        cdMo_[0][0] = -1; cdMo_[0][1] = 0; cdMo_[0][2] =  0; cdMo_[0][3] = 0.1112315;
        cdMo_[1][0] = 0; cdMo_[1][1] = -1; cdMo_[1][2] =  0; cdMo_[1][3] = 0.21367;
        cdMo_[2][0] = 0; cdMo_[2][1] =  0; cdMo_[2][2] = 1; cdMo_[2][3] = 0.6;
        s_star_t_.buildFrom(cdMo_);
        task_.setServo(vpServo::EYEINHAND_CAMERA);    // Camera is monted on the robot end-effector and velocities are computed in the camera frame
        task_.setInteractionMatrixType(vpServo::CURRENT);    // Interaction matrix is computed with the current visual features s
        task_.setLambda(0.1);         // Set the contant 0.5
        task_.addFeature(s_t_, s_star_t_);          // Add current and desired translation feature
        task_.addFeature(s_tu_, s_star_tu_);           // Add current and desired ThetaU feature for the rotation
    }
    bool Calc(const Eigen::Transform<double, 3, Eigen::Isometry>& cMo, Eigen::Matrix<double, 6, 1>& cart_v) {
        cMo_[0][0] = cMo(0, 0); cMo_[0][1] = cMo(0, 1); cMo_[0][2] = cMo(0, 2); cMo_[0][3] = cMo(0, 3);
        cMo_[1][0] = cMo(1, 0); cMo_[1][1] = cMo(1, 1); cMo_[1][2] = cMo(1, 2); cMo_[1][3] = cMo(1, 3);
        cMo_[2][0] = cMo(2, 0); cMo_[2][1] = cMo(2, 1); cMo_[2][2] = cMo(2, 2); cMo_[2][3] = cMo(2, 3);
        cMo_[3][0] = cMo(3, 0); cMo_[3][1] = cMo(3, 1); cMo_[3][2] = cMo(3, 2); cMo_[3][3] = cMo(3, 3);
        cdMc_ = cdMo_ * cMo_.inverse();
        s_t_.buildFrom(cMo_);
        s_tu_.buildFrom(cdMc_);
        v_c_ = task_.computeControlLaw(); // Compute camera velocity skew
        cart_v << v_c_[0], v_c_[1], v_c_[2], v_c_[3], v_c_[4], v_c_[5];
        error_ = (task_.getError()).sumSquare(); // error = s^2 - s_star^2
        // ROS_INFO_STREAM("error_: " << error_);
        if(error_ < 0.00001) {
            return true;
        }
        return false;
        // ROS_INFO_STREAM("error_: " << error_);
    }
private:
    vpHomogeneousMatrix cdMc_;      //the displacement the camera has to achieve to move from the desired camera frame and the current one
    vpHomogeneousMatrix cMo_;
    vpHomogeneousMatrix oMo_;
    vpHomogeneousMatrix cdMo_;
    vpFeatureTranslation s_t_;     //the current visual feature s
    vpFeatureThetaU s_tu_;
    vpFeatureTranslation s_star_t_;      //the desired visual feature s*
    vpFeatureThetaU s_star_tu_;
    vpServo task_;         // the visual servo task
    vpColVector v_c_;      // Camera velocity
    double error_;         // Task error
};

class SE3Pid {
public:
    SE3Pid():kp_(0.01), ki_(0.0), kd_(0.0), dt_(1), max_(0.1), integral_(Eigen::Matrix<double, 6, 1>::Zero()), pre_error_(Eigen::Matrix<double, 6, 1>::Zero()) {}
    ~SE3Pid() {}
    bool Init() {
        cdMo_ = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
        Eigen::Matrix<double, 3, 3> rot;
        rot << -1.0, 0.0, 0.0,
                0.0, -1.0, 0.0,
                0.0, 0.0, 1.0;
        cdMo_.rotate(Eigen::Quaterniond(rot));
        cdMo_.pretranslate(Eigen::Vector3d(0.1112315, 0.21367, 0.6));
        cdMo_se3_ = Sophus::SE3d(Eigen::Quaterniond(cdMo_.matrix().block<3, 3>(0, 0)), Eigen::Vector3d(cdMo_.matrix().block<3, 1>(0, 3)));
        oMcd_se3_ = Sophus::SE3d(Eigen::Quaterniond(cdMo_.inverse().matrix().block<3, 3>(0, 0)), Eigen::Vector3d(cdMo_.inverse().matrix().block<3, 1>(0, 3)));
    }
    bool Calc(const Eigen::Transform<double, 3, Eigen::Isometry>& cMo, Eigen::Transform<double, 3, Eigen::Isometry>& pose)  {
        Sophus::SE3d cMo_se3(Eigen::Quaterniond(cMo.matrix().block<3, 3>(0, 0)), Eigen::Vector3d(cMo.matrix().block<3, 1>(0, 3)));
        Eigen::Matrix<double, 6, 1> error = (cMo_se3.inverse() * cdMo_se3_).log();
        Eigen::Matrix<double, 6, 1> diff = 0.1*error;
        Sophus::SE3d se_i = cMo_se3 * Sophus::SE3d::exp(diff);
        pose = se_i.matrix();
        return false;
    }
    bool Calc(const Eigen::Transform<double, 3, Eigen::Isometry>& cMo, Eigen::Matrix<double, 6, 1>& cart_v)  {
        Sophus::SE3d cMo_se3(Eigen::Quaterniond(cMo.matrix().block<3, 3>(0, 0)), Eigen::Vector3d(cMo.matrix().block<3, 1>(0, 3)));
        Eigen::Matrix<double, 6, 1> error = (cMo_se3.inverse() * cdMo_se3_).log();
        Eigen::Matrix<double, 6, 1> delta = 0.1*error;
        // ROS_INFO_STREAM("delta: " << delta);
        cart_v = delta;
        // if (error.norm() < 1e-6) {
        //     return true;
        // }
        // // Proportional term
        // Eigen::Matrix<double, 6, 1> p_out = kp_ * error;
        // // Integral term
        // integral_ += error * dt_;
        // Eigen::Matrix<double, 6, 1> i_out = ki_ * integral_;
        // // Derivative term
        // Eigen::Matrix<double, 6, 1> derivative = (error - pre_error_) / dt_;
        // Eigen::Matrix<double, 6, 1> d_out = kd_ * derivative;
        // // Calculate total output
        // Eigen::Matrix<double, 6, 1> diff = p_out + i_out + d_out;
        // if (diff.norm() > max_) {
        //     ROS_INFO_STREAM("pid is too large: " << diff);
        //     diff = diff * max_/diff.norm();
        // }
        // cart_v = diff;
        // pre_error_ = error;
        // return false;
    }
    bool Calc2o(const Eigen::Transform<double, 3, Eigen::Isometry>& cMo, Eigen::Matrix<double, 6, 1>& cart_v)  {
        Sophus::SE3d oMc_se3(Eigen::Quaterniond(cMo.inverse().matrix().block<3, 3>(0, 0)), Eigen::Vector3d(cMo.inverse().matrix().block<3, 1>(0, 3)));
        Eigen::Matrix<double, 6, 1> error = (oMc_se3.inverse() * oMcd_se3_).log();
        Eigen::Matrix<double, 6, 1> delta = 0.1*error;
        // ROS_INFO_STREAM("delta: " << delta);
        cart_v = delta;
    }
private:
    float kp_;
    float ki_;
    float kd_;
    float dt_;
    float max_;
    Eigen::Matrix<double, 6, 1> pre_error_;
    Eigen::Matrix<double, 6, 1> integral_;
    Eigen::Transform<double, 3, Eigen::Isometry> cdMo_;
    Sophus::SE3d cdMo_se3_;
    Sophus::SE3d oMcd_se3_;
};

class Robot {
public:
    // Robot(ros::NodeHandle& nh):nh_(nh), planning_group_("panda_arm"), move_group_("panda_arm"), robot_model_loader_("robot_description") {
    Robot(ros::NodeHandle& nh):nh_(nh), planning_group_("panda_arm"), move_group_("panda_arm"), reference_point_position_(0.0, 0.0, 0.0) {
        // std::vector<double> joints = move_group_.getCurrentJointValues();
        std::vector<double> joints = {0, 0, 0, -M_PI/2, 0, M_PI/2, 0};
        move_group_.setStartStateToCurrentState();
        move_group_.setJointValueTarget(joints);
        move_group_.move();
    }
    ~Robot() {}
    Eigen::Transform<double, 3, Eigen::Isometry> bMe() {
        geometry_msgs::PoseStamped pose_msg = move_group_.getCurrentPose("panda_link8");
        Eigen::Transform<double, 3, Eigen::Isometry> pose = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
        pose.rotate(Eigen::Quaterniond(pose_msg.pose.orientation.w, pose_msg.pose.orientation.x, pose_msg.pose.orientation.y, pose_msg.pose.orientation.z));
        pose.pretranslate(Eigen::Vector3d(pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z));
        return pose;
    }
    // Eigen::MatrixXd jacobian() {
    //     kinematic_state_ = move_group_.getCurrentState();
    //     kinematic_state_->getJacobian(kinematic_state_->getJointModelGroup("panda_arm"),
    //                            kinematic_state_->getLinkModel(kinematic_state_->getJointModelGroup("panda_arm")->getLinkModelNames().back()),
    //                            reference_point_position_, jacobian_);
    //     return jacobian_;
    // }
    Eigen::MatrixXd jacobian() {
        kinematic_state_ = move_group_.getCurrentState();
        // ROS_INFO_STREAM(kinematic_state_->getJointModelGroup("panda_arm")->getLinkModelNames().back());
        return kinematic_state_->getJacobian(kinematic_state_->getJointModelGroup("panda_arm"));
    }
    bool Move(const Eigen::Matrix<double, 7, 1>& joint_v) {
        joint_position_ = move_group_.getCurrentJointValues();
        for (int i=0; i<7; i++) {
            joint_position_.at(i) += joint_v(i, 0);
        }
        move_group_.setStartStateToCurrentState();
        move_group_.setJointValueTarget(joint_position_);
        move_group_.move();
    }
    bool Move(const Eigen::Transform<double, 3, Eigen::Isometry>& pose) {
        move_group_.setStartStateToCurrentState();
        move_group_.setPoseTarget(pose);
        move_group_.move();
    }
private:
    ros::NodeHandle nh_;
    std::string planning_group_;
    moveit::planning_interface::MoveGroupInterface move_group_;
    robot_state::RobotStatePtr kinematic_state_;
    Eigen::Vector3d reference_point_position_;
    Eigen::MatrixXd jacobian_;
    std::vector<double> joint_position_;

    geometry_msgs::PoseStamped pose_msg_;
    Eigen::Transform<double, 3, Eigen::Isometry> pose_eg_;
};

class VisualServoing {
public:
    VisualServoing(ros::NodeHandle& nh):nh_(nh), robot_(nh) {
        eMc_ = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
        Eigen::Matrix<double, 3, 3> rot;
        rot << 0.0, 1.0, 0.0,
                -1.0, 0.0, 0.0,
                0.0, 0.0, 1.0;
        eMc_.rotate(Eigen::Quaterniond(rot));
        eMc_.pretranslate(Eigen::Vector3d(0.0, 0.0, 0.02));

        cdMo_ = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
        rot << -1.0, 0.0, 0.0,
                0.0, -1.0, 0.0,
                0.0, 0.0, 1.0;
        cdMo_.rotate(Eigen::Quaterniond(rot));
        cdMo_.pretranslate(Eigen::Vector3d(0.1112315, 0.21367, 0.7107));

        cVe_ = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Transform<double, 3, Eigen::Isometry> cMe = eMc_.inverse();
        Eigen::Matrix<double, 3, 3> t_skew;
        t_skew << 0, -cMe.matrix()(2, 3), cMe.matrix()(1, 3),
              cMe.matrix()(2, 3), 0, -cMe.matrix()(0, 3),
              -cMe.matrix()(1, 3), cMe.matrix()(0, 3), 0;
        Eigen::Matrix<double, 3, 3> r(cMe.matrix().block<3, 3>(0, 0));
        Eigen::Matrix<double, 3, 3> t_r = t_skew * r;
        for (unsigned int i = 0; i < 3; i++) {
            for (unsigned int j = 0; j < 3; j++) {
                cVe_(i, j) = r(i, j);
                cVe_(i+3, j+3) = r(i, j);
                cVe_(i, j+3) = t_r(i, j);
            }
        }

        pbvs_.Init();
        pid_.Init();
        cMo_sub_ = nh_.subscribe("board/pose", 1, &VisualServoing::CalibPoseCallback, this);
    }
    ~VisualServoing() {}
    void CalibPoseCallback(const geometry_msgs::PoseStamped& cMo_msg) {
        // std::unique_lock<std::mutex> locker(mutex_);
        Eigen::Transform<double, 3, Eigen::Isometry> cMo = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
        cMo.rotate(Eigen::Quaterniond(cMo_msg.pose.orientation.w, cMo_msg.pose.orientation.x, cMo_msg.pose.orientation.y, cMo_msg.pose.orientation.z));
        cMo.pretranslate(Eigen::Vector3d(cMo_msg.pose.position.x, cMo_msg.pose.position.y, cMo_msg.pose.position.z));
        // ROS_INFO_STREAM("cMo: " << cMo.matrix());
        // locker.unlock();
        mutex_.lock();
        cMo_ = cMo;
        mutex_.unlock();
    }
    void RunVisp() {
        Eigen::Transform<double, 3, Eigen::Isometry> cMo;
        mutex_.lock();
        cMo = cMo_;
        mutex_.unlock();
        if (pbvs_.Calc(cMo, cart_v_)) {
            ROS_INFO_STREAM("motion_finished");
            return;
        }
        ROS_INFO_STREAM("pbvs_ cart_v_:" << cart_v_*10);
        jacobian_ = robot_.jacobian();
        Eigen::Matrix<double, 6, 1> cart_v;
        // cart_v << cart_v_(1, 0), cart_v_(0, 0), -cart_v_(2, 0), cart_v_(4, 0), cart_v_(3, 0), -cart_v_(5, 0);
        cart_v << cart_v_(1, 0), cart_v_(0, 0), -cart_v_(2, 0), cart_v_(4, 0), cart_v_(3, 0), -cart_v_(5, 0);
        Eigen::Matrix<double, 7, 1> q_dot = jacobian_.transpose() * (jacobian_ * jacobian_.transpose()).inverse() * cart_v;
        // robot_.Move(q_dot);
    }
    void RunPid() {
        Eigen::Transform<double, 3, Eigen::Isometry> cMo;
        mutex_.lock();
        cMo = cMo_;
        mutex_.unlock();
        pid_.Calc(cMo, cart_v_);
        ROS_INFO_STREAM("pid_ cart_v_:" << cart_v_*10);
        jacobian_ = robot_.jacobian();
        Eigen::Matrix<double, 6, 1> cart_v;
        cart_v << cart_v_(1, 0), cart_v_(0, 0), cart_v_(2, 0), cart_v_(4, 0), cart_v_(3, 0), cart_v_(5, 0);
        sleep(1);
        Eigen::Matrix<double, 7, 1> q_dot = jacobian_.transpose() * (jacobian_ * jacobian_.transpose()).inverse() * cart_v;
        // robot_.Move(q_dot);
    }
    void RunPid2o() {
        jacobian_ = robot_.jacobian();
        Eigen::Matrix<double, 3, 3> eRb = robot_.bMe().inverse().matrix().block<3, 3>(0, 0);
        Eigen::Matrix<double, 6, 6> eVb = Eigen::Matrix<double, 6, 6>::Zero();
        for(int i=0; i<3; i++) {
            for (int j = 0; j < 3; j++) {
                eVb(i, j) = eRb(i, j);
                eVb(i+3, j+3) = eRb(i, j);
            }
        }
        Eigen::MatrixXd eJe = eVb * jacobian_;
        Eigen::Transform<double, 3, Eigen::Isometry> cMo;
        mutex_.lock();
        cMo = cMo_;
        mutex_.unlock();
        pid_.Calc2o(cMo, cart_v_);
        ROS_INFO_STREAM("pid_ Calc2o cart_v_:" << cart_v_*10);
        // pid_.Calc(cMo, cart_v_);
        // ROS_INFO_STREAM("pid_ Calc cart_v_:" << cart_v_*10);
        // pbvs_.Calc(cMo, cart_v_);
        // ROS_INFO_STREAM("pbvs_ Calc cart_v_:" << cart_v_*10);
        Eigen::Matrix<double, 6, 1> cart_v;
        cart_v << cart_v_(1, 0), -cart_v_(0, 0), cart_v_(2, 0), cart_v_(4, 0), -cart_v_(3, 0), cart_v_(5, 0);
        // sleep(1);
        Eigen::Matrix<double, 7, 1> q_dot = eJe.transpose() * (eJe * eJe.transpose()).inverse() * cVe_.inverse() * cart_v_;
        robot_.Move(q_dot);
    }
    void RunPidPose() {
        Eigen::Transform<double, 3, Eigen::Isometry> cMo;
        mutex_.lock();
        cMo = cMo_;
        mutex_.unlock();
        Eigen::Transform<double, 3, Eigen::Isometry> cMo_next;
        pid_.Calc(cMo, cMo_next);
        Eigen::Transform<double, 3, Eigen::Isometry> bMo = robot_.bMe() * eMc_ * cMo;
        Eigen::Transform<double, 3, Eigen::Isometry> bMe_next = bMo * cMo_next.inverse()*eMc_.inverse();
        robot_.Move(bMe_next);
    }
    void Run() {
        jacobian_ = robot_.jacobian();
        Eigen::Matrix<double, 3, 3> eRb = robot_.bMe().inverse().matrix().block<3, 3>(0, 0);
        Eigen::Matrix<double, 6, 6> eVb = Eigen::Matrix<double, 6, 6>::Zero();
        for(int i=0; i<3; i++) {
            for (int j = 0; j < 3; j++) {
                eVb(i, j) = eRb(i, j);
                eVb(i+3, j+3) = eRb(i, j);
            }
        }
        Eigen::MatrixXd eJe = eVb * jacobian_;
        Eigen::Matrix<double, 6, 1> cart_v;
        cart_v << 0, 0, 0, 0.01, 0.01, 0;
        sleep(1);
        Eigen::Matrix<double, 7, 1> q_dot = eJe.transpose() * (eJe * eJe.transpose()).inverse() * cVe_.inverse() * cart_v;
        robot_.Move(q_dot);
    }
private:
    ros::NodeHandle nh_;
    ros::Subscriber cMo_sub_;
    Robot robot_;
    SE3Pid pid_;
    PBVS pbvs_;
    Eigen::Transform<double, 3, Eigen::Isometry> bMe_;
    Eigen::Transform<double, 3, Eigen::Isometry> bMc_;
    Eigen::Transform<double, 3, Eigen::Isometry> eMc_;
    Eigen::Transform<double, 3, Eigen::Isometry> bMcd_;
    Eigen::Transform<double, 3, Eigen::Isometry> cdMo_;
    // std::atomic<Eigen::Transform<double, 3, Eigen::Isometry>> cMo_;
    Eigen::Transform<double, 3, Eigen::Isometry> cMo_;
    Eigen::Matrix<double, 6, 1> cart_v_;
    Eigen::MatrixXd jacobian_;
    Eigen::Matrix<double, 6, 6> cVe_;
    std::mutex mutex_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "panda_visual_servoing");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(4);
    spinner.start();
    ROS_INFO("hello panda_visual_servoing");

    VisualServoing vs(nh);
    while(1) {
        // vs.RunVisp();
        // vs.RunPid();
        vs.RunPid2o();
        // vs.RunPidPose();
        // vs.Run();
    }

    // ros::waitForShutdown();
    return 0;
}


#pragma once

#include <vector>
#include <Eigen/Dense>

class BaseVector {
public:
    BaseVector() {}

    explicit BaseVector(const std::vector<double>& data) {
        _data = data;
    }

    // Read-only access when indexing the data
    const double operator[](int index) const {
        return _data[index];
    }

    size_t size() { return _data.size(); }

    const std::vector<double> data() const { return _data; }

    const std::vector<double>* data_ptr() const { return &_data; }

protected:
    std::vector<double> _data;

    void push_back(double value) {_data.push_back(value);}
};

class Vector2 : public BaseVector {
public:
    using BaseVector::BaseVector;

    Vector2(double x, double y) {
        _data.push_back(x);
        _data.push_back(y);
    }

    double x() const {return _data[0];}

    double y() const {return _data[1];}
};

struct Vector3 : BaseVector {
public:
    Vector3(double x, double y, double z) {
        _data.push_back(x);
        _data.push_back(y);
        _data.push_back(z);
    }

    // Read-only access when indexing the data
    const double operator[](int index) const {
        return _data[index];
    }

    double x() const {return _data[0];}

    double y() const {return _data[1];}

    double z() const {return _data[2];}
};

double magnitude(const Vector2& u);
Vector2 scale(const Vector2& u, double alpha);
Vector2 normalize(const Vector2& u);
Vector2 add(const Vector2& u, const Vector2& v);
Vector2 subtract(const Vector2& u, const Vector2& v);

double magnitude(const Vector3& u);
Vector3 scale(const Vector3& u, double alpha);
Vector3 normalize(const Vector3& u);
Vector3 add(const Vector3& u, const Vector3& v);
Vector3 subtract(const Vector3& u, const Vector3& v);


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/fmod.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
template <typename T>
constexpr void test()
{
    // Error Handling
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(std::numeric_limits<T>::quiet_NaN(), T(1))), "If x is NaN, NaN is returned");
        static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(T(1), std::numeric_limits<T>::quiet_NaN())), "If y is NaN, NaN is returned");
    }

    static_assert(boost::math::ccmath::fmod(T(0), T(1.0)) == 0);
    static_assert(boost::math::ccmath::fmod(T(-0), T(1.0)) == -0);
    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(std::numeric_limits<T>::infinity(), T(1.0))));
    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(-std::numeric_limits<T>::infinity(), T(1.0))));

    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(T(1), T(0))));
    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fmod(T(1), T(-0))));
    static_assert(boost::math::ccmath::fmod(T(1), std::numeric_limits<T>::infinity()) == T(1));
    static_assert(boost::math::ccmath::fmod(T(1), -std::numeric_limits<T>::infinity()) == T(1));

    // Functionality
    static_assert(boost::math::ccmath::fmod(T(3.0/2), T(1.0) == T(3.0/2)));
    static_assert(boost::math::ccmath::fmod(T(7.0/3), T(2.0) == T(1.0/3)));
    static_assert(boost::math::ccmath::fmod(T(-8.0/3), T(2.0) == T(-2.0/3)));

    // Correct promoted types
    if constexpr (!std::is_same_v<T, float>)
    {
        constexpr auto test_type = boost::math::ccmath::fmod(T(1), 1.0f);
        static_assert(std::is_same_v<T, std::remove_cv_t<decltype(test_type)>>);
    }
    else
    {
        constexpr auto test_type = boost::math::ccmath::fmod(1.0f, 1);
        static_assert(std::is_same_v<double, std::remove_cv_t<decltype(test_type)>>);
    }
}

int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


/**
*
* @file
*
* @brief  Mixer test
*
* @author vitamin.caig@gmail.com
*
**/

#include <error_tools.h>
#include <math/numeric.h>
#include <sound/matrix_mixer.h>
#include <sound/mixer_parameters.h>

#include <iostream>
#include <iomanip>

#include <boost/range/size.hpp>

#define FILE_TAG 25E829A2

namespace Sound
{
  //BOOST_STATIC_ASSERT(SAMPLE_MIN == 0 && SAMPLE_MID == 32768 && SAMPLE_MAX == 65535);

  const int_t THRESHOLD = 5 * (Sample::MAX - Sample::MIN) / 1000;//0.5%

  Gain CreateGain(double l, double r)
  {
    return Gain(Gain::Type(l), Gain::Type(r));
  }

  const Gain GAINS[] = {
    CreateGain(0.0, 0.0),
    CreateGain(1.0, 1.0),
    CreateGain(1.0, 0.0),
    CreateGain(0.0, 1.0),
    CreateGain(0.5, 0.5),
    CreateGain(0.1, 0.9)
  };

  const String GAIN_NAMES[] = {
    "empty", "full", "left", "right", "middle", "-10dB,-0.45dB"
  };

  const Gain INVALID_GAIN = CreateGain(2.0, 3.0);

  const Sample::Type INPUTS[] = {
    Sample::MIN,
    Sample::MID,
    Sample::MAX
  };

  const String INPUT_NAMES[] = {
    "min", "mid", "max"
  };

  const Sample OUTS[] = {
  //zero matrix
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MID, Sample::MID),
  //full matrix
     Sample(Sample::MIN, Sample::MIN),
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MAX, Sample::MAX),
  //left matrix
     Sample(Sample::MIN, Sample::MID),
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MAX, Sample::MID),
  //right matrix
     Sample(Sample::MID, Sample::MIN),
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MID, Sample::MAX),
  //mid matrix
     Sample((Sample::MID+Sample::MIN)/2, (Sample::MID+Sample::MIN)/2),
     Sample(Sample::MID, Sample::MID),
     Sample((Sample::MID+Sample::MAX)/2, (Sample::MID+Sample::MAX)/2),
  //balanced
     //left=25 right=230
     //(25*32768)/256, (230*32768)/256
     //(25*65535)/256, (230*65535)/256
     Sample(Sample::MID+(int_t(Sample::MIN)-Sample::MID)/10, Sample::MID+9*(int_t(Sample::MIN)-Sample::MID)/10),
     Sample(Sample::MID, Sample::MID),
     Sample(Sample::MID+(int_t(Sample::MAX)-Sample::MID)/10, Sample::MID+9*(int_t(Sample::MAX)-Sample::MID)/10),
  };

  template<class Res>
  typename Res::Type MakeSample(Sample::Type in)
  {
    typename Res::Type res;
    res.assign(in);
    return res;
  }

  template<unsigned Channels>
  typename FixedChannelsMatrixMixer<Channels>::Matrix MakeMatrix(const Gain& mg)
  {
    typename FixedChannelsMatrixMixer<Channels>::Matrix res;
    res.assign(mg);
    return res;
  }

  bool ShowIfError(const Error& e)
  {
    if (e)
    {
      std::cerr << e.ToString();
    }
    return e;
  }

  bool Check(Sample::Type data, Sample::Type ref)
  {
    return Math::Absolute(int_t(data) - ref) <= THRESHOLD;
  }

  void Check(const Sample& data, const Sample& ref)
  {
    if (Check(data.Left(), ref.Left()) && Check(data.Right(), ref.Right()))
    {
      std::cout << " passed\n";
    }
    else
    {
      std::cout << " failed\n";
      throw MakeFormattedError(THIS_LINE, "Value=<%1%,%2%> while expected=<%3%,%4%>",
        data.Left(), data.Right(), ref.Left(), ref.Right());
    }
  }

  template<unsigned Channels>
  void TestMixer()
  {
    std::cout << "**** Testing for " << Channels << " channels ****\n";

    const typename FixedChannelsMatrixMixer<Channels>::Ptr mixer = FixedChannelsMatrixMixer<Channels>::Create();

    std::cout << "--- Test for invalid matrix---\n";
    try
    {
      mixer->SetMatrix(MakeMatrix<Channels>(INVALID_GAIN));
      throw "Failed";
    }
    catch (const Error& e)
    {
      std::cout << " Passed\n";
      std::cerr << e.ToString();
    }
    catch (const std::string& str)
    {
      throw Error(THIS_LINE, str);
    }

    assert(boost::size(OUTS) == boost::size(GAINS) * boost::size(INPUTS));
    assert(boost::size(GAINS) == boost::size(GAIN_NAMES));
    assert(boost::size(INPUTS) == boost::size(INPUT_NAMES));

    const Sample* result(OUTS);
    for (unsigned matrix = 0; matrix != boost::size(GAINS); ++matrix)
    {
      std::cout << "--- Test for " << GAIN_NAMES[matrix] << " matrix ---\n";
      mixer->SetMatrix(MakeMatrix<Channels>(GAINS[matrix]));
      for (unsigned input = 0; input != boost::size(INPUTS); ++input, ++result)
      {
        std::cout << "Checking for " << INPUT_NAMES[input] << " input: ";
        Check(mixer->ApplyData(MakeSample<MultichannelSample<Channels> >(INPUTS[input])), *result);
      }
    }
    std::cout << "Parameters:" << std::endl;
    for (uint_t inChan = 0; inChan != Channels; ++inChan)
    {
      for (uint_t outChan = 0; outChan != Sample::CHANNELS; ++outChan)
      {
        const Parameters::NameType name = Parameters::ZXTune::Sound::Mixer::LEVEL(Channels, inChan, outChan);
        const Parameters::IntType val = Parameters::ZXTune::Sound::Mixer::LEVEL_DEFAULT(Channels, inChan, outChan);
        std::cout << name.FullPath() << ": " << val << std::endl;
      }
    }
  }
}

int main()
{
  using namespace Sound;
  try
  {
    TestMixer<1>();
    TestMixer<2>();
    TestMixer<3>();
    TestMixer<4>();
    std::cout << " Succeed!" << std::endl;
  }
  catch (const Error& e)
  {
    std::cerr << e.ToString();
    return 1;
  }
}


/** @file
 *
 *  Definition of the LikelihoodField class.
 */

#include "grid_map_lf/LikelihoodField.h"

#include <grid_map_core/GridMapMath.hpp>

#include <boost/math/distributions/normal.hpp>
#include <cmath>

namespace grid_map {

void LikelihoodField::setData(const grid_map::GridMap& grid_map, const std::string layer)
{
  data_ = grid_map[layer];
  size_ = grid_map.getSize();
  length_ = grid_map.getLength();
  position_ = grid_map.getPosition();
  resolution_ = grid_map.getResolution();
}

const Matrix& LikelihoodField::getData() const
{
  return data_;
}

void LikelihoodField::calculateLikelihoodField(const GridMap& gridMap,
                                               const std::string layer,
                                               const double sigma)
{
  if(sigma == 0)
  {
    throw std::domain_error("Standard deviation of 0 is not allowed");
  }

  boost::math::normal_distribution<double> normalDistribution(0, sigma);
  double normalization = pdf(normalDistribution, 0);

  // set properties
  resolution_ = gridMap.getResolution();
  size_ = gridMap.getSize();
  length_ = gridMap.getLength();
  position_ = gridMap.getPosition();

  // Size the Matrix and set to zero
  data_.resize(size_(0), size_(1));
  data_.setZero();

  const auto& data = gridMap[layer];

  for (std::size_t i = 0; i < size_(0); ++i)
  {
    for (std::size_t j = 0; j < size_(1); ++j)
    {
      // if unknown, set likelihood to 0.5
      if(std::isnan(data(i, j)))
      {
        data_(i, j) = 0.5;
      }
      else
      {
        data_(i, j) = pdf(normalDistribution, data(i, j))/normalization;
      }
    }
  }
}

double LikelihoodField::getLikelihoodAt(const Vector& position) const
{
  double xCenter = size_.x() / 2.0;
  double yCenter = size_.y() / 2.0;
  int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
  int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
  i = std::max(i, 0);
  i = std::min(i, size_.x() - 1);
  j = std::max(j, 0);
  j = std::min(j, size_.y() - 1);

  return data_(i, j);
}

std::ostream& operator<< (std::ostream& out, const LikelihoodField& in)
{
  out << in.data_ << std::endl;
  return out;
}

} // namespace grid_map


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/list.hpp>

#include <limits>

#include <cmath>
#include <boost/safe_float.hpp>
#include <boost/safe_float/convenience.hpp>
#include <boost/safe_float/policy/check_addition_overflow.hpp>
#include <boost/safe_float/policy/check_addition_underflow.hpp>
#include <boost/safe_float/policy/check_addition_inexact.hpp>
#include <boost/safe_float/policy/check_addition_invalid_result.hpp>

//types to be tested
using test_types=boost::mpl::list<
    float, double, long double
>;

using namespace boost::safe_float;

/**
  This test suite checks different policies on addition operations using default parameters for the other policies.
  */
BOOST_AUTO_TEST_SUITE( safe_float_addition_test_suite )

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_addition_throws_on_overflow, FPT, test_types){
    // define two FPT numbers suppose to positive overflow
    FPT a = std::numeric_limits<FPT>::max();
    FPT b = std::numeric_limits<FPT>::max();
    // check FPT overflows to inf after add
    BOOST_CHECK(std::isinf(a+b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_addition_overflow> c(std::numeric_limits<FPT>::max());
    safe_float<FPT, policy::check_addition_overflow> d(std::numeric_limits<FPT>::max());

    // check the addition throws
    BOOST_CHECK_THROW(c+d, std::exception);

    // define two FPT numbers suppose to negative overflow
    FPT e = std::numeric_limits<FPT>::lowest();
    FPT f = std::numeric_limits<FPT>::lowest();
    // check FPT overflows to inf after add
    BOOST_CHECK(std::isinf(e+f));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_addition_overflow> g(std::numeric_limits<FPT>::lowest());
    safe_float<FPT, policy::check_addition_overflow> h(std::numeric_limits<FPT>::lowest());

    // check the addition throws
    BOOST_CHECK_THROW(g+h, std::exception);

}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_addition_inexact_rounding, FPT, test_types){
    // define two FPT numbers suppose to produce an inexact rounded result
    FPT a = 1;
    FPT b = pow(2, std::numeric_limits<FPT>::digits);

    // check adding and substracting b does not obtain a again.
    BOOST_CHECK(a+b-b != a);

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_addition_inexact> c(FPT(1));
    safe_float<FPT, policy::check_addition_inexact> d((FPT)pow(2, std::numeric_limits<FPT>::digits));

    // check the addition throws
    BOOST_CHECK_THROW(c+d, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_addition_underflow, FPT, test_types){
    // define two FPT numbers suppose to underflow
    FPT a, b, c;
    if(std::is_same<FPT, float>()) {
        a =  4.01254977e-38f;
        b = -4.01254949e-38f;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the addition produces an denormal result (considered underflow)
        c = a + b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_addition_underflow> d(a);
        safe_float<FPT, policy::check_addition_underflow> e(b);

        // check the addition throws
        BOOST_CHECK_THROW(d+e, std::exception);
    } else if (std::is_same<FPT, double>()) {
        a =  2.2250738585072019e-308;
        b = -2.2250738585072014e-308;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the addition produces an denormal result (considered underflow)
        c = a + b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_addition_underflow> d(a);
        safe_float<FPT, policy::check_addition_underflow> e(b);

        // check the addition throws
        BOOST_CHECK_THROW(d+e, std::exception);
    } else if(std::is_same<FPT, long double>()) {
        a =  3.40132972460942461217e-4932l;
        b = -3.40132972460942461181e-4932l;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the addition produces an denormal result (considered underflow)
        c = a + b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_addition_underflow> d(a);
        safe_float<FPT, policy::check_addition_underflow> e(b);

        // check the addition throws
        BOOST_CHECK_THROW(d+e, std::exception);
    } else {
        BOOST_ERROR("underflow test only implemented for, float, double and long double");
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_addition_invalid_result, FPT, test_types){
    // define two FPT numbers suppose to produce a NAN
    FPT a = std::numeric_limits<FPT>::infinity();
    FPT b = -(std::numeric_limits<FPT>::infinity());

    // check adding produced NaN
    BOOST_CHECK(std::isnan(a+b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_addition_invalid_result> c(std::numeric_limits<FPT>::infinity());
    safe_float<FPT, policy::check_addition_invalid_result> d(-(std::numeric_limits<FPT>::infinity()));

    // check the addition throws
    BOOST_CHECK_THROW(c+d, std::exception);
}

BOOST_AUTO_TEST_SUITE_END()


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

static float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>

using namespace std;
using namespace boost::numeric::odeint;

#include <stdio.h>

typedef boost::array< double , 1 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    const double a = 1.2;
    dxdt[0] = -a * x[0];
}

double foobar(double t, uint64_t iters) {
    state_type x = { 1.0 }; // initial conditions

    //typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    typedef euler< state_type , typename state_type::value_type , state_type , double > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/iters );

    //printf("final result t=%f x(t)=%f, exp(-1.2* t)=%f\n", t, x[0], exp(- 1.2 * t));
    return x[0];
}

void adept_sincos(double inp, uint64_t iters);

static void enzyme_sincos(double inp, uint64_t iters) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = foobar(inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res = foobar(inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double res2;

  res2 = __enzyme_autodiff<double>(foobar, inp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), res2);
  }
}

int main(int argc, char** argv) {

  int max_iters = atoi(argv[1]) ;
  double inp = 2.1;

  unsigned i=0;
  for(int iters=max_iters/20; iters<=max_iters; iters+=max_iters/20) {
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    enzyme_sincos(inp, iters);
    i++;
    if (i == 10) break;
  }
}


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\author See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        CT const alpha1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid).azimuth;
        CT const alpha2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid).azimuth;

        return apply(lon_a1, lat_a1, alpha1, lon_b1, lat_b1, alpha2, lon, lat, spheroid);
    }

    template <typename Spheroid>
    static inline bool apply(CT const& lon1, CT const& lat1, CT const& alpha1,
                             CT const& lon2, CT const& lat2, CT const& alpha2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        // TODO - handle special cases like degenerated segments, equator, poles, etc.

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;
        CT const f = detail::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const e_sqr = f * (c2 - f);

        CT const sin_alpha1 = sin(alpha1);
        CT const sin_alpha2 = sin(alpha2);

        CT const tan_beta1 = one_minus_f * tan(lat1);
        CT const tan_beta2 = one_minus_f * tan(lat2);
        CT const beta1 = atan(tan_beta1);
        CT const beta2 = atan(tan_beta2);
        CT const cos_beta1 = cos(beta1);
        CT const cos_beta2 = cos(beta2);
        CT const sin_beta1 = sin(beta1);
        CT const sin_beta2 = sin(beta2);

        // Clairaut constants (lower-case in the paper)
        int const sign_C1 = math::abs(alpha1) <= pi_half ? 1 : -1;
        int const sign_C2 = math::abs(alpha2) <= pi_half ? 1 : -1;
        // Cj = 1 if on equator
        CT const C1 = sign_C1 * cos_beta1 * sin_alpha1;
        CT const C2 = sign_C2 * cos_beta2 * sin_alpha2;

        CT const sqrt_1_C1_sqr = math::sqrt(c1 - math::sqr(C1));
        CT const sqrt_1_C2_sqr = math::sqrt(c1 - math::sqr(C2));

        // handle special case: segments on the equator
        bool const on_equator1 = math::equals(sqrt_1_C1_sqr, c0);
        bool const on_equator2 = math::equals(sqrt_1_C2_sqr, c0);
        if (on_equator1 && on_equator2)
        {
            return false;
        }
        else if (on_equator1)
        {
            CT const dL2 = d_lambda_e_sqr(sin_beta2, c0, C2, sqrt_1_C2_sqr, e_sqr);
            CT const asin_t2_t02 = asin(C2 * tan_beta2 / sqrt_1_C2_sqr);
            lat = c0;
            lon = lon2 - asin_t2_t02 + dL2;
            return true;
        }
        else if (on_equator2)
        {
            CT const dL1 = d_lambda_e_sqr(sin_beta1, c0, C1, sqrt_1_C1_sqr, e_sqr);
            CT const asin_t1_t01 = asin(C1 * tan_beta1 / sqrt_1_C1_sqr);
            lat = c0;
            lon = lon1 - asin_t1_t01 + dL1;
            return true;
        }

        CT const t01 = sqrt_1_C1_sqr / C1;
        CT const t02 = sqrt_1_C2_sqr / C2;

        CT const asin_t1_t01 = asin(tan_beta1 / t01);
        CT const asin_t2_t02 = asin(tan_beta2 / t02);
        CT const t01_t02 = t01 * t02;
        CT const t01_t02_2 = c2 * t01_t02;
        CT const sqr_t01_sqr_t02 = math::sqr(t01) + math::sqr(t02);

        CT t = tan_beta1;
        int t_id = 0;

        // find the initial t using simplified spherical solution
        // though not entirely since the reduced latitudes and azimuths are spheroidal
        // [Sjoberg07]
        CT const k_base = lon1 - lon2 + asin_t2_t02 - asin_t1_t01;

        {
            CT const K = sin(k_base);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k_base);
            CT const D1 = math::sqrt(d1 - d2);
            CT const D2 = math::sqrt(d1 + d2);
            CT const K_t01_t02 = K * t01_t02;

            CT const T1 = K_t01_t02 / D1;
            CT const T2 = K_t01_t02 / D2;
            CT asin_T1_t01 = 0;
            CT asin_T1_t02 = 0;
            CT asin_T2_t01 = 0;
            CT asin_T2_t02 = 0;

            // test 4 possible results
            CT l1 = 0, l2 = 0, dl = 0;
            bool found = check_t<0>( T1,
                                    lon1,  asin_T1_t01 = asin(T1 / t01), asin_t1_t01,
                                    lon2,  asin_T1_t02 = asin(T1 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<1>(-T1,
                                    lon1, -asin_T1_t01                 , asin_t1_t01,
                                    lon2, -asin_T1_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<2>( T2,
                                    lon1,  asin_T2_t01 = asin(T2 / t01), asin_t1_t01,
                                    lon2,  asin_T2_t02 = asin(T2 / t02), asin_t2_t02,
                                    t, l1, l2, dl, t_id)
                      || check_t<3>(-T2,
                                    lon1, -asin_T2_t01                 , asin_t1_t01,
                                    lon2, -asin_T2_t02                 , asin_t2_t02,
                                    t, l1, l2, dl, t_id);

            boost::ignore_unused(found);
        }

        // [Sjoberg07]
        //int const d2_sign = t_id < 2 ? -1 : 1;
        int const t_sign = (t_id % 2) ? -1 : 1;
        // [Sjoberg02]
        CT const C1_sqr = math::sqr(C1);
        CT const C2_sqr = math::sqr(C2);

        CT beta = atan(t);
        CT dL1 = 0, dL2 = 0;
        CT asin_t_t01 = 0;
        CT asin_t_t02 = 0;

        for (int i = 0; i < 10; ++i)
        {
            CT const sin_beta = sin(beta);

            // integrals approximation
            dL1 = d_lambda_e_sqr(sin_beta1, sin_beta, C1, sqrt_1_C1_sqr, e_sqr);
            dL2 = d_lambda_e_sqr(sin_beta2, sin_beta, C2, sqrt_1_C2_sqr, e_sqr);

            // [Sjoberg07]
            /*CT const k = k_base + dL1 - dL2;
            CT const K = sin(k);
            CT const d1 = sqr_t01_sqr_t02;
            //CT const d2 = t01_t02_2 * math::sqrt(c1 - math::sqr(K));
            CT const d2 = t01_t02_2 * cos(k);
            CT const D = math::sqrt(d1 + d2_sign * d2);
            CT const t_new = t_sign * K * t01_t02 / D;
            CT const dt = math::abs(t_new - t);
            t = t_new;
            CT const new_beta = atan(t);
            CT const dbeta = math::abs(new_beta - beta);
            beta = new_beta;*/

            // [Sjoberg02] - it converges faster


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm accumulator mean
#include <boost/test/unit_test.hpp>
#include "fern/algorithm/accumulator/mean.h"

namespace faa = fern::algorithm::accumulator;

BOOST_AUTO_TEST_CASE(default_construct)
{
    faa::Mean<int> mean;
    // Don't calculate the mean. Division by zero.
}

BOOST_AUTO_TEST_CASE(accumulate)
{
    {
        faa::Mean<int> mean(5);
        BOOST_CHECK_EQUAL(mean(), 5);

        mean(2);
        BOOST_CHECK_EQUAL(mean(), 7 / 2u);

        mean = 3;
        BOOST_CHECK_EQUAL(mean(), 3);
    }

    {
        faa::Mean<int, double> mean(5);
        BOOST_CHECK_EQUAL(mean(), 5.0);

        mean(2);
        BOOST_CHECK_EQUAL(mean(), 3.5);

        mean = 3;
        BOOST_CHECK_EQUAL(mean(), 3.0);
    }
}

BOOST_AUTO_TEST_CASE(merge)
{
    {
        auto mean(faa::Mean<int>(5) | faa::Mean<int>(15));
        BOOST_CHECK_EQUAL(mean(), 10);
    }

    {
        auto mean(faa::Mean<int, double>(5) | faa::Mean<int, double>(20));
        BOOST_CHECK_EQUAL(mean(), 12.5);
    }
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include "gtest/gtest.h"

#include <iostream>
#include <iomanip>
#include <fstream>
#include <chrono>

#include <Eigen/Eigenvalues>

#include "teaser/registration.h"
#include "test_utils.h"

TEST(TranslationTest, TLSTranslation) {
  // Problem 1: Zero translation
  {
    std::ifstream objectFile("./data/registration_test/translation_test_v1_inliers.csv");
    auto object_points = teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(objectFile);

    // Parameters for estimating translation
    float noise_bound = 0.025; // arbitrary
    float cbar2 = 1;

    // Estimating translation
    Eigen::Vector3f actual_t;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    teaser::TLSTranslationSolver translationSolver(noise_bound, cbar2);
    // Pass the same vector of points b/c we are testing for zero translation
    translationSolver.solveForTranslation(object_points, object_points, &actual_t, &actual_inliers);

    // Expected
    Eigen::Vector3f expected_t = Eigen::Vector3f::Zero();
    EXPECT_TRUE((actual_t - expected_t).norm() < 1e-5);
  }
  // Problem 2: Translation in x / y / z only
  {
    std::ifstream objectFile("./data/registration_test/translation_test_v1_inliers.csv");
    auto object_points = teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(objectFile);

    // Parameters for estimating translation
    float noise_bound = 0.025; // arbitrary
    float cbar2 = 1;

    // Estimating translation
    // 1. in x only
    Eigen::Matrix<float, 3, Eigen::Dynamic> translated_points = object_points;
    translated_points.row(0).array() += 1;
    Eigen::Vector3f actual_t;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    teaser::TLSTranslationSolver translationSolver(noise_bound, cbar2);
    translationSolver.solveForTranslation(object_points, translated_points, &actual_t,
                                          &actual_inliers);

    // Expected
    Eigen::Vector3f expected_t;
    expected_t << 1, 0, 0;
    EXPECT_TRUE((actual_t - expected_t).norm() < 1e-5);

    // 2. in y only
    translated_points = object_points;
    translated_points.row(1).array() += 1;
    translationSolver.solveForTranslation(object_points, translated_points, &actual_t,
                                          &actual_inliers);

    // Expected
    expected_t << 0, 1, 0;
    EXPECT_TRUE((actual_t - expected_t).norm() < 1e-5);

    // 3. in z only
    translated_points = object_points;
    translated_points.row(2).array() += 1;
    translationSolver.solveForTranslation(object_points, translated_points, &actual_t,
                                          &actual_inliers);

    // Expected
    expected_t << 0, 0, 1;
    EXPECT_TRUE((actual_t - expected_t).norm() < 1e-5);
  }
  // Problem 3: An arbitrary translation
  {
    // Prepare input data
    std::ifstream objectFile("./data/registration_test/translation_test_v1_inliers.csv");
    std::ifstream sceneFile("./data/registration_test/translation_test_v2_inliers.csv");
    auto object_points = teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(objectFile);
    auto scene_points = teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(sceneFile);
    EXPECT_EQ(object_points.cols(), scene_points.cols());

    // Parameters for estimating translation
    float noise_bound = 0.00673642835;
    float cbar2 = 1;

    // Estimating translation
    Eigen::Vector3f actual_t;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> actual_inliers;
    actual_inliers.resize(1, object_points.cols());
    teaser::TLSTranslationSolver translationSolver(noise_bound, cbar2);
    translationSolver.solveForTranslation(object_points, scene_points, &actual_t, &actual_inliers);

    // Expected
    Eigen::Vector3f expected_t;
    expected_t << -0.098430131086161, 0.008679113091532, 0.197317864174211;
    EXPECT_TRUE((actual_t - expected_t).norm() < 1e-5);
  }
}


// Copyright (C) 2015 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 "theia/sfm/estimators/estimate_homography.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/four_point_homography.h"
#include "theia/sfm/pose/util.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing a homography from 4 feature correspondences. The
// feature correspondences should be normalized by the focal length with the
// principal point at (0, 0).
class HomographyEstimator
    : public Estimator<FeatureCorrespondence, Eigen::Matrix3d> {
 public:
  HomographyEstimator() {}

  // 4 correspondences are needed to determine a homography.
  double SampleSize() const { return 4; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<Eigen::Matrix3d>* homography) const {
    std::vector<Eigen::Vector2d> image1_points(4), image2_points(4);
    for (int i = 0; i < 4; i++) {
      image1_points[i] = correspondences[i].feature1;
      image2_points[i] = correspondences[i].feature2;
    }

    Eigen::Matrix3d homography_matrix;
    if (!FourPointHomography(image1_points,
                             image2_points,
                             &homography_matrix)) {
      return false;
    }

    homography->emplace_back(homography_matrix);
    return true;
  }

  // The error for a correspondences given a model. This is the asymmetric
  // distance that measures reprojection error in one image.
  double Error(const FeatureCorrespondence& correspondence,
               const Eigen::Matrix3d& homography) const {
    const Eigen::Vector3d reprojected_point =
        homography * correspondence.feature1.homogeneous();
    return (correspondence.feature2 - reprojected_point.hnormalized())
        .squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(HomographyEstimator);
};

}  // namespace

bool EstimateHomography(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& correspondences,
    Eigen::Matrix3d* homography,
    RansacSummary* ransac_summary) {
  HomographyEstimator homography_estimator;
  std::unique_ptr<SampleConsensusEstimator<HomographyEstimator> > ransac =
      CreateAndInitializeRansacVariant(ransac_type,
                                       ransac_params,
                                       homography_estimator);
  // Estimate the homography.
  return ransac->Estimate(correspondences, homography, ransac_summary);
}

}  // namespace theia


#include <vector>
#include <Eigen/Dense>
#include <algorithm>

# pragma once

using std::vector; using std::string;

typedef Eigen::MatrixXd matrix;

int atom(int ao_index, int orbitals_per_atom);

int orb_index(int ao_index, int orbitals_per_atom);

matrix calculate_fock_matrix(matrix hamiltonian_matrix, matrix interaction_matrix, matrix density_matrix, vector<string> orbitals, int orbitals_per_atom, double dipole);


#define BOOST_TEST_MODULE "test_potential_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/local/CosinePotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(CosinePotential_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type    k = 2.0;
    const std::int32_t n = 2;
    const real_type   v0 = 3.0;

    mjolnir::CosinePotential<real_type> potential(k, n, v0);

    const real_type x_min = -pi;
    const real_type x_max =  pi;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(CosinePotential_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3;
    constexpr real_type tol = 1e-3;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    const real_type    k = 2.0;
    const std::int32_t n = 2;
    const real_type   v0 = 3.0;

    mjolnir::CosinePotential<real_type> potential(k, n, v0);

    const real_type x_min = -2 * pi;
    const real_type x_max =  2 * pi;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);
}


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

void run(vector<int> & memory) {
    int pc = 0;

    while (pc < memory.size()) {
        switch (memory[pc]) {
            case 1: { // add
                int src1 = memory[pc + 1];
                int src2 = memory[pc + 2];
                int dest = memory[pc + 3];
                memory[dest] = memory[src1] + memory[src2];
                pc += 4;
                break;
            }
            case 2: {// mul
                int src1 = memory[pc + 1];
                int src2 = memory[pc + 2];
                int dest = memory[pc + 3];
                memory[dest] = memory[src1] * memory[src2];
                pc += 4;
                break;
            }
            case 99: // pass though
                pc++;
                return;
            default:
                cout << "Unhandled op code: " << memory[pc] << endl;
                return;
        };
    }

    cout << "No more instructions" << memory[pc] << endl;
}

int main() {
    ifstream file ("2019/2.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    // Read the program
    vector<int> memory;
    string line;
    while (getline(file, line, ',')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        memory.push_back(stoi(line));
    }

    const vector<int> backup_memory(memory); // make a backup

    memory[1] = 12;
    memory[2] = 2;

    //cout << memory << endl;
    run(memory);
    //cout << memory << endl;

    cout << "Answer 2.1: " << memory[0] << endl;

    // Bruteforce lazy!
    for (int noun = 0; noun < 99; noun++) {
        for (int verb = 0; verb < 99; verb++) {
            memory = backup_memory; // reset
            memory[1] = noun;
            memory[2] = verb;
            run(memory);

            if (memory[0] == 19690720) {
                cout << "Answer 2.2: " << (100 * noun + verb) << endl;
                return 0;
            }
        }
    }

    cout << "Answer 2.2: Not found" << endl;

    file.close();
}


#include "../write_code_segment.hpp"

#include "vm/processor.hpp"
#include "vm/stack.hpp"
#include "vm/exceptions.hpp"

#include <utility>
#include <cstdint>
#include <array>

#include <boost/test/unit_test.hpp>

using perseus::detail::processor;
using perseus::detail::opcode;

BOOST_AUTO_TEST_SUITE( vm )

BOOST_AUTO_TEST_SUITE( execution )

BOOST_AUTO_TEST_SUITE( boolean_arithmetic )

BOOST_AUTO_TEST_CASE( boolean_and )
{
  for( std::uint8_t a = 0; a < 2; ++a )
  {
    for( std::uint8_t b = 0; b < 2; ++b )
    {
      auto code = create_code_segment( opcode::and_b, opcode::exit );
      perseus::stack stack;
      stack.push< std::uint8_t >( a );
      stack.push< std::uint8_t >( b );
      perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
      BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), a && b );
      BOOST_CHECK_EQUAL( result.size(), 0 );
    }
  }
}

BOOST_AUTO_TEST_CASE( boolean_or )
{
  for( std::uint8_t a = 0; a < 2; ++a )
  {
    for( std::uint8_t b = 0; b < 2; ++b )
    {
      auto code = create_code_segment( opcode::or_b, opcode::exit );
      perseus::stack stack;
      stack.push< std::uint8_t >( a );
      stack.push< std::uint8_t >( b );
      perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
      BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), a || b );
      BOOST_CHECK_EQUAL( result.size(), 0 );
    }
  }
}

BOOST_AUTO_TEST_CASE( boolean_equals )
{
  for( std::uint8_t a = 0; a < 2; ++a )
  {
    for( std::uint8_t b = 0; b < 2; ++b )
    {
      auto code = create_code_segment( opcode::equals_b, opcode::exit );
      perseus::stack stack;
      stack.push< std::uint8_t >( a );
      stack.push< std::uint8_t >( b );
      perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
      BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), a == b );
      BOOST_CHECK_EQUAL( result.size(), 0 );
    }
  }
}

BOOST_AUTO_TEST_CASE( boolean_not_equals )
{
  for( std::uint8_t a = 0; a < 2; ++a )
  {
    for( std::uint8_t b = 0; b < 2; ++b )
    {
      auto code = create_code_segment( opcode::not_equals_b, opcode::exit );
      perseus::stack stack;
      stack.push< std::uint8_t >( a );
      stack.push< std::uint8_t >( b );
      perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
      BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), a != b );
      BOOST_CHECK_EQUAL( result.size(), 0 );
    }
  }
}

BOOST_AUTO_TEST_CASE( boolean_negate )
{
  for( std::uint8_t a = 0; a < 2; ++a )
  {
    auto code = create_code_segment( opcode::negate_b, opcode::exit );
    perseus::stack stack;
    stack.push< std::uint8_t >( a );
    perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
    BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), !a );
    BOOST_CHECK_EQUAL( result.size(), 0 );
  }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( int32_arithmetic )

BOOST_AUTO_TEST_CASE( int32_add )
{
  auto code = create_code_segment( opcode::add_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 4250 );
  stack.push< std::int32_t >( -8 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 4242 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_subtract )
{
  auto code = create_code_segment( opcode::subtract_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 1008 );
  stack.push< std::int32_t >( 1050 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), -42 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_multiply )
{
  auto code = create_code_segment( opcode::multiply_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 6 );
  stack.push< std::int32_t >( 7 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 42 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_divide_no_remainder )
{
  auto code = create_code_segment( opcode::divide_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 42 );
  stack.push< std::int32_t >( 7 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 6 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_divide_round_down )
{
  BOOST_TEST_MESSAGE( "48/7 is closer to 7 than 6, but integer division is supposed to round down." );
  auto code = create_code_segment( opcode::divide_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 48 );
  stack.push< std::int32_t >( 7 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 6 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_modulo )
{
  auto code = create_code_segment( opcode::modulo_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 47 );
  stack.push< std::int32_t >( 7 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 5 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_less_than_true )
{
  auto code = create_code_segment( opcode::less_than_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 1 );
  stack.push< std::int32_t >( 2 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), true );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_less_than_false )
{
  auto code = create_code_segment( opcode::less_than_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 2 );
  stack.push< std::int32_t >( 2 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), false );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_less_than_or_equals_true )
{
  auto code = create_code_segment( opcode::less_than_or_equals_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 2 );
  stack.push< std::int32_t >( 2 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), true );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_less_than_or_equals_false )
{
  auto code = create_code_segment( opcode::less_than_or_equals_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( 3 );
  stack.push< std::int32_t >( 2 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( static_cast< bool >( result.pop< std::uint8_t >() ), false );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_CASE( int32_negate )
{
  auto code = create_code_segment( opcode::negate_i32, opcode::exit );
  perseus::stack stack;
  stack.push< std::int32_t >( -42 );
  perseus::stack result = processor( std::move( code ) ).execute( 0, std::move( stack ) );
  BOOST_CHECK_EQUAL( result.pop< std::int32_t >(), 42 );
  BOOST_CHECK_EQUAL( result.size(), 0 );
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSCH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing csch capabilities

    hyperbolic cosecant: \f$1/\sinh(x)\f$.

    @par Semantic:

    For every parameter of floating type @c T

    @code
    T r = csch(x);
    @endcode

    @see rec, sinh

  **/
  Value csch(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/csch.hpp>
#include <boost/simd/function/simd/csch.hpp>

#endif


#include "MatrixStack.h"

#include <cassert>
#include <stdio.h>
#include <vector>

#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

MatrixStack::MatrixStack() {
  mstack = make_shared<stack<Matrix4f>>();
  mstack->push(Matrix4f::Identity());
}

MatrixStack::~MatrixStack() {}

void MatrixStack::pushMatrix() {
  const Matrix4f &top = mstack->top();
  mstack->push(top);
  assert(mstack->size() < 100);
}

void MatrixStack::popMatrix() {
  assert(!mstack->empty());
  mstack->pop();
  // There should always be one matrix left.
  assert(!mstack->empty());
}

void MatrixStack::loadIdentity() {
  Matrix4f &top = mstack->top();
  top = Matrix4f::Identity();
}

void MatrixStack::translate(const Vector3f &t) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E(0, 3) = t(0);
  E(1, 3) = t(1);
  E(2, 3) = t(2);
  top *= E;
}

void MatrixStack::translate(float x, float y, float z) {
  translate(Vector3f(x, y, z));
}

void MatrixStack::scale(const Vector3f &s) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E(0, 0) = s(0);
  E(1, 1) = s(1);
  E(2, 2) = s(2);
  top *= E;
}

void MatrixStack::scale(float x, float y, float z) { scale(Vector3f(x, y, z)); }

void MatrixStack::scale(float s) { scale(Vector3f(s, s, s)); }

void MatrixStack::rotate(float angle, const Vector3f &axis) {
  Matrix4f &top = mstack->top();
  Matrix4f E = Matrix4f::Identity();
  E.block<3, 3>(0, 0) =
      AngleAxisf(angle * M_PI / 180.0f, axis.normalized()).toRotationMatrix();
  top *= E;
}

void MatrixStack::rotate(float angle, float x, float y, float z) {
  rotate(angle, Vector3f(x, y, z));
}

void MatrixStack::multMatrix(const Matrix4f &matrix) {
  Matrix4f &top = mstack->top();
  top *= matrix;
}

void MatrixStack::ortho(float left, float right, float bottom, float top,
                        float zNear, float zFar) {
  assert(left != right);
  assert(bottom != top);
  assert(zFar != zNear);
  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M = Matrix4f::Zero();
  M(0, 0) = 2.0f / (right - left);
  M(1, 1) = 2.0f / (top - bottom);
  M(2, 2) = -2.0f / (zFar - zNear);
  M(0, 3) = -(right + left) / (right - left);
  M(1, 3) = -(top + bottom) / (top - bottom);
  M(2, 3) = -(zFar + zNear) / (zFar - zNear);
  M(3, 3) = 1.0f;
}

void MatrixStack::ortho2D(float left, float right, float bottom, float top) {
  ortho(left, right, bottom, top, -1.0, 1.0);
}

void MatrixStack::perspective(float fovy, float aspect, float zNear,
                              float zFar) {
  assert(fovy != 0.0f);
  assert(aspect != 0.0f);
  assert(zFar != zNear);
  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M = Matrix4f::Zero();
  float tanHalfFovy = tan(0.5f * fovy * M_PI / 180.0f);
  M(0, 0) = 1.0f / (aspect * tanHalfFovy);
  M(1, 1) = 1.0f / (tanHalfFovy);
  M(2, 2) = -(zFar + zNear) / (zFar - zNear);
  M(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
  M(3, 2) = -1.0f;
}

void MatrixStack::frustum(float left, float right, float bottom, float top,
                          float nearval, float farval) {
  // http://cgit.freedesktop.org/mesa/mesa/tree/src/mesa/math/m_matrix.c
  float x, y, a, b, c, d;
  x = (2.0f * nearval) / (right - left);
  y = (2.0f * nearval) / (top - bottom);
  a = (right + left) / (right - left);
  b = (top + bottom) / (top - bottom);
  c = -(farval + nearval) / (farval - nearval);
  d = -(2.0f * farval * nearval) / (farval - nearval);

  // Sets the top of the stack
  Matrix4f &M = mstack->top();
  M(0, 0) = x;
  M(0, 1) = 0.0f;
  M(0, 2) = a;
  M(0, 3) = 0.0f;
  M(1, 0) = 0.0f;
  M(1, 1) = y;
  M(1, 2) = b;
  M(1, 3) = 0.0f;
  M(2, 0) = 0.0f;
  M(2, 1) = 0.0f;
  M(2, 2) = c;
  M(2, 3) = d;
  M(3, 0) = 0.0f;
  M(3, 1) = 0.0f;
  M(3, 2) = -1.0f;
  M(3, 3) = 0.0f;
}

void MatrixStack::lookAt(const Vector3f &eye, const Vector3f &center,
                         const Vector3f &up) {
  // http://cgit.freedesktop.org/mesa/mesa/tree/src/glu/mesa/glu.c?h=mesa_3_2_dev
  Vector3f x, y, z;
  z = (eye - center).normalized();
  y = up;
  x = y.cross(z);
  y = z.cross(x);
  x.normalize();
  y.normalize();
  Matrix4f M = Matrix4f::Identity();
  M.block<1, 3>(0, 0) = x;
  M.block<1, 3>(1, 0) = y;
  M.block<1, 3>(2, 0) = z;
  multMatrix(M);
  translate(-eye);
}

void MatrixStack::lookAt(float ex, float ey, float ez, float tx, float ty,
                         float tz, float ux, float uy, float uz) {
  lookAt(Vector3f(ex, ey, ez), Vector3f(tx, ty, tz), Vector3f(ux, uy, uz));
}

const Matrix4f &MatrixStack::topMatrix() const { return mstack->top(); }

void MatrixStack::print(const Matrix4f &mat, const char *name) const {
  if (name) {
    printf("%s = [\n", name);
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      printf("%- 5.2f ", mat(i, j));
    }
    printf("\n");
  }
  if (name) {
    printf("];");
  }
  printf("\n");
}

void MatrixStack::print(const char *name) const { print(mstack->top(), name); }

// void MatrixStack::printStack() const
// {
// 	// Copy everything to a non-const stack
// 	auto tempStack = mstack;
// 	while(!tempStack.empty()) {
// 		Matrix4f &top = tempStack.top();
// 		print(top);
// 		tempStack.pop();
// 	}
// }

// #include <iostream>
// int main(int argc, char **argv)
// {
// 	MatrixStack M;
// 	M.frustum(-1, 1, -2, 2, 0.1, 10.0);
// 	std::cout << M.topMatrix() << std::endl;
// }


//
//  AutoFlipSVD.hpp
//  DOT
//
//  Created by Minchen Li on 6/21/18.
//

#ifndef AutoFlipSVD_hpp
#define AutoFlipSVD_hpp

#include "ImplicitQRSVD.h"

#include <Eigen/Eigen>

#include <iostream>

namespace DOT {

    template<typename MatrixType>
    class AutoFlipSVD : Eigen::JacobiSVD<MatrixType>
    {
    protected:
        bool flipped_U, flipped_V, flipped_sigma;

        typename Eigen::JacobiSVD<MatrixType>::SingularValuesType singularValues_flipped;
        MatrixType matrixU_flipped, matrixV_flipped;

    public:
        AutoFlipSVD(void) {}
        AutoFlipSVD(const MatrixType& mtr, unsigned int computationOptions = 0)
        {
            compute(mtr, computationOptions);
        }

    public:
        template<int dim = MatrixType::RowsAtCompileTime>
        typename std::enable_if<dim == 3, AutoFlipSVD<MatrixType>>::type&
        compute(const MatrixType& mtr, unsigned int computationOptions)
        {
            flipped_U = flipped_V = flipped_sigma = true;
#ifdef USE_IQRSVD
            JIXIE::singularValueDecomposition(mtr,
                                              matrixU_flipped,
                                              singularValues_flipped,
                                              matrixV_flipped);
#else
            if((computationOptions & Eigen::ComputeFullU) ||
               (computationOptions & Eigen::ComputeFullV))
            {
                fastSVD3d(mtr, matrixU_flipped, singularValues_flipped, matrixV_flipped);
            }
            else {
                fastComputeSingularValues3d(mtr, singularValues_flipped);
            }
#endif
            return *this;
        }
        template<int dim = MatrixType::RowsAtCompileTime>
        typename std::enable_if<dim == 2, AutoFlipSVD<MatrixType>>::type&
        compute(const MatrixType& mtr, unsigned int computationOptions)
        {
#ifdef USE_IQRSVD
            flipped_U = flipped_V = flipped_sigma = true;
            JIXIE::singularValueDecomposition(mtr,
                                              matrixU_flipped,
                                              singularValues_flipped,
                                              matrixV_flipped);
#else
            flipped_U = flipped_V = flipped_sigma = false;
            Eigen::JacobiSVD<MatrixType>::compute(mtr, computationOptions);
            flip2d(mtr, computationOptions);
#endif
            return *this;
        }

        void set(const Eigen::Matrix3d& U, const Eigen::Vector3d& Sigma, const Eigen::Matrix3d& V)
        {
            flipped_U = true, flipped_V = true, flipped_sigma = true;
            matrixU_flipped = U;
            singularValues_flipped = Sigma;
            matrixV_flipped = V;
        }

    protected:
        void flip2d(const MatrixType& mtr, unsigned int computationOptions) {
            //!!! this flip algorithm is only valid in 2D
            bool fullUComputed = (computationOptions & Eigen::ComputeFullU);
            bool fullVComputed = (computationOptions & Eigen::ComputeFullV);
            if(fullUComputed && fullVComputed) {
                if(Eigen::JacobiSVD<MatrixType>::m_matrixU.determinant() < 0.0) {
                    matrixU_flipped = Eigen::JacobiSVD<MatrixType>::m_matrixU;
                    matrixU_flipped.col(1) *= -1.0;
                    flipped_U = true;

                    if(!flipped_sigma) {
                        singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                    }
                    singularValues_flipped[1] *= -1.0;
                    flipped_sigma = true;
                }
                if(Eigen::JacobiSVD<MatrixType>::m_matrixV.determinant() < 0.0) {
                    matrixV_flipped = Eigen::JacobiSVD<MatrixType>::m_matrixV;
                    matrixV_flipped.col(1) *= -1.0;
                    flipped_V = true;

                    if(!flipped_sigma) {
                        singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                    }
                    singularValues_flipped[1] *= -1.0;
                    flipped_sigma = true;
                }
            }
            else if(mtr.determinant() < 0.0) {
                singularValues_flipped = Eigen::JacobiSVD<MatrixType>::m_singularValues;
                singularValues_flipped[1] *= -1.0;
                flipped_sigma = true;
            }

            if(std::isnan(singularValues()[0]) || std::isnan(singularValues()[1])) {
                // degenerated case
                singularValues_flipped.setZero();
                flipped_sigma = true;
                if(fullUComputed && fullVComputed) {
                    matrixU_flipped.setIdentity();
                    matrixV_flipped.setIdentity();
                    flipped_U = flipped_V = true;
                }
            }
        }

        //TODO: merge with IglUtils::computeCofactorMtr
        template<int dim>
        void computeCofactorMtr(const Eigen::Matrix<double, dim, dim>& F,
                                Eigen::Matrix<double, dim, dim>& A)
        {
            switch(dim) {
                case 2:
                    A(0, 0) = F(1, 1);
                    A(0, 1) = -F(1, 0);
                    A(1, 0) = -F(0, 1);
                    A(1, 1) = F(0, 0);
                    break;

                case 3:
                    A(0, 0) = F(1, 1) * F(2, 2) - F(1, 2) * F(2, 1);
                    A(0, 1) = F(1, 2) * F(2, 0) - F(1, 0) * F(2, 2);
                    A(0, 2) = F(1, 0) * F(2, 1) - F(1, 1) * F(2, 0);
                    A(1, 0) = F(0, 2) * F(2, 1) - F(0, 1) * F(2, 2);
                    A(1, 1) = F(0, 0) * F(2, 2) - F(0, 2) * F(2, 0);
                    A(1, 2) = F(0, 1) * F(2, 0) - F(0, 0) * F(2, 1);
                    A(2, 0) = F(0, 1) * F(1, 2) - F(0, 2) * F(1, 1);
                    A(2, 1) = F(0, 2) * F(1, 0) - F(0, 0) * F(1, 2);
                    A(2, 2) = F(0, 0) * F(1, 1) - F(0, 1) * F(1, 0);
                    break;

                default:
                    assert(0 && "dim not 2 or 3");
                    break;
            }
        }
        void fastEigenvalues(const Eigen::Matrix3d& A_Sym,
                             Eigen::Vector3d& lambda)
        // 24 mults, 20 adds, 1 atan2, 1 sincos, 2 sqrts
        {
            using T = double;
            using std::max;
            using std::swap;
            T m = ((T)1 / 3) * (A_Sym(0, 0) + A_Sym(1, 1) + A_Sym(2, 2));
            T a00 = A_Sym(0, 0) - m;
            T a11 = A_Sym(1, 1) - m;
            T a22 = A_Sym(2, 2) - m;
            T a12_sqr = A_Sym(0, 1) * A_Sym(0, 1);
            T a13_sqr = A_Sym(0, 2) * A_Sym(0, 2);
            T a23_sqr = A_Sym(1, 2) * A_Sym(1, 2);
            T p = ((T)1 / 6) * (a00 * a00 + a11 * a11 + a22 * a22 + 2 * (a12_sqr + a13_sqr + a23_sqr));
            T q = (T).5 * (a00 * (a11 * a22 - a23_sqr) - a11 * a13_sqr - a22 * a12_sqr) + A_Sym(0, 1) * A_Sym(0, 2) * A_Sym(1, 2);
            T sqrt_p = sqrt(p);
            T disc = p * p * p - q * q;
            T phi = ((T)1 / 3) * atan2(sqrt(max((T)0, disc)), q);
            T c = cos(phi), s = sin(phi);
            T sqrt_p_cos = sqrt_p * c;
            T root_three_sqrt_p_sin = sqrt((T)3) * sqrt_p * s;
            lambda(0) = m + 2 * sqrt_p_cos;
            lambda(1) = m - sqrt_p_cos - root_three_sqrt_p_sin;
            lambda(2) = m - sqrt_p_cos + root_three_sqrt_p_sin;
            if (lambda(0) < lambda(1))
                swap(lambda(0), lambda(1));
            if (lambda(1) < lambda(2))
                swap(lambda(1), lambda(2));
            if (lambda(0) < lambda(1))
                swap(lambda(0), lambda(1));
        }
        void fastEigenvectors(const Eigen::Matrix3d& A_Sym,
                              const Eigen::Vector3d& lambda,
                              Eigen::Matrix3d& V)
        // 71 mults, 44 adds, 3 divs, 3 sqrts
        {
            // flip if necessary so that first eigenvalue is the most different
            using T = double;
            using std::sqrt;
            using std::swap;
            bool flipped = false;
            Eigen::Vector3d lambda_flip(lambda);
            if (lambda(0) - lambda(1) < lambda(1) - lambda(2)) { // 2a
                swap(lambda_flip(0), lambda_flip(2));
                flipped = true;
            }

            // get first eigenvector
            Eigen::Matrix3d C1;
            computeCofactorMtr<3>(A_Sym - lambda_flip(0) * Eigen::Matrix3d::Identity(), C1);
            Eigen::Matrix3d::Index i;
            T norm2 = C1.colwise().squaredNorm().maxCoeff(&i); // 3a + 12m+6a + 9m+6a+1d+1s = 21m+15a+1d+1s
            Eigen::Vector3d v1;
            if (norm2 != 0) {
                T one_over_sqrt = (T)1 / sqrt(norm2);
                v1 = C1.col(i) * one_over_sqrt;
            }
            else
                v1 << 1, 0, 0;

            // form basis for orthogonal complement to v1, and reduce A to this space
            Eigen::Vector3d v1_orthogonal = v1.unitOrthogonal(); // 6m+2a+1d+1s (tweak: 5m+1a+1d+1s)
            Eigen::Matrix<T, 3, 2> other_v;
            other_v.col(0) = v1_orthogonal;
            other_v.col(1) = v1.cross(v1_orthogonal); // 6m+3a (tweak: 4m+1a)
            Eigen::Matrix2d A_reduced = other_v.transpose() * A_Sym * other_v; // 21m+12a (tweak: 18m+9a)

            // find third eigenvector from A_reduced, and fill in second via cross product
            Eigen::Matrix2d C3;
            computeCofactorMtr<2>(A_reduced - lambda_flip(2) * Eigen::Matrix2d::Identity(), C3);
            Eigen::Matrix2d::Index j;
            norm2 = C3.colwise().squaredNorm().maxCoeff(&j); // 3a + 12m+6a + 9m+6a+1d+1s = 21m+15a+1d+1s
            Eigen::Vector3d v3;
            if (norm2 != 0) {
                T one_over_sqrt = (T)1 / sqrt(norm2);
                v3 = other_v * C3.col(j) * one_over_sqrt;
            }
            else
                v3 = other_v.col(0);

            Eigen::Vector3d v2 = v3.cross(v1); // 6m+3a

            // finish
            if (flipped) {
                V.col(0) = v3;
                V.col(1) = v2;
                V.col(2) = -v1;
            }
            else {
                V.col(0) = v1;
                V.col(1) = v2;
                V.col(2) = v3;
            }
        }
        void fastSolveEigenproblem(const Eigen::Matrix3d& A_Sym,
                                   Eigen::Vector3d& lambda,
                                   Eigen::Matrix3d& V)
        // 71 mults, 44 adds, 3 divs, 3 sqrts
        {
            fastEigenvalues(A_Sym, lambda);
            fastEigenvectors(A_Sym, lambda, V);
        }

        void fastSVD3d(const Eigen::Matrix3d& A,
                       Eigen::Matrix3d& U,
                       Eigen::Vector3d& singular_values,
                       Eigen::Matrix3d& V)
        // 182 mults, 112 adds, 6 divs, 11 sqrts, 1 atan2, 1 sincos
        {
            using T = double;
            // decompose normal equations
            Eigen::Vector3d lambda;
            fastSolveEigenproblem(A.transpose() * A, lambda, V);

            // compute singular values
            if (lambda(2) < 0)
                lambda = (lambda.array() >= (T)0).select(lambda, (T)0);
            singular_values = lambda.array().sqrt();
            if (A.determinant() < 0)
                singular_values(2) = -singular_values(2);

            // compute singular vectors
            U.col(0) = A * V.col(0);
            T norm = U.col(0).norm();
            if (norm != 0) {
                T one_over_norm = (T)1 / norm;
                U.col(0) = U.col(0) * one_over_norm;
            }
            else
                U.col(0) << 1, 0, 0;
            Eigen::Vector3d v1_orthogonal = U.col(0).unitOrthogonal();
            Eigen::Matrix<T, 3, 2> other_v;
            other_v.col(0) = v1_orthogonal;
            other_v.col(1) = U.col(0).cross(v1_orthogonal);
            Eigen::Vector2d w = other_v.transpose() * A * V.col(1);
            norm = w.norm();
            if (norm != 0) {
                T one_over_norm = (T)1 / norm;
                w = w * one_over_norm;
            }
            else
                w << 1, 0;
            U.col(1) = other_v * w;
            U.col(2) = U.col(0).cross(U.col(1));
        }

        void fastComputeSingularValues3d(const Eigen::Matrix3d& A,
                                         Eigen::Vector3d& singular_values)
        {
            using T = double;
            // decompose normal equations
            Eigen::Vector3d lambda;
            fastEigenvalues(A.transpose() * A, lambda);

            // compute singular values
            if (lambda(2) < 0)
                lambda = (lambda.array() >= (T)0).select(lambda, (T)0);
            singular_values = lambda.array().sqrt();
            if (A.determinant() < 0)
                singular_values(2) = -singular_values(2);
        }

    public:
        const typename Eigen::JacobiSVD<MatrixType>::SingularValuesType& singularValues(void) const {
            if(flipped_sigma) {
                return singularValues_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::singularValues();
            }
        }
        const MatrixType& matrixU(void) const {
            if(flipped_U) {
                return matrixU_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::matrixU();
            }
        }
        const MatrixType& matrixV(void) const {
            if(flipped_V) {
                return matrixV_flipped;
            }
            else {
                return Eigen::JacobiSVD<MatrixType>::matrixV();
            }
        }

        void setIdentity(void) {
            flipped_sigma=true;
            flipped_V=true;
            flipped_U=true;

            matrixU_flipped.setIdentity();
            matrixV_flipped.setIdentity();
            singularValues_flipped.setOnes();
        }
    };

}

#endif /* AutoFlipSVD_hpp */