#ifndef MPI_IMPL_DISTRIBUTED_DOT_HPP
#define MPI_IMPL_DISTRIBUTED_DOT_HPP

#include <unordered_map>

#include <boost/mpi.hpp>
#include <armadillo>

#include "types.hpp"
#include "get_edge_values.hpp"
#include "mpi_impl/Mesh.hpp"

namespace schro_mpi
{
    template <std::floating_point real>
    real dot(const Mesh<real>& mesh, const SparseData<matrix<real>>& a, const SparseData<matrix<real>>& b)
    {
        int n = mesh.N;

        // compute dot for interior nodes
        real p_interiors = 0;

        auto interior = arma::span(1, n-2);
        for (const auto& [el, u] : a)
        {
            const auto& v = b.at(el);
            p_interiors += arma::dot(u.submat(interior, interior), v.submat(interior, interior));
        }

        // compute dot along edges
        real p_edges = 0;

        for (const auto& [id, edge] : mesh.edges)
        {
            const int e = edge.elements[0];
            if (mesh.elements.contains(e)) {
                const int s = std::abs(edge.element_sides[0]);
                const auto& u = a.at(e);
                const auto& v = b.at(e);

                real pe = 0;
                if (s == 2 or s == 4) {
                    int i = mesh.smap(s);
                    for (int j=1; j < mesh.N-1; ++j)
                        pe += u.at(i,j) * v.at(i,j);
                } else {
                    int j = mesh.smap(s);
                    for (int i=1; i < mesh.N-1; ++i)
                        pe += u.at(i,j) * v.at(i,j);
                }

                p_edges += pe;
            }
        }

        // compute dot on corners
        real p_corners = 0;

        for (const auto& [id, node] : mesh.nodes)
        {
            const auto& info = node.connected_elements[0];
            if (mesh.elements.contains(info.element_id))
                p_corners += a.at(info.element_id).at(info.i, info.j) * b.at(info.element_id).at(info.i, info.j);
        }

        real p = p_interiors + p_edges + p_corners;

        p = mpi::all_reduce(mesh.comm, p, std::plus<real>{});

        return p;
    }

} // namespace schro_mpi

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <range/v3/all.hpp>
struct Jio
{
};
struct Jie
{
};
struct Inc
{
};
struct Tpl
{
};
struct Hlf
{
};
struct Jmp
{
};
struct Machine
{
  Machine()
  {
    m["packages_"] = 0;
    m["b"] = 0;
  }
  int pos_;
  int minpos_ = 0;
  int maxpos_ = 48;
  std::unordered_map<std::string, int> m;
  std::vector<std::string> ops_;
  void Jio(const std::string &operand, int value)
  {
    int n = m[operand];
    if (n == 1) {
      pos_ += value;
    } else {
      ++pos_;
    }
  }
  void Jie(const std::string &operand, int value)
  {
    int n = m[operand] % 2;
    if (n == 0) {
      pos_ += value;
    } else {
      ++pos_;
    }
  }

  void Tpl(const std::string &operand)
  {
    m[operand] *= 3;
    ++pos_;
  }

  void Hlf(const std::string &operand)
  {
    m[operand] /= 2;
    ++pos_;
  }
  void Inc(const std::string &operand)
  {
    m[operand] += 1;
    ++pos_;
  }
  void Jmp(int value)
  {
    pos_ += value;
  }
  bool valid() const
  {
    return (minpos_ <= pos_) && (pos_ < maxpos_);
  }
  void execute()
  {
    while (valid()) {
      std::istringstream iss(ops_[pos_]);
      std::string s;
      std::string op, opand;
      iss >> op;
      int opint;
      if (op == "hlf") {
        iss >> opand;
        Hlf(opand);
      } else if (op == "tpl") {
        iss >> opand;
        Tpl(opand);
      } else if (op == "inc") {
        iss >> opand;
        Inc(opand);
      } else if (op == "jmp") {
        iss >> opint;
        Jmp(opint);
      } else if (op == "jie") {
        iss >> opand >> opint;
        Jie(opand, opint);
      } else if (op == "jio") {
        iss >> opand >> opint;
        Jio(opand, opint);
      }
    }
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    Machine m;
    //Jio jio;
    //Jie jie;
    //Jmp jmp;
    //Inc inc;
    //Hlf hlf;
    //Tpl tpl;
    std::string s;

    for (; std::getline(ifs, s);) {
      m.ops_.push_back(s);
    }
    m.execute();
    fmt::print("{}\n", m.m["b"]);
  }
}


#include <boost/graph/rmat_graph_generator.hpp>


#include <ros/ros.h>
#include <iostream>
#include <tf/transform_listener.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_srvs/Empty.h>
#include <Eigen/Eigen>
#include <geometry_msgs/Twist.h>
#include <crazyflie_controller/Mocap.h>

#include "pid.hpp"

double get(
    const ros::NodeHandle& n,
    const std::string& name) {
    double value;
    n.getParam(name, value);
    return value;
}

class Controller

{

public:

    Controller(
        // const std::string& worldFrame,
        // const std::string& frame,
        const ros::NodeHandle& n)
        : m_pubNav()
        , m_pubCmdVtemp()
        , m_pidX(
            get(n, "PIDs/X/kp"),
            get(n, "PIDs/X/kd"),
            get(n, "PIDs/X/ki"),
            get(n, "PIDs/X/minOutput"),
            get(n, "PIDs/X/maxOutput"),
            get(n, "PIDs/X/integratorMin"),
            get(n, "PIDs/X/integratorMax"),
            "x")
        , m_pidY(
            get(n, "PIDs/Y/kp"),
            get(n, "PIDs/Y/kd"),
            get(n, "PIDs/Y/ki"),
            get(n, "PIDs/Y/minOutput"),
            get(n, "PIDs/Y/maxOutput"),
            get(n, "PIDs/Y/integratorMin"),
            get(n, "PIDs/Y/integratorMax"),
            "y")
        , m_pidZ(
            get(n, "PIDs/Z/kp"),
            get(n, "PIDs/Z/kd"),
            get(n, "PIDs/Z/ki"),
            get(n, "PIDs/Z/minOutput"),
            get(n, "PIDs/Z/maxOutput"),
            get(n, "PIDs/Z/integratorMin"),
            get(n, "PIDs/Z/integratorMax"),
            "z")
        , m_pidVx(
            get(n, "PIDs/Vx/kp"),
            get(n, "PIDs/Vx/kd"),
            get(n, "PIDs/Vx/ki"),
            get(n, "PIDs/Vx/minOutput"),
            get(n, "PIDs/Vx/maxOutput"),
            get(n, "PIDs/Vx/integratorMin"),
            get(n, "PIDs/Vx/integratorMax"),
            "x")
        , m_pidVy(
            get(n, "PIDs/Vy/kp"),
            get(n, "PIDs/Vy/kd"),
            get(n, "PIDs/Vy/ki"),
            get(n, "PIDs/Vy/minOutput"),
            get(n, "PIDs/Vy/maxOutput"),
            get(n, "PIDs/Vy/integratorMin"),
            get(n, "PIDs/Vy/integratorMax"),
            "x")
        , m_pidVz(
            get(n, "PIDs/Vz/kp"),
            get(n, "PIDs/Vz/kd"),
            get(n, "PIDs/Vz/ki"),
            get(n, "PIDs/Vz/minOutput"),
            get(n, "PIDs/Vz/maxOutput"),
            get(n, "PIDs/Vz/integratorMin"),
            get(n, "PIDs/Vz/integratorMax"),
            "x")
        , m_pidYaw(
            get(n, "PIDs/Yaw/kp"),
            get(n, "PIDs/Yaw/kd"),
            get(n, "PIDs/Yaw/ki"),
            get(n, "PIDs/Yaw/minOutput"),
            get(n, "PIDs/Yaw/maxOutput"),
            get(n, "PIDs/Yaw/integratorMin"),
            get(n, "PIDs/Yaw/integratorMax"),
            "yaw")
        , m_state(Idle)
        , m_goal()
        , m_cmdV()
        , m_dronePositionWorld()
        , m_droneVelocityWorld()
        , m_droneEuler()
        , m_pqrt()
        , m_Cbe()
        , m_subscribeGoal()
        , m_subscribeCmdV()
        , m_subscribeDroneState()
        , m_subscribePQRT()
        , m_serviceTakeoff()
        , m_serviceLand()
        , m_serviceGame()
        , m_serviceAuto()
        , m_serviceIdentRoll()
        , m_serviceIdentPitch()
        , m_serviceIdentYaw()
        , m_serviceIdentThrust()
        , m_thrust(0)
        , m_startZ(0)
        , m_trimThrust(43000)
    {
        ros::NodeHandle nh;
        m_pubNav = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
        m_pubCmdVtemp = nh.advertise<geometry_msgs::TwistStamped>("cmdVtemp", 1);

        m_subscribeGoal = nh.subscribe("goal", 1, &Controller::goalChanged, this);
        m_subscribeCmdV = nh.subscribe("cmdV", 1, &Controller::cmdVChanged, this);
        m_subscribeDroneState = nh.subscribe("mocap", 1, &Controller::droneMoved, this);
        m_subscribePQRT = nh.subscribe("pqrt", 1, &Controller::pqrtChanged, this);

        m_serviceTakeoff = nh.advertiseService("cftakeoff", &Controller::takeoff, this);
        m_serviceAuto = nh.advertiseService("cfauto", &Controller::automatic, this);
        m_serviceGame = nh.advertiseService("cfplay", &Controller::play, this);
        m_serviceLand = nh.advertiseService("cfland", &Controller::land, this);

        m_serviceIdentRoll   = nh.advertiseService("cmdroll",   &Controller::cmdroll, this);
        m_serviceIdentPitch  = nh.advertiseService("cmdpitch",  &Controller::cmdpitch, this);
        m_serviceIdentYaw    = nh.advertiseService("cmdyaw",    &Controller::cmdyaw, this);
        m_serviceIdentThrust = nh.advertiseService("cmdthrust", &Controller::cmdthrust, this);
    }

    void run(double frequency)
    {
        ros::NodeHandle node;
        ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this);
        ros::spin();
    }

private:
    void goalChanged(
        const geometry_msgs::PoseStamped& msg)
    {
        m_goal[0] = msg.pose.position.x;
        m_goal[1] = msg.pose.position.y;
        m_goal[2] = msg.pose.position.z;
    }

    void cmdVChanged(
        const geometry_msgs::Twist& msg)
    {
        m_cmdV[0] = msg.linear.x;
        m_cmdV[1] = msg.linear.y;
        m_cmdV[2] = msg.linear.z;
    }

    void droneMoved(
        const crazyflie_controller::Mocap& msg)
    {
        m_dronePositionWorld[0] = msg.position[0];
        m_dronePositionWorld[1] = msg.position[1];
        m_dronePositionWorld[2] = msg.position[2];
        m_droneVelocityWorld[0] = msg.velocity[0];
        m_droneVelocityWorld[1] = msg.velocity[1];
        m_droneVelocityWorld[2] = msg.velocity[2];
        m_droneEuler = quaternion_to_euler_w(msg);
        m_Cbe = quaternion_to_Cbe(msg);
    }

    void pqrtChanged(
        const geometry_msgs::Twist& msg)
    {
        m_pqrt = msg;
    }

    bool takeoff(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Takeoff requested!");
        m_state = TakingOff;
        m_startZ = m_dronePositionWorld[2];
        return true;
    }

    bool land(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Landing requested!");
        m_state = Landing;
        return true;
    }

    bool play(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("playing requested!");
        m_state = Playing;
        return true;
    }

    bool automatic(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("automatic requested!");
        m_state = Automatic;
        return true;
    }

    bool cmdroll(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {

        ROS_INFO("roll model identification");
        m_state = IdentRoll;
        return true;
    }

    bool cmdpitch(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {

        ROS_INFO("pitch model identification");
        m_state = IdentPitch;
        return true;
    }

    bool cmdyaw(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {

        ROS_INFO("yaw model identification");
        m_state = IdentYaw;
        return true;
    }

    bool cmdthrust(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {

        ROS_INFO("thrust model identification");
        m_state = IdentThrust;
        return true;
    }

    void pidXYZReset()
    {
        m_pidX.reset();
        m_pidY.reset();
        m_pidZ.reset();
        m_pidYaw.reset();
    }

    void pidVReset()
    {
        m_pidVx.reset();
        m_pidVy.reset();
        m_pidVz.reset();
    }

    void pidReset()
    {
        pidXYZReset();
        pidVReset();
    }

    Eigen::Vector3d quaternion_to_euler_w(const crazyflie_controller::Mocap& mocap)
    {
       float quat[4];
       quat[0] = mocap.quaternion[0];
       quat[1] = mocap.quaternion[1];
       quat[2] = mocap.quaternion[2];
       quat[3] = mocap.quaternion[3];
       Eigen::Vector3d ans;
       ans[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]));
       ans[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1]));
       ans[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]));
       return ans;
    }

    Eigen::Matrix3d quaternion_to_Cbe(const crazyflie_controller::Mocap& mocap)
    {
        Eigen::Matrix3d m_Cbe;
        Eigen::Matrix3d Ceb;
        Eigen::Matrix<double, 3, 4> L;// Quaternion auxiliary matirx
        Eigen::Matrix<double, 3, 4> R;// Quaternion auxiliary matirx
        double q0, q1, q2, q3;
        q0 = mocap.quaternion[0];
        q1 = mocap.quaternion[1];
        q2 = mocap.quaternion[2];
        q3 = mocap.quaternion[3];
         // update the auxiliary matrix
        /*
        L = [-q1 q0 q3 -q2;
             -q2 -q3 q0 q1;
             -q3 q2 -q1 q0]
        R = [-q1 q0 -q3 q2;
             -q2 q3 q0 -q1;
             -q3 -q2 q1 q0]
        R_IB = RL^T
        */
        L(0,0) = - q1;
        L(1,0) = - q2;
        L(2,0) = - q3;

        L(0,1) = q0;
        L(1,2) = q0;
        L(2,3) = q0;

        L(0,2) = q3;
        L(0,3) = - q2;
        L(1,1) = - q3;
        L(1,3) = q1;
        L(2,1) = q2;
        L(2,2) = - q1;

        R(0,0) = - q1;
        R(1,0) = - q2;
        R(2,0) = - q3;

        R(0,1) = q0;
        R(1,2) = q0;
        R(2,3) = q0;

        R(0,2) = - q3;
        R(0,3) =  q2;
        R(1,1) =  q3;
        R(1,3) = - q1;
        R(2,1) = - q2;
        R(2,2) =  q1;

        Ceb = R * L.transpose();
        m_Cbe = Ceb.transpose();

        return m_Cbe;
    }

    void iteration(const ros::TimerEvent& e)
    {

        float dt = e.current_real.toSec() - e.last_real.toSec();
        switch(m_state)
        {
        case TakingOff:
            {
                if (m_dronePositionWorld[2] > m_startZ + 0.05 || m_thrust > m_trimThrust)
                {
                    pidReset();
                    m_pidZ.setIntegral((m_thrust-m_trimThrust)/m_pidZ.ki());
                    m_state = Automatic;
                    m_thrust = m_trimThrust;
                    ROS_INFO("Automatic!");
                }
                else
                {
                    m_thrust += 12000 * dt;
                    geometry_msgs::Twist msg;
                    msg.linear.z = m_thrust;
                    m_pubNav.publish(msg);
                }

            }
            break;
        case Landing:
            {
                 m_goal[0] = m_dronePositionWorld[0];
                 m_goal[1] = m_dronePositionWorld[1];
                 // m_goal[2] = m_startZ + 0.05;
                 m_goal[2] = 0.1;
                // if (m_dronePositionWorld[2] <= m_startZ + 0.1) {
                if (m_dronePositionWorld[2] <= 0.15) {
                    m_state = Idle;
                    geometry_msgs::Twist msg;
                    m_pubNav.publish(msg);
                }
            }

            // intentional fall-thru
        case Automatic:
            {
//                std::cout<<"automatic";
//                pidVReset();
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                // std::cout<<"cmdVz, dz\t"<<m_goal[2]<<"\t\t"<<positionErr[2]<<std::endl;

                Eigen::Vector3d cmdV;
                cmdV[0] = m_pidX.update(0.0, positionErr[0]);
                cmdV[1] = m_pidY.update(0.0, positionErr[1]);
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);
                // std::cout<<"cmdVz, dx\t"<<m_goal[0]*100<<"\t\t"<<m_dronePositionWorld[0]*100<<std::endl;
                // std::cout<<"cmdVz, dy\t"<<m_goal[1]*100<<"\t\t"<<m_dronePositionWorld[1]*100<<std::endl;
                // std::cout<<"cmdVz, dz\t"<<m_goal[2]*100<<"\t\t"<<m_dronePositionWorld[2]*100<<std::endl;

                geometry_msgs::TwistStamped cmdVtemp;
                cmdVtemp.header.stamp = ros::Time::now();
                cmdVtemp.twist.linear.x = cmdV[0];
                cmdVtemp.twist.linear.y = cmdV[1];
                cmdVtemp.twist.linear.z = cmdV[2];
//                ROS_INFO("publish intermeidate velocity cmd");
                m_pubCmdVtemp.publish(cmdVtemp);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pidVx.update(0, velocityErrBody[0]);
                msg.linear.y = m_pidVy.update(0, velocityErrBody[1]);
                msg.linear.z =  m_pidVz.update(0, velocityErrBody[2]) + m_trimThrust;
                msg.angular.z = m_pidYaw.update(0.0, m_droneEuler[2]);
                m_pubNav.publish(msg);
            }
            break;

        case Playing:
            {
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                Eigen::Vector3d cmdV;
                cmdV[0] = m_cmdV[0];
                cmdV[1] = m_cmdV[1];
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);

                geometry_msgs::TwistStamped cmdVtemp;
                cmdVtemp.header.stamp = ros::Time::now();
                cmdVtemp.twist.linear.x = cmdV[0];
                cmdVtemp.twist.linear.y = cmdV[1];
                cmdVtemp.twist.linear.z = cmdV[2];
//                ROS_INFO("publish intermeidate velocity cmd");
                m_pubCmdVtemp.publish(cmdVtemp);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pidVx.update(0, velocityErrBody[0]);
                msg.linear.y = m_pidVy.update(0, velocityErrBody[1]);
                msg.linear.z = m_pidVz.update(0, velocityErrBody[2]) + m_trimThrust;
                msg.angular.z = m_pidYaw.update(0.0, m_droneEuler[2]);
                m_pubNav.publish(msg);
            }
            break;

        case IdentRoll:
            {
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                Eigen::Vector3d cmdV;
                cmdV[0] = m_pidX.update(0.0, positionErr[0]);
                cmdV[1] = m_pidY.update(0.0, positionErr[1]);
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pidVx.update(0, velocityErrBody[0]);
                msg.linear.y = m_pqrt.linear.y;
                msg.linear.z = m_pidVz.update(0, velocityErrBody[2]) + m_trimThrust;
                msg.angular.z = m_pidYaw.update(0.0, m_droneEuler[2]);
                m_pubNav.publish(msg);
            }
            break;

        case IdentPitch:
            {
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                Eigen::Vector3d cmdV;
                cmdV[0] = m_pidX.update(0.0, positionErr[0]);
                cmdV[1] = m_pidY.update(0.0, positionErr[1]);
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pqrt.linear.x;
                msg.linear.y = m_pidVy.update(0, velocityErrBody[1]);
                msg.linear.z = m_pidVz.update(0, velocityErrBody[2]) + m_trimThrust;
                msg.angular.z = m_pidYaw.update(0.0, m_droneEuler[2]);
                m_pubNav.publish(msg);
            }
            break;

        case IdentYaw:
            {
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                Eigen::Vector3d cmdV;
                cmdV[0] = m_pidX.update(0.0, positionErr[0]);
                cmdV[1] = m_pidY.update(0.0, positionErr[1]);
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pidVx.update(0, velocityErrBody[0]);
                msg.linear.y = m_pidVy.update(0, velocityErrBody[1]);
                msg.linear.z =  m_pidVz.update(0, velocityErrBody[2]) + m_trimThrust;
                msg.angular.z = m_pqrt.angular.z;
                m_pubNav.publish(msg);
            }
            break;

        case IdentThrust:
            {
                Eigen::Vector3d positionErr = m_goal - m_dronePositionWorld;
                Eigen::Vector3d cmdV;
                cmdV[0] = m_pidX.update(0.0, positionErr[0]);
                cmdV[1] = m_pidY.update(0.0, positionErr[1]);
                cmdV[2] = m_pidZ.update(0.0, positionErr[2]);

                Eigen::Vector3d velocityErrBody = m_Cbe*(cmdV - m_droneVelocityWorld);
                geometry_msgs::Twist msg;
                msg.linear.x = m_pidVx.update(0, velocityErrBody[0]);
                msg.linear.y = m_pidVy.update(0, velocityErrBody[1]);
                msg.linear.z =  m_pqrt.linear.z + m_trimThrust;
                msg.angular.z = m_pidYaw.update(0.0, m_droneEuler[2]);
                m_pubNav.publish(msg);
            }
            break;

        case Idle:
            {
                geometry_msgs::Twist msg;
                m_pubNav.publish(msg);
            }
            break;
        }
    }

private:
    enum State
    {
        Idle = 0,
        Automatic = 1,
        TakingOff = 2,
        Landing = 3,
        Playing = 4,
        IdentRoll = 5,
        IdentPitch = 6,
        IdentYaw = 7,
        IdentThrust = 8,
    };

private:
    ros::Publisher m_pubNav;
    ros::Publisher m_pubCmdVtemp;
    PID m_pidX;
    PID m_pidY;
    PID m_pidZ;
    PID m_pidVx;
    PID m_pidVy;
    PID m_pidVz;
    PID m_pidYaw;
    State m_state;
    Eigen::Vector3d m_goal;
    Eigen::Vector3d m_cmdV;
    Eigen::Vector3d m_dronePositionWorld;
    Eigen::Vector3d m_droneVelocityWorld;
    Eigen::Vector3d m_droneEuler;
    Eigen::Matrix3d m_Cbe;
    geometry_msgs::Twist m_pqrt;
    ros::Subscriber m_subscribeGoal;
    ros::Subscriber m_subscribeCmdV;
    ros::Subscriber m_subscribeDroneState;
    ros::Subscriber m_subscribePQRT;
    ros::ServiceServer m_serviceTakeoff;
    ros::ServiceServer m_serviceLand;
    ros::ServiceServer m_serviceGame;
    ros::ServiceServer m_serviceAuto;
    ros::ServiceServer m_serviceIdentRoll;
    ros::ServiceServer m_serviceIdentPitch;
    ros::ServiceServer m_serviceIdentYaw;
    ros::ServiceServer m_serviceIdentThrust;
    float m_thrust;
    float m_trimThrust;
    float m_startZ;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "controller");

  // Read parameters
  ros::NodeHandle n("~");
  // std::string worldFrame;
  // n.param<std::string>("worldFrame", worldFrame, "/world");
  // std::string frame;
  // n.getParam("frame", frame);
  double frequency;
  n.param("frequency", frequency, 50.0);

  // Controller controller(worldFrame, frame, n);
  Controller controller(n);
  controller.run(frequency);

  return 0;

}


#include <iostream>
#include <stack>
#include <queue>

#include <boost/graph/adjacency_list.hpp>

#include "dfs.hpp"
#include "fifodfs.hpp"
#include "rdfs.hpp"

#include "options.hpp"
#include "debug.hpp"
#include "test_suite.hpp"
#include "range.hpp"

using namespace std;

boost::property<boost::edge_color_t, boost::default_color_type> typedef color;
boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS,
    boost::no_property, color> typedef graph;

typedef pair<int, int> node_pair;

template <class Graph>
Graph dfs(Graph const & G, int v) {
  Graph T;
  int parent;
  stack<node_pair> S;
  vector<bool> V(num_vertices(G));
  S.emplace(-1, v);
  while(!S.empty()) {
    tie(parent, v) = S.top();
    S.pop();
    if(!V[v]) {
      V[v] = true;
      if(parent > -1) add_edge(parent, v, T);
      for(auto w : shuffled(adjacent_vertices(v, G))) {
        S.emplace(v, w);
      }
    }
  }
  show("tree-dfs.dot", G, T);
  return T;
}

template <class Graph>
Graph dfs2(Graph const & G, int v) {
  vector<bool> V(num_vertices(G));
  stack<node_pair> E;
  Graph T;
  while(true) {
    V[v] = true;

    int x = -1, y = -1;
    for(auto w : range(adjacent_vertices(v, G)))
      if(!V[w]) {
        tie(x, y) = tie(v, w);
        E.emplace(v, w);
      }

    if(y == -1) {
      while(!E.empty() && V[E.top().second]) E.pop();
      if(E.empty()) break;
      tie(x, y) = E.top();
    } else {
      E.pop();
    }

    add_edge(x, y, T);

    v = y;
  }
  return T;
}

template <class Graph>
Graph dfs_fifo(Graph const & G, int v) {
  int rank;
  vector<int> V(num_vertices(G), rank = 0);
  queue<node_pair> E;
  Graph T;
  while(true) {
    V[v] = ++rank;

    int x, y = -1;
    for(auto w : range(adjacent_vertices(v, G)))
      if(V[w] == 0) {
        tie(x, y) = tie(v, w);
        break;
      }

    if(y == -1) {
      while(!E.empty() && V[E.front().second] != 0) E.pop();
      if(E.empty()) break;
      tie(x, y) = E.front();
    }

    for(int w : range(adjacent_vertices(x, G)))
      if(w != y && V[w] == 0) {
        E.emplace(x, w);
      }

    add_edge(x, y, T);

    v = y;
  }
  show("tree-dfs-fifo.dot", G, T);
  return T;
}

template <class Graph>
Graph dfs_fifo2(Graph const & G, int v) {
  vector<bool> V(num_vertices(G));
  queue<node_pair> E;
  Graph T;
  while(true) {
    V[v] = true;

    int x = -1, y = -1;
    for(auto w : range(adjacent_vertices(v, G)))
      if(!V[w]) {
        if(y == -1) tie(x, y) = tie(v, w);
        else E.emplace(v, w);
      }

    if(y == -1) {
      while(!E.empty() && V[E.front().second]) E.pop();
      if(E.empty()) break;
      tie(x, y) = E.front();
    }

    add_edge(x, y, T);

    v = y;
  }
  return T;
}

template <class Graph>
Graph bfs(Graph const & G, int v) {
  Graph T;
  int parent;
  queue<node_pair> Q;
  vector<bool> V(num_vertices(G));
  Q.emplace(-1, v);
  V[v] = true;
  while(!Q.empty()) {
    tie(parent, v) = Q.front();
    Q.pop();
    for(auto w : range(adjacent_vertices(v, G))) {
      if(!V[w]) {
        V[w] = true;
        add_edge(v, w, T);
        Q.emplace(v, w);
      }
    }
  }
  show("tree-bfs.dot", G, T);
  return T;
}

template<class Graph>
bool isomorphic(Graph const & A, Graph const & B) {
  if(num_vertices(A) != num_vertices(B))
    return false;
  if(num_edges(A) != num_edges(B))
      return false;
  for(auto e : range(edges(A)))
    if(!edge(source(e, A), target(e, A), B).second)
      return false;
  return true;
}

int main(int argc, char** argv) {
  options opt(argc, argv);
  int z = opt.get<int>("-z", 1);
  int n = opt.get<int>("-n", 20);
  float p = opt.get<float>("-p", 0.2);

  std::srand ( unsigned ( std::time(0) ) );

  ios_base::sync_with_stdio(0);

  test_suite<graph> suite("gnp", z, n, p);

  for(auto G : suite) {
    auto tree1 = rdfs_tree<graph, graph>(G);
    auto tree2 = rdfs_tree<graph, graph>(G);
    if(!isomorphic(tree1, tree2)) {
      show("iso-12-1.dot", G, tree1);
      show("iso-12-2.dot", G, tree2);
      return 1;
    }
  }

  return 0;
}


//==================================================================================================
/*!
  @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_ENUMERATE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ENUMERATE_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
  /*!
    @ingroup group-swar
    Generates a value containing sequentially increasing values, starting with @c seed and
    repetitively evaluating @c seed+=step.

    @par Scalar Semantic:
    For any type @c T , the following code:
    @code
    auto r = enumerate<T>(seed, step);
    @endcode
    is equivalent to:
    @code
    T r{seed};
    @endcode

    @par SIMD Semantic:
    For any type @c T and integral constant @c N, the following code:
    @code
    auto r = enumerate<boost::simd::pack<T,N>>(seed, step);
    @endcode
    is equivalent to:
    @code
    boost::simd::pack<T,N> r{seed, seed+step, ..., seed+(N-1)*step};
    @endcode

    @param seed Initial value of store, equals to @c 0 by default.
    @param step Increment to apply on each subsequent generated value, equals to @c 1 by default.
    @return A value containing the sequence of value generated from @c seed and @c step
  **/
  template<typename T, typename B, typename S> T enumerate(const B& seed = 0, const S& step = 1);
#endif
} }

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

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the kkmeans object
    and spectral_cluster() routine from the dlib C++ Library.

    The kkmeans object is an implementation of a kernelized k-means clustering
    algorithm.  It is implemented by using the kcentroid object to represent
    each center found by the usual k-means clustering algorithm.

    So this object allows you to perform non-linear clustering in the same way
    a svm classifier finds non-linear decision surfaces.

    This example will make points from 3 classes and perform kernelized k-means
    clustering on those points.  It will also do the same thing using spectral
    clustering.

    The classes are as follows:
        - points very close to the origin
        - points on the circle of radius 10 around the origin
        - points that are on a circle of radius 4 but not around the origin at all
*/

#include <iostream>
#include <vector>

#include <dlib/clustering.h>
#include <dlib/rand.h>

using namespace std;
using namespace dlib;

int main()
{
    // Here we declare that our samples will be 2 dimensional column vectors.
    // (Note that if you don't know the dimensionality of your vectors at compile time
    // you can change the 2 to a 0 and then set the size at runtime)
    typedef matrix<double,2,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the kcentroid object.  It is the object used to
    // represent each of the centers used for clustering.  The kcentroid has 3 parameters
    // you need to set.  The first argument to the constructor is the kernel we wish to
    // use.  The second is a parameter that determines the numerical accuracy with which
    // the object will perform part of the learning algorithm.  Generally, smaller values
    // give better results but cause the algorithm to attempt to use more dictionary vectors
    // (and thus run slower and use more memory).  The third argument, however, is the
    // maximum number of dictionary vectors a kcentroid is allowed to use.  So you can use
    // it to control the runtime complexity.
    kcentroid<kernel_type> kc(kernel_type(0.1),0.01, 8);

    // Now we make an instance of the kkmeans object and tell it to use kcentroid objects
    // that are configured with the parameters from the kc object we defined above.
    kkmeans<kernel_type> test(kc);

    std::vector<sample_type> samples;
    std::vector<sample_type> initial_centers;

    sample_type m;

    dlib::rand rnd;

    // we will make 50 points from each class
    const long num = 50;

    // make some samples near the origin
    double radius = 0.5;
    for (long i = 0; i < num; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // add this sample to our set of samples we will run k-means
        samples.push_back(m);
    }

    // make some samples in a circle around the origin but far away
    radius = 10.0;
    for (long i = 0; i < num; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // add this sample to our set of samples we will run k-means
        samples.push_back(m);
    }

    // make some samples in a circle around the point (25,25)
    radius = 4.0;
    for (long i = 0; i < num; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // translate this point away from the origin
        m(0) += 25;
        m(1) += 25;

        // add this sample to our set of samples we will run k-means
        samples.push_back(m);
    }

    // tell the kkmeans object we made that we want to run k-means with k set to 3.
    // (i.e. we want 3 clusters)
    test.set_number_of_centers(3);

    // You need to pick some initial centers for the k-means algorithm.  So here
    // we will use the dlib::pick_initial_centers() function which tries to find
    // n points that are far apart (basically).
    pick_initial_centers(3, initial_centers, samples, test.get_kernel());

    // now run the k-means algorithm on our set of samples.
    test.train(samples,initial_centers);

    // now loop over all our samples and print out their predicted class.  In this example
    // all points are correctly identified.
    for (unsigned long i = 0; i < samples.size()/3; ++i)
    {
        cout << test(samples[i]) << " ";
        cout << test(samples[i+num]) << " ";
        cout << test(samples[i+2*num]) << "\n";
    }

    // Now print out how many dictionary vectors each center used.  Note that
    // the maximum number of 8 was reached.  If you went back to the kcentroid
    // constructor and changed the 8 to some bigger number you would see that these
    // numbers would go up.  However, 8 is all we need to correctly cluster this dataset.
    cout << "num dictionary vectors for center 0: " << test.get_kcentroid(0).dictionary_size() << endl;
    cout << "num dictionary vectors for center 1: " << test.get_kcentroid(1).dictionary_size() << endl;
    cout << "num dictionary vectors for center 2: " << test.get_kcentroid(2).dictionary_size() << endl;

    // Finally, we can also solve the same kind of non-linear clustering problem with
    // spectral_cluster().  The output is a vector that indicates which cluster each sample
    // belongs to.  Just like with kkmeans, it assigns each point to the correct cluster.
    std::vector<unsigned long> assignments = spectral_cluster(kernel_type(0.1), samples, 3);
    cout << mat(assignments) << endl;

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MIN_POS_INCLUDE
#define MTL_MIN_POS_INCLUDE

#include <utility>

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/pos_type.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/look_at_each_nonzero.hpp>

namespace mtl {

    namespace vec {

	template <typename Vector>
	struct min_pos_functor
	{
	    typedef typename Collection<Vector>::value_type       value_type;
	    typedef typename mtl::traits::pos_type<Vector>::type  pos_type;
	    typedef std::pair<value_type, pos_type>               result_type;

	    // initialize with max value and max position
	    min_pos_functor() : value(math::identity(math::min<result_type>(), result_type())) {}

	    void operator()(const value_type& x, const pos_type& p)
	    {
		if (x < value.first)
		    value= std::make_pair(x, p);
 	    }

	    bool unchanged() const { return value.second == math::identity(math::min<pos_type>(), pos_type()); }

	    result_type  value;
	};
	///Returns position of minimal entry of %vector v
	template <typename Vector>
	typename min_pos_functor<Vector>::pos_type
	inline min_pos(const Vector& v)
	{
	    min_pos_functor<Vector> f;
	    look_at_each_nonzero_pos(v, f);

	    MTL_DEBUG_THROW_IF(f.unchanged(), runtime_error("min_pos cannot be applied on empty container"));
	    return f.value.second;
	}

    } // namespace vector

    namespace mat {

	using mtl::vec::min_pos;
    }

} // namespace mtl

#endif // MTL_MIN_POS_INCLUDE


// 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 file if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

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

#include "theia/sfm/pose/five_point_focal_length_radial_distortion.h"
#include "theia/test/test_utils.h"

namespace theia {

namespace {

using Eigen::Array;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

void P5pfrTestWithNoise(const Matrix3d& gt_rotation,
                        const Vector3d& gt_translation,
                        const double focal_length,
                        const double radial_distortion,
                        const std::vector<Vector3d>& world_points_vector,
                        const double noise,
                        const double reproj_tolerance) {
  Map<const Matrix<double, 3, 5> > world_points(world_points_vector[0].data());

  // Camera intrinsics matrix.
  const Matrix3d camera_matrix =
      Eigen::DiagonalMatrix<double, 3>(1.0, 1.0, 1.0 / focal_length);
  // Create the projection matrix P = K * [R t].
  Matrix<double, 3, 4> gt_projection;
  gt_projection << gt_rotation, gt_translation;
  gt_projection = camera_matrix * gt_projection;

  // Reproject 3D points to get undistorted image points.
  Matrix<double, 2, 5> undistorted_image_point =
      (gt_projection * world_points.colwise().homogeneous())
          .colwise()
          .hnormalized();

  // Determine radius of undistorted points and use that to compute the radius
  // of the distorted points.
  Array<double, 1, 5> radius_undistorted =
      undistorted_image_point.colwise().norm();
  Array<double, 1, 5> radius_distorted =
      (1.0 -
       (1.0 - 4.0 * radial_distortion * radius_undistorted.square()).sqrt()) /
      (2.0 * radial_distortion * radius_undistorted);
  Array<double, 1, 5> distortion_vec = radius_distorted / radius_undistorted;

  // Apply radial distortion.
  std::vector<Vector2d> distorted_image_points_vector(5);
  Map<Matrix<double, 2, 5> > distorted_image_point(
      distorted_image_points_vector[0].data());
  distorted_image_point = undistorted_image_point.cwiseProduct(
      distortion_vec.matrix().replicate<2, 1>());

  // Add noise to distorted image points.
  if (noise) {
    std::default_random_engine generator;
    std::normal_distribution<double> distribution(0.0, noise);
    for (int i = 0; i < 5; i++) {
      distorted_image_point.col(i).x() += distribution(generator);
      distorted_image_point.col(i).y() += distribution(generator);
    }
  }

  // Run P5Pfr algorithm.
  std::vector<Matrix<double, 3, 4> > soln_projection;
  std::vector<std::vector<double> > soln_distortion;
  CHECK(
      theia::FivePointFocalLengthRadialDistortion(distorted_image_points_vector,
                                                  world_points_vector,
                                                  1,
                                                  &soln_projection,
                                                  &soln_distortion));

  bool matched_transform = false;
  for (int i = 0; i < 4; ++i) {
    matched_transform = true;
    // Check the reprojection error.
    for (int n = 0; n < 5; n++) {
      const double distortion_w =
          1.0 +
          soln_distortion[i][0] * distorted_image_point.col(n).squaredNorm();
      Eigen::Vector2d undist_pt = distorted_image_point.col(n) / distortion_w;
      Eigen::Vector3d reproj_pt =
          soln_projection[i] * world_points.col(n).homogeneous();
      const double reproj_error =
          (undist_pt - reproj_pt.hnormalized()).squaredNorm();
      if (reproj_error > reproj_tolerance) {
        matched_transform = false;
        break;
      }
    }

    if (matched_transform) {
      break;
    }
  }
  // One of the solutions must have been a valid solution.
  EXPECT_TRUE(matched_transform);
}

void BasicTest(const double noise, const double reproj_tolerance) {
  // focal length (values used in the ICCV paper)
  const double focal_length = 1.3;
  // radial distortion (values used in the ICCV paper)
  const double radial_distortion = -0.35;

  const double x = -0.10;  // rotation of the view around x axis
  const double y = -0.20;  // rotation of the view around y axis
  const double z = 0.30;   // rotation of the view around z axis

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0, -sin(z), cos(z), 0, 0, 0, 1;
  Ry << cos(y), 0, -sin(y), 0, 1, 0, sin(y), 0, cos(y);
  Rx << 1, 0, 0, 0, cos(x), sin(x), 0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation =
      Vector3d(-0.00950692, 000.0171496, 000.0508743);

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector(5);
  Map<Matrix<double, 3, 5> > world_points(world_points_vector[0].data());
  world_points << -0.42941, 0.000621211, -0.350949, -1.45205, -1.294, 0.415794,
      -0.556605, -1.92898, -1.89976, -1.12445, 1.4949, 0.838307, 1.41972,
      1.25756, 0.805163;
  P5pfrTestWithNoise(gt_rotation,
                     gt_translation,
                     focal_length,
                     radial_distortion,
                     world_points_vector,
                     noise,
                     reproj_tolerance);
}

void PlanarTestWithNoise(const double noise, const double reproj_tolerance) {
  // focal length (values used in the ICCV paper)
  const double focal_length = 1.3;
  // radial distortion (values used in the ICCV paper)
  const double radial_distortion = -0.35;
  const double size = 100;
  const double depth = 150;

  const double x = -0.10;  // rotation of the view around x axis
  const double y = -0.20;  // rotation of the view around y axis
  const double z = 0.30;   // rotation of the view around z axis

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0, -sin(z), cos(z), 0, 0, 0, 1;
  Ry << cos(y), 0, -sin(y), 0, 1, 0, sin(y), 0, cos(y);
  Rx << 1, 0, 0, 0, cos(x), sin(x), 0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation =
      Vector3d(-0.00950692, 000.0171496, 000.0508743);

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector(5);
  world_points_vector[0] = Eigen::Vector3d(-size / 2, -size / 2, depth);
  world_points_vector[1] = Eigen::Vector3d(size / 2, -size / 2, depth);
  world_points_vector[2] = Eigen::Vector3d(size / 2, size / 2, depth);
  world_points_vector[3] = Eigen::Vector3d(-size / 2, size / 2, depth);
  world_points_vector[4] = Eigen::Vector3d(0.0, 0.0, depth);

  P5pfrTestWithNoise(gt_rotation,
                     gt_translation,
                     focal_length,
                     radial_distortion,
                     world_points_vector,
                     noise,
                     reproj_tolerance);
}

TEST(P5Pfr, BasicTest) { BasicTest(0.0, 1e-12); }

TEST(P5Pfr, BasicNoiseTest) { BasicTest(0.5 / 800.0, 5 / 800.0); }

TEST(P5Pfr, PlanarTestNoNoise) { PlanarTestWithNoise(0.0, 1e-12); }

TEST(P5Pfr, PlanarTestWithNoise) {
  PlanarTestWithNoise(0.5 / 800.0, 5 / 800.0);
}

}  // namespace
}  // namespace theia


#define BOOST_LOG_DYN_LINK 1
#include <boost/scoped_ptr.hpp>
#include <boost/log/trivial.hpp>

#include <ros/ros.h>
#include <ros/publisher.h>
#include <signal.h>
#include <camera_info_manager/camera_info_manager.h>
#include <image_transport/image_transport.h>
#include <image_transport/camera_publisher.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <driver_base/SensorLevels.h>
#include <boost/filesystem.hpp>
#include <std_msgs/String.h>

#include <pix2image.h>

namespace POLPro
{
    std::vector<cv::Mat> raw2mat(const cv::Mat& origin, const bool show=true) {
        // define the size of the output
        cv::Size output_size(origin.cols / 2, origin.rows / 2);
        // declare the vector containing the 4 angles images
        const int nb_angles = 4;
        std::vector<cv::Mat> output_img(nb_angles);
        for (auto it = output_img.begin(); it != output_img.end(); ++it)
            *it = cv::Mat::zeros(output_size, CV_8U);

        // copy the data in the new image
        for (int angle = 0; angle < nb_angles; ++angle) {
            int offset_row = angle / 2;
            int offset_col = angle % 2;
            BOOST_LOG_TRIVIAL(debug) << "offset_row " << offset_row
                                     << " offset_col " << offset_col;

            for (int row = 0; row < origin.rows/2; ++row)
                for (int col = 0; col < origin.cols/2; ++col)
                    output_img[angle].at<uchar>(row, col) = origin.at<uchar>(
                        2 * row + offset_row, 2 * col + offset_col);
        }

        if (show)
            imshow(output_img, false, false);

        return output_img;
    }

    std::vector<cv::Mat> compute_stokes(const std::vector<cv::Mat>& angles_img,
                                        const bool show=true) {
        // define the number of images to have for Stokes
        const int nb_stokes_img = 3;
        // Create zeros images
        std::vector<cv::Mat> output_img(nb_stokes_img);
        for (auto it = output_img.begin(); it != output_img.end(); ++it)
            *it = cv::Mat::zeros(angles_img[0].size(), CV_32F);

        // compute the Stokes parameters maps
        // S0: add the different angles
        for (auto it = angles_img.begin(); it != angles_img.end(); ++it)
            cv::add(output_img[0], *it, output_img[0], cv::noArray(),
                    CV_32F);
        output_img[0] /= 2.0;
        BOOST_LOG_TRIVIAL(debug) << minmax(output_img[0], "s0");

        // S1: subtract angles 0 and 90
        cv::subtract(angles_img[0], angles_img[2], output_img[1],
                     cv::noArray(), CV_32F);
        BOOST_LOG_TRIVIAL(debug) << minmax(output_img[1], "s1");

        // S2: subtract angles 45 and 135
        cv::subtract(angles_img[1], angles_img[3], output_img[2],
                     cv::noArray(), CV_32F);
        BOOST_LOG_TRIVIAL(debug) << minmax(output_img[2], "s2");

        if (show)
            imshow(output_img, false, true);

        return output_img;
    }

    std::vector<cv::Mat> compute_stokes(const cv::Mat& origin,
                                        const bool show=true) {
        // refactor the raw image
        std::vector<cv::Mat> angles_img = raw2mat(origin, show);

        return compute_stokes(angles_img, show);
    }

    std::vector<cv::Mat> compute_polar_params(
        const std::vector<cv::Mat>& origin, const bool show=true) {
        std::vector<cv::Mat> stokes_img;
        // Check if we have the original data or the stokes
        if (origin.size() == 4) {
            stokes_img = compute_stokes(origin, show);
        } else {
            stokes_img = origin;
        }

        // define the number of maps
        const int nb_params = 3;
        // create the zeros images
        std::vector<cv::Mat> output_img(nb_params);
        for (auto it = output_img.begin(); it != output_img.end(); ++it)
            *it = cv::Mat::zeros(stokes_img[0].size(), CV_32F);

        // compute the polar coordinate in degrees
        cv::cartToPolar(stokes_img[1], stokes_img[2],
                        output_img[0], output_img[1],
                        true);
        // normalize the maps
        // degree of polarization
        output_img[0] /= stokes_img[0];
        // angle of polarization
        output_img[1] *= 0.5;
        // copy s0
        stokes_img[0].copyTo(output_img[2]);
        if (show)
            imshow(output_img, false, false);

        return output_img;
    }

    std::vector<cv::Mat> compute_polar_params(const cv::Mat& origin,
                                              const bool show=true) {
        // compute the Stokes' parameters
        std::vector<cv::Mat> stokes_img = compute_stokes(origin, show);

        return compute_polar_params(stokes_img, show);
    }

    std::string minmax(const cv::Mat& img, const std::string& s) {
        double min, max;
        cv::Point idmin, idmax;
        cv::minMaxLoc(img, &min, &max, &idmin, &idmax) ;

        return "Image " + s
            + ": min=" + std::to_string(min)
            + " - max= " + std::to_string(max);
    }

    void imshow(std::vector<cv::Mat> img, const bool as_hsv=false,
                const bool is_stokes=true) {

        // through an error if there is not 3d img and hsv is turned on
        if ((img.size() != 3) && as_hsv)
            throw std::invalid_argument("img needs to be a 3 channels images"
                                        " if you need hsv support");

        // Convert the data if Stokes or polarization parameters
        if (img.size() == 3) {
            if (is_stokes) {
                // Stokes parameters normalization
                img[0] /= 2.0;
                img[1] = (img[1] + 255.0) / 2.0;
                img[2] = (img[2] + 255.0) / 2.0;
            } else {
                // polarization parameters normalization
                img[0] = img[0] * 255;
                img[2] = img[2] / 2;
            }
            // Convert to uint8
            for (int i = 0; i < img.size(); ++i){
                img[i].convertTo(img[i], CV_8UC1);
            }

        }
        // Declare the output image
        cv::Mat output_img;

        if (as_hsv) {
            // Merge the image together to have a 3 channels image

            std::vector<cv::Mat> channels;
            channels.push_back(img[1].clone());
            channels.push_back(img[0].clone());
            channels.push_back(img[2].clone());

            cv::Mat bgr_img;
            cv::merge(channels, bgr_img);
            cv::cvtColor(bgr_img, output_img, CV_HLS2BGR);
        } else {
            // Concatenate the images available together
            cv::Size img_size(img[0].cols, img[0].rows);
            output_img = cv::Mat::zeros
                (img[0].rows*2, img[0].cols*2, CV_8UC1);
            int rows = img[0].rows;
            int cols = img[0].cols;

            for (int i = 0; i < img.size(); ++i) {
                // we need to shift the image next to each other properly
                int offset_col = i % 2;
                int offset_row = i / 2;

                img[i].copyTo(output_img(
                                  cv::Rect(img_size.width * offset_col,
                                           img_size.height * offset_row,
                                           img_size.width,
                                           img_size.height)));

            }
        }

        cv::imshow("Output image", output_img);
    }
}  // Namespace POLPro


// A2DD.h
#ifndef A2DD_H
#define A2DD_H

#include <armadillo>

namespace LLS {

using namespace arma;

class LLS_impl
{

public:
  //A2DD(auto& x_arr, auto& y_arr, int N);
  LLS_impl(std::array<double, 4ul>& x_arr, std::array<double, 4ul>& y_arr, int N);
  int getParams(double& B1, double& B2);

private:
  mat eq_mat;
  colvec y_vec;
  colvec parameter;

};

}

#endif


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/compare_less_equal.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" compare_less_equal",  STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using bs::compare_less_equal;
  using r_t = decltype(compare_less_equal(T(), T()));

  // specific values tests
  STF_LESS_EQUAL(compare_less_equal(bs::One<T>(), bs::One<T>()),  r_t(true));
  STF_LESS_EQUAL(compare_less_equal(bs::One<T>(), bs::Zero<T>()), r_t(false));
  STF_LESS_EQUAL(compare_less_equal(bs::Zero<T>(), bs::One<T>()), r_t(true));

} // end of test for floating_


#include <iostream>
#include <stdio.h>
#include <fstream>
#include <vector>
//#include <map>
//#include <set>
#include <unordered_set>
//#include <unordered_map>
#include <cmath>
#include <algorithm>

// #include <gnuplot-iostream.h>
#include <assert.h>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include "vector3d.hpp"
#include "swiss_to_lat_lon.hpp"
#include "height_map.hpp"

const int horizon_angles = 360;
const int tile_size = 40;

namespace po = boost::program_options;

struct pos_hoz {
    vector3d pos;
    vector3d norm;
    short elevation_angles[horizon_angles];
    pos_hoz() : pos(0,0,0), norm(0,0,0) {
        memset(elevation_angles, 0, sizeof(short) * horizon_angles);
    }
};

bool EQ_DBL(double a, double b){
    return std::abs(a-b)<EPS_DBL;
}

int main(int ac, char** av) {

    bool test = false;
    //Variables to be assigned by program options
    double height_map_resolution;
    double north_bound;
    double south_bound;
    double east_bound;
    double west_bound;

    double elevation_angles_tan[horizon_angles];

    std::string input_file;
    std::string output_dir;
    bool verbose = false;

    // Declare the supported options.
    po::options_description op_desc("Allowed options");
    op_desc.add_options()
    ("help", "print options table")
    ("input-file,i", po::value<std::string>(&input_file), "File containing height map data in x<space>y<space>z<newline> swiss coordinates format")
    ("output-dir,o", po::value<std::string>(&output_dir), "Directory for output files (default data_out_<date_time>)")
    ("resolution,R", po::value<double>(&height_map_resolution)->default_value(25.0), "resolution of data (default: 25.0)")
    ("nmax",po::value<double>(&north_bound)->default_value(1e100), "maximum north coordinate to be treated (default 1e100)")
    ("nmin",po::value<double>(&south_bound)->default_value(-1e100), "minimum north coordinate to be treated (default -1e100)")
    ("emax",po::value<double>(&east_bound)->default_value(1e100), "maximum east coordinate to be treated (default 1e100)")
    ("emin",po::value<double>(&west_bound)->default_value(-1e100), "minimum east coordinate to be treated (default -1e100)")
    ("verbose, v", "Verbose: output lots of text")
    ;

    po::positional_options_description pd;
    pd.add("input-file", 1).add("output-file", 1);

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, op_desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout <<"ComputeHorizons [options] [input file] [output base]"<<std::endl<< op_desc << std::endl;
        return 1;
    }

    if (vm.count("verbose")){
        verbose = true;
    }
    if(!vm.count("input-file")){
        std::cout << "Input file must be specified" << std::endl;
        exit(255);
    }
    if(!vm.count("output-dir")){
        std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
        std::string time_string(30, 0);
        std::strftime(&time_string[0], time_string.size(), "%Yx%mx%dT%Hx%Mx%S", std::localtime(&now));
        output_dir = std::string("hoz_out_")+time_string;
        std::cout << "Output directory will be: "+output_dir << std::endl;
    }

    //create output directory
    boost::filesystem::path dir(output_dir);
    if(!boost::filesystem::create_directory(dir)) {
        std::cout << "Failed to creat output directory" << "\n";
        exit(255);
    }

    std::ifstream ifs(input_file);
    if (!ifs.is_open())
        throw std::runtime_error("could not open file : " + std::string(input_file));

    height_map grid_points(ifs, south_bound, north_bound, east_bound, west_bound);

    //grid_points is a height_map, an unordered_set of all points in bounding box with
    //the maximum and minimum x,y,h of all values stored.

    std::pair<double,double> NE = swiss_to_lat_lon(grid_points.xmax()+height_map_resolution/2.0, grid_points.ymax()+height_map_resolution/2.0);
    std::pair<double,double> SW = swiss_to_lat_lon(grid_points.xmin()-height_map_resolution/2.0, grid_points.ymin()-height_map_resolution/2.0);

    std::cout << "NE: " << grid_points.xmax() <<", " << grid_points.ymax() << " SW: "<< grid_points.xmin() << " , " << grid_points.ymin() <<  std::endl;
    std::cout  << "NE: " << NE.first <<", " << NE.second << " SW: "<< SW.first << ", " << SW.second <<  std::endl;
    std::cout << "Maximum height: " << grid_points.hmax() << std::endl;
    std::cout << "Number of points in dataset: " << grid_points.size() << std::endl;

    if(test){
        double y_test = height_map_resolution*floor(601430.0/height_map_resolution);
        double x_test = height_map_resolution*floor(126243.0/height_map_resolution);
        auto it_v = grid_points.find_point(vector3d(x_test,y_test,0));      //get the gridpoint from the set
        std::cout << "Test point: " << x_test <<" "<< y_test << std::endl;
        std::cout << "Resolution: " << height_map_resolution << ", tile size: "<< tile_size << std::endl;
        vector3d v = *it_v;
        double elevation_angles[360];
        int theta_start = 0;
        int theta_end = 360;

        //        time_t start_time  = time(NULL);
        clock_t t = clock();
        for(int theta = theta_start;theta<theta_end;theta++){
            double phi = atan(grid_points.compute_elevation_angle(v,theta, height_map_resolution, 360.0/horizon_angles));
            short phi_short = (short)((phi / M_PI_2) * std::numeric_limits<short>::max());
            elevation_angles[theta] = phi_short;

        }
        double run_time = ((double)(clock()-t))/CLOCKS_PER_SEC;
        //        double run_time = difftime(time(NULL), start_time);
        std::cout << "phi = [";
        for(int theta = theta_start;theta<theta_end;theta++){
            std::cout << elevation_angles[theta] << ";";
        }
        std::cout << "];" << std::endl;
        std::cout << "runtime test point: " << run_time << std::endl;
        std::cout << "estimated runtime all points: " << run_time*grid_points.size() << std::endl;

        return 0;
    }

    std::string file_name;
    std::ofstream ofs;

    time_t start_time  = time(NULL);
    int N=0;
    int NT=0;

    int first_tile_y = (int) floor(grid_points.ymin()/(tile_size*height_map_resolution));
    first_tile_y = first_tile_y * (tile_size*height_map_resolution);
    int first_tile_x = (int) floor(grid_points.xmin()/(tile_size*height_map_resolution));
    first_tile_x = first_tile_x * (tile_size*height_map_resolution);
    int last_tile_y = (int) floor(grid_points.ymax()/(tile_size*height_map_resolution));
    last_tile_y = last_tile_y * (tile_size*height_map_resolution);
    int last_tile_x = (int) floor(grid_points.xmax()/(tile_size*height_map_resolution));
    last_tile_x = last_tile_x * (tile_size*height_map_resolution);
    int N_tiles_x = (last_tile_x - first_tile_x )/(tile_size*height_map_resolution) + 1;
    int N_tiles_y = (last_tile_y - first_tile_y )/(tile_size*height_map_resolution) + 1;

    std::cout << "FTy " << first_tile_y << " LTy " << last_tile_y << " FTx " << first_tile_x << " LTx " << last_tile_x << std::endl;
    std::cout << "calculating " << N_tiles_x << " x " << N_tiles_y << " tiles = " << N_tiles_x*N_tiles_y << " total" << std::endl;

    pos_hoz tile_points[tile_size*tile_size];

    for(int x_tile = 0;x_tile<N_tiles_x; x_tile++){
        for(int y_tile = 0; y_tile<N_tiles_y;y_tile++){
            double coord_x = (first_tile_x + height_map_resolution*tile_size*x_tile);
            double coord_x_end = coord_x+height_map_resolution*tile_size;
            int tile_point = 0;
            bool tile_empty = true;
            while(coord_x < coord_x_end){
                double coord_y = (first_tile_y + height_map_resolution*tile_size*y_tile);
                double coord_y_end = coord_y+height_map_resolution*tile_size;
                while(coord_y < coord_y_end){

                    //get point and points to the north, west, south and east
                    vector3d v(coord_x, coord_y, 0.0);
                    vector3d vN(v.x+height_map_resolution,v.y,0);
                    vector3d vW(v.x,v.y+height_map_resolution,0);
                    vector3d vS(v.x-height_map_resolution,v.y,0);
                    vector3d vE(v.x,v.y-height_map_resolution,0);

                    auto itv=grid_points.find_point(v);
                    auto itE=grid_points.find_point(vE);
                    auto itN=grid_points.find_point(vN);
                    auto itW=grid_points.find_point(vW);
                    auto itS=grid_points.find_point(vS);

                    if (grid_points.is_end(itv) ||grid_points.is_end(itE) || grid_points.is_end(itN) || grid_points.is_end(itW) || grid_points.is_end(itS)){
                        coord_y = coord_y + height_map_resolution;
                        continue;
                        //if one of the neighboring points are not found we continue with the next point
                    }
                    v = *itv;
                    vE = *itE;
                    vN = *itN;
                    vW = *itW;
                    vS = *itS;
                    tile_points[tile_point].pos = v;

                    vector3d Normal = ((((vE-v)^(vN-v))+((vW-v)^(vS-v)))/2).norm();       //normal is the crossproduct of two prependicular differences. Avgd.

                    tile_points[tile_point].norm = Normal;

/*                    for(int theta=0;theta<horizon_angles;theta++){
                        double phi = atan(grid_points.compute_elevation_angle(v,theta, height_map_resolution, 360.0/horizon_angles));
                        tile_points[tile_point].elevation_angles[theta] = (short)((phi / M_PI_2) * std::numeric_limits<short>::max());
                    }
 */ //old crap for finding elevations for an angle

                    //assign very negative tangent angle for the starting value
                    for(int theta=0;theta<horizon_angles;theta++){
                        elevation_angles_tan[theta] = -1e10;
                    }
                    //iterate over all points
                    for (auto itr = grid_points.first_point(); itr != grid_points.last_point(); ++itr){
                        vector3d v2 = *itr;
                        int theta;
                        if(EQ_DBL(v2.x,v.x)){
                            if(v2.y>v.y){
                                theta = 90;
                            }
                            else{
                                theta = 270;
                            }
                            double hdiff = v2.z-v.z;
                            double dxy = v.distxy(v2);
                            elevation_angles_tan[theta] = hdiff / dxy;
                        }
                        if(EQ_DBL(v2.y,v.y)){
                            if(v2.x>v.x){
                                theta = 0;
                            }
                            else{
                                theta = 180;
                            }
                            double hdiff = v2.z-v.z;
                            double dxy = v.distxy(v2);
                            elevation_angles_tan[theta] = hdiff / dxy;
                        }
                        // Find the four neighbours to our point
                        vector3d v2N(v2.x+height_map_resolution,v2.y,0);
                        vector3d v2W(v2.x,v2.y+height_map_resolution,0);
                        vector3d v2S(v2.x-height_map_resolution,v2.y,0);
                        vector3d v2E(v2.x,v2.y-height_map_resolution,0);
                        auto itE=grid_points.find_point(v2E);
                        auto itN=grid_points.find_point(v2N);
                        auto itW=grid_points.find_point(v2W);
                        auto itS=grid_points.find_point(v2S);
                        v2E = *itE;
                        v2N = *itN;
                        v2W = *itW;
                        v2S = *itS;

                        //calculate the integral multiples of horizon_angles
                        int v2th = ceil(horizon_angles*atan2(v2.y-v.y, v2.x-v.x)/(2*M_PI));
                        int v2Nth = ceil(horizon_angles*atan2(v2N.y-v.y, v2N.x-v.x)/(2*M_PI));
                        int v2Eth = ceil(horizon_angles*atan2(v2E.y-v.y, v2E.x-v.x)/(2*M_PI));
                        int v2Sth = ceil(horizon_angles*atan2(v2S.y-v.y, v2S.x-v.x)/(2*M_PI));
                        int v2Wth = ceil(horizon_angles*atan2(v2W.y-v.y, v2W.x-v.x)/(2*M_PI));
                        for(int th=v2th;th<v2Nth;++th){

                        }

                    }

                    tile_empty = false;
                    N++;
                    coord_y = coord_y + height_map_resolution;
                    tile_point++;
                }

                coord_x = coord_x + height_map_resolution;

                if (1) {
                    double run_time = difftime(time(NULL), start_time);
                    int rdays = (int)floor(run_time/86400.0);
                    int rhours = (int)floor((run_time-rdays*86400)/3600.0);
                    int rminutes = (int)floor((run_time-rdays*86400.0-rhours*3600.0)/60.0);
                    int rseconds = (int)floor(run_time-rdays*86400.0-rhours*3600.0-rminutes*60.0);
                    double remaining_time = ((grid_points.size() - N)*1.0)*run_time/(N*1.0);
                    int days = (int)floor(remaining_time/86400.0);
                    int hours = (int)floor((remaining_time-days*86400)/3600.0);
                    int minutes = (int)floor((remaining_time-days*86400-hours*3600.0)/60.0);
                    int seconds = (int)floor(remaining_time-days*86400-hours*3600.0-minutes*60.0);
                    double progress = round(1000.0*100.0*(N*1.0)/(grid_points.size()*1.0))/1000.0;
                    if(N > 0){
                        std::cout <<"   Progress: "<< progress <<" %  ("<<N<<", "<<NT<<")  time: "<<rdays<<"d"<<rhours<<":"<<rminutes<<":"<<rseconds<<" time left: "<<days<<"d"<<hours<<":"<<minutes<<":"<<seconds<<"      \r";
                        std::cout.flush();
                    }
                }

            }
            if(tile_point > 0){
                std::string tile_name;
                int tile_x = (first_tile_x + height_map_resolution*tile_size*x_tile);
                int tile_y = (first_tile_y + height_map_resolution*tile_size*y_tile);
                tile_name = output_dir + std::string("/tile_") + std::to_string(tile_x) + std::string("_") + std::to_string(tile_y) + std::string(".hoz");
                //            std::cout << std::endl << "tile_name " << tile_name << std::endl;
                ofs.open(tile_name, std::iostream::out | std::iostream::binary);
                //            std::cout << "first point" << tile_points[0].pos << "norm: " << tile_points[0].norm<< std::endl;
                //open output file
                if (!ofs.is_open()){
                    std::cout << "Can't open output file " << tile_name << std::endl;
                }
                ofs.write((char*)&tile_points[0], sizeof(pos_hoz)*tile_point);
                ofs.flush();
                ofs.close();
                if(NT%10==0)
                    std::cout << std::endl;
                NT++;
            }

        }
    }
    return 0;
}


#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/shared_ptr.hpp>

#include "Random.h"

namespace calc
{

  class UniformGen::Impl
  {
  public:
    Impl( unsigned seed , int lower , int upper )
      :rgen_(seed),
       udist_(lower,upper),
       gen_(rgen_,udist_)
    {}

    int next() { return gen_(); }

  public:
    boost::mt19937       rgen_;
    boost::uniform_int<> udist_;
    boost::variate_generator< boost::mt19937 , boost::uniform_int<> > gen_;
  };

  class NormalGen::Impl
  {
  public:
    Impl( unsigned seed , double mu , double sigma )
      :rgen_(seed),
       ndist_(mu,sigma),
       gen_(rgen_,ndist_)
    {}

    double next() { return gen_(); }

  public:
    boost::mt19937       rgen_;
    boost::normal_distribution<double>  ndist_;
    boost::variate_generator< boost::mt19937 , boost::normal_distribution<double>  > gen_;
  };

  class LogNormalGen::Impl
  {
  public:
    Impl( unsigned seed , double mu , double sigma )
      :rgen_(seed),
       ndist_(mu,sigma),
       gen_(rgen_,ndist_)
    {}

    double next() { return gen_(); }

  public:
    boost::mt19937       rgen_;
    boost::lognormal_distribution<double>  ndist_;
    boost::variate_generator< boost::mt19937 , boost::lognormal_distribution<double>  > gen_;
  };

}

calc::UniformGen::UniformGen( unsigned seed , int lower , int upper )
  :seed_(seed),lower_(lower),upper_(upper),
   impl_( new Impl( seed , lower, upper ) )
{}

int calc::UniformGen::operator()() { return impl_->next(); }

calc::NormalGen::NormalGen( unsigned seed , double mu , double sigma )
  :seed_(seed),mu_(mu),sigma_(sigma),
   impl_( new Impl( seed , mu , sigma ) )
{}

double calc::NormalGen::operator()() { return impl_->next(); }

calc::LogNormalGen::LogNormalGen( unsigned seed , double mu , double sigma )
  :seed_(seed),mu_(mu),sigma_(sigma),
   impl_( new Impl( seed , mu , sigma ) )
{}

double calc::LogNormalGen::operator()() { return impl_->next(); }


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <opencv2/core.hpp>
#include <boost/algorithm/string.hpp>

#define CTS350 0
#define CIR204 1

/*!
   \brief Retrieves a vector of the (radar) file names in ascending order of time stamp
   \param datadir (absolute) path to the directory that contains (radar) files
   \param radar_files [out] A vector to be filled with a string for each file name
   \param extension Optional argument to specify the desired file extension. Files without this extension are rejected
*/
void get_file_names(std::string datadir, std::vector<std::string> &radar_files, std::string extension = "");

/*!
   \brief Decode a single Oxford Radar RobotCar Dataset radar example
   \param path path to the radar image png file
   \param timestamps [out] Timestamp for each azimuth in int64 (UNIX time)
   \param azimuths [out] Rotation for each polar radar azimuth (radians)
   \param valid [out] Mask of whether azimuth data is an original sensor reasing or interpolated from adjacent azimuths
   \param fft_data [out] Radar power readings along each azimuth
*/
void load_radar(std::string path, std::vector<int64_t> &timestamps, std::vector<double> &azimuths,
    std::vector<bool> &valid, cv::Mat &fft_data, int navtech_version = CTS350);

void load_velodyne(std::string path, std::vector<int64_t> &timestamps, std::vector<double> &azimuths,
    Eigen::MatrixXd &pc);

void load_velodyne2(std::string path, Eigen::MatrixXd &pc);

void load_velodyne3(std::string path, Eigen::MatrixXd &pc, Eigen::MatrixXd & intensities, std::vector<float> &times);

double get_azimuth_index(std::vector<double> &azimuths, double azimuth);

/*!
   \brief Decode a single Oxford Radar RobotCar Dataset radar example
   \param azimuths Rotation for each polar radar azimuth (radians)
   \param fft_data Radar power readings along each azimuth
   \param radar_resolution Resolution of the polar radar data (metres per pixel)
   \param cart_resolution Cartesian resolution (meters per pixel)
   \param cart_pixel_width Width and height of the returned square cartesian output (pixels).
   \param interpolate_crossover If true, interpolates between the end and start azimuth of the scan.
   \param cart_img [out] Cartesian radar power readings
*/
void radar_polar_to_cartesian(std::vector<double> &azimuths, cv::Mat &fft_data, float radar_resolution,
    float cart_resolution, int cart_pixel_width, bool interpolate_crossover, cv::Mat &cart_img,
    int output_type = CV_32F, int navtech_version = CTS350);

/*!
   \brief Converts points from polar coordinates to cartesian coordinates
   \param azimuths The actual azimuth of each row in the fft data reported by the Navtech sensor
   \param polar_points Matrix of point locations (azimuth_bin, range_bin) x N
   \param radar_resolution Resolution of the polar radar data (metres per pixel)
   \param cart_points [out] Matrix of points in cartesian space (x, y) x N in metric
*/
void polar_to_cartesian_points(std::vector<double> azimuths, Eigen::MatrixXd polar_points, float radar_resolution,
    Eigen::MatrixXd &cart_points);

void polar_to_cartesian_points(std::vector<double> azimuths, std::vector<int64_t> times, Eigen::MatrixXd polar_points,
    float radar_resolution, Eigen::MatrixXd &cart_points, std::vector<int64_t> &point_times);

/*!
   \brief Converts points from metric cartesian coordinates to pixel coordinates in the BEV image
   \param cart_points Vector of points in metric cartesian space (x, y)
   \param cart_resolution Cartesian resolution (meters per pixel)
   \param cart_pixel_width: Width and height of the returned square cartesian output (pixels)
   \param bev_points [out] Vector of pixel locations in the BEV cartesian image (u, v)
*/

void convert_to_bev(Eigen::MatrixXd &cart_points, float cart_resolution, int cart_pixel_width,
    std::vector<cv::Point2f> &bev_points);

void convert_to_bev(Eigen::MatrixXd &cart_points, float cart_resolution, int cart_pixel_width, int patch_size,
    std::vector<cv::KeyPoint> &bev_points, std::vector<int64_t> &point_times);

/*!
   \brief Converts points from pixel coordinates in the BEV image to metric cartesian coordinates
   \param bev_points Vector of pixel locations in the BEV cartesian image (u, v)
   \param cart_resolution Cartesian resolution (meters per pixel)
   \param cart_pixel_width: Width and height of the returned square cartesian output (pixels)
   \param cart_points [out] Vector of points in metric cartesian space (x, y)
*/
void convert_from_bev(std::vector<cv::KeyPoint> bev_points, float cart_resolution, int cart_pixel_width,
    Eigen::MatrixXd &cart_points);

/*!
   \brief Draws a red dot for each feature on the top-down cartesian view of the radar image
   \param cart_img Cartesian radar power readings
   \param cart_targets Matrix of points in cartesian space (x, y) < N in metric
   \param cart_resolution Cartesian resolution (meters per pixel)
   \param cart_pixel_width Width and height of the square cartesian image.
   \param vis [out] Output image with the features drawn onto it
*/
void draw_points(cv::Mat cart_img, Eigen::MatrixXd cart_targets, float cart_resolution, int cart_pixel_width,
    cv::Mat &vis, std::vector<uint> color = {0, 0, 255});

void draw_points(cv::Mat &vis, Eigen::MatrixXd cart_targets, float cart_resolution, int cart_pixel_width,
    std::vector<uint> color = {0, 0, 255});

/*!
   \brief Retrieves the ground truth odometry between radar timestamps t1 and t2
   \param gtfile (absolute) file location of the radar_odometry.csv file
   \param t1
   \param t2
   \param gt [out] Vector of floats for the ground truth transform between radar timestamp t1 and t2 (x, y, z, r, p, y)
*/
bool get_groundtruth_odometry(std::string gtfile, int64 t1, int64 t2, std::vector<float> &gt);

bool get_groundtruth_odometry2(std::string gtfile, int64_t t, std::vector<double> &gt);

void draw_matches(cv::Mat &img, std::vector<cv::KeyPoint> kp1, std::vector<cv::KeyPoint> kp2,
    std::vector<cv::DMatch> matches, int radius = 4);

void getTimes(Eigen::MatrixXd cart_targets, std::vector<double> azimuths, std::vector<int64_t> times,
    std::vector<int64_t> &tout);

/*!
   \brief Load arguments from the command line and check their validity.
*/
int validateArgs(const int argc, const char *argv[], std::string &root, std::string &seq, std::string &app);
int validateArgs(const int argc, const char *argv[], std::string &root);


#include <iomanip>
#include <boost/timer/timer.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/sum.hpp>
#include <iacaMarks.h>

template< typename FType > FType calcPi();

int main()
{
    boost::timer::cpu_timer timer;

    timer.start();
    double pi = calcPi<double>();
    timer.stop();

    std::cout << "Pi: " << std::setprecision( 20 ) << pi << std::endl;
    std::cout << timer.format();

    return 0;
}

template< typename FType > FType calcPi()
{
    using dpack = boost::simd::pack<FType, 4*boost::simd::pack<FType>::static_size>;

    dpack div;
    dpack one;
    dpack sum = dpack( 0 );
    dpack step = dpack( 2 * dpack::static_size );

    FType val = -1;
    FType fac = 3;
    for( size_t i = 0; i < dpack::static_size; ++i )
    {
        one[ i ] = val;
        div[ i ] = fac;
        val *= -1;
        fac += 2;
    }

        IACA_START
    for( size_t i = 0; i < 1024ull*1024ull*1024ull*16ull / dpack::static_size ; ++i )
    {
        sum += one / div;
        div += step;
    }
        IACA_END

    return 4 * (1 + boost::simd::sum( sum ) );
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/sgtbx/seminvariant.h>
#include <cctbx/sgtbx/site_symmetry.h>
#include <cctbx/sgtbx/operator_from_axis_direction.h>
#include <scitbx/array_family/shared.h>
#include <boost/python/module.hpp>
#include <boost/python/scope.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <scitbx/boost_python/container_conversions.h>

namespace cctbx { namespace sgtbx { namespace boost_python {

  void wrap_brick();
  void wrap_change_of_basis_op();
  void wrap_find_affine();
  void wrap_lattice_symmetry();
  void wrap_phase_info();
  void wrap_reciprocal_space_asu();
  void wrap_rot_mx();
  void wrap_rt_mx();
  void wrap_search_symmetry();
  void wrap_seminvariant();
  void wrap_site_symmetry();
  void wrap_space_group();
  void wrap_space_group_type();
  void wrap_sym_equiv_sites();
  void wrap_symbols();
  void wrap_tensor_rank_2();
  void wrap_tr_vec();
  void wrap_wyckoff();
  void wrap_select_generators();

namespace {

  struct parse_string_wrappers
  {
    typedef parse_string w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("parse_string", no_init)
        .def(init<std::string const&>((arg("str"))))
        .def("string", &w_t::string)
        .def("where", &w_t::where)
      ;
    }
  };

  struct crystal_system_code_to_string
  {
    static PyObject* convert(crystal_system::code const& c)
    {
      using namespace boost::python;
      return incref(object(crystal_system::label(c)).ptr());
    }
  };

  struct matrix_group_code_to_string
  {
    static PyObject* convert(matrix_group::code const& c)
    {
      using namespace boost::python;
      return incref(object(c.label()).ptr());
    }
  };

  fractional<>
  fractional_mod_positive(
    fractional<> const& site)
  {
    return site.mod_positive();
  }

  fractional<>
  fractional_mod_short(
    fractional<> const& site)
  {
    return site.mod_short();
  }

  void register_tuple_mappings()
  {
    using namespace scitbx::boost_python::container_conversions;

    tuple_mapping_variable_capacity<af::shared<site_symmetry_ops> >();
    tuple_mapping_fixed_capacity<af::small<ss_vec_mod, 3> >();
  }

  void init_module()
  {
    using namespace boost::python;

    sanity_check();

    scope s;
    s.attr("sg_t_den") = sg_t_den;
    s.attr("cb_r_den") = cb_r_den;
    s.attr("cb_t_den") = cb_t_den;

    register_tuple_mappings();

    parse_string_wrappers::wrap();

    to_python_converter<crystal_system::code, crystal_system_code_to_string>();
    to_python_converter<matrix_group::code, matrix_group_code_to_string>();

    def("fractional_mod_positive", fractional_mod_positive, (arg("site")));
    def("fractional_mod_short", fractional_mod_short, (arg("site")));

    wrap_brick();
    wrap_change_of_basis_op();
    wrap_find_affine();
    wrap_lattice_symmetry();
    wrap_phase_info();
    wrap_reciprocal_space_asu();
    wrap_rot_mx();
    wrap_rt_mx();
    wrap_search_symmetry();
    wrap_seminvariant();
    wrap_site_symmetry();
    wrap_space_group();
    wrap_space_group_type();
    wrap_sym_equiv_sites();
    wrap_symbols();
    wrap_tensor_rank_2();
    wrap_tr_vec();
    wrap_wyckoff();
    wrap_select_generators();

    def("n_fold_operator_from_axis_direction",
      n_fold_operator_from_axis_direction, (
        arg("ev_cart"), arg("n"), arg("sense")=1));
  }

} // namespace <anonymous>
}}} // namespace cctbx::sgtbx::boost_python

BOOST_PYTHON_MODULE(cctbx_sgtbx_ext)
{
  cctbx::sgtbx::boost_python::init_module();
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_RIGHT_TRI_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mdivide_left_tri.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/scal/err/domain_error.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the solution of the system Ax=b when A is triangular
     * @param A Triangular matrix.  Specify upper or lower with TriView
     * being Eigen::Upper or Eigen::Lower.
     * @param b Right hand side matrix or vector.
     * @return x = b A^-1, solution of the linear system.
     * @throws std::domain_error if A is not square or the rows of b don't
     * match the size of A.
     */
    template <int TriView, typename T1, typename T2,
              int R1, int C1, int R2, int C2>
    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  R1, C2>
    mdivide_right_tri(const Eigen::Matrix<T1, R1, C1> &b,
                      const Eigen::Matrix<T2, R2, C2> &A) {
      check_square("mdivide_right_tri", "A", A);
      check_multiplicable("mdivide_right_tri", "b", b, "A", A);
      // FIXME: This is nice and general but requires some extra memory
      //        and copying.
      if (TriView == Eigen::Lower) {
        return transpose(mdivide_left_tri<Eigen::Upper>(transpose(A),
                                                        transpose(b)));
      } else if (TriView == Eigen::Upper) {
        return transpose(mdivide_left_tri<Eigen::Lower>(transpose(A),
                                                        transpose(b)));
      }

      domain_error("mdivide_left_tri",
                   "triangular view must be Eigen::Lower or Eigen::Upper",
                   "", "");
    }

  }
}
#endif


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     loops_and_trees.cpp
 * \author   Collin Johnson
 *
 * Definition of label_loops_and_trees function.
 */

#include "hssh/local_topological/area_detection/labeling/loops_and_trees.h"
#include "hssh/local_topological/area_detection/labeling/area_graph.h"
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <iostream>
#include <stack>

#define DEBUG_LOOPS
// #define DEBUG_CONNECTED

namespace vulcan
{
namespace hssh
{

using namespace boost;

using Vertex = graph_traits<LoopGraph>::vertex_descriptor;
using Edge = graph_traits<LoopGraph>::edge_descriptor;

// Define property map for storing the distances between nodes
using DistProperty = exterior_vertex_property<LoopGraph, double>;
using DistVec = DistProperty::container_type;
using DistMatrix = DistProperty::matrix_type;
using DistMap = DistProperty::matrix_map_type;

/*
 * LoopLabellingVisitor is a DFSVisitor for BGL.
 */
struct LoopLabellingVisitor : public default_bfs_visitor
{
    LoopLabellingVisitor(std::vector<Vertex>& pred, std::vector<Vertex>& source, std::vector<Vertex>& target)
    : predecessors(pred)
    , sourcePath(source)
    , targetPath(target)
    {
    }

    // DFSVisitor concept interface
    void initialize_vertex(Vertex v, const LoopGraph& graph);
    void tree_edge(Edge e, const LoopGraph& graph);
    void non_tree_edge(Edge e, const LoopGraph& graph);

    std::vector<Vertex>& predecessors;
    std::vector<Vertex>& sourcePath;
    std::vector<Vertex>& targetPath;
};

void construct_loop_graph(AreaGraph& areaGraph, LoopGraph& loopGraph)
{
    std::unordered_map<AreaNode*, Vertex> nodeToVertex;

    std::for_each(areaGraph.beginNodes(), areaGraph.endNodes(), [&](const auto& node) {
        auto v = add_vertex(loopGraph);
        nodeToVertex[node.get()] = v;
        loopGraph[v].node = node.get();
    });

    std::vector<std::pair<AreaNode*, AreaNode*>> addedEdges;

    std::for_each(areaGraph.beginEdges(), areaGraph.endEdges(), [&](const auto& edge) {
        auto endpoints = edge->getEndpoints();

        for (auto& added : addedEdges) {
            // Ignore previously added edges
            if ((added.first == endpoints[0].get()) && (added.second == endpoints[1].get())) {
                return;
            }
        }

        auto e = add_edge(nodeToVertex[endpoints[0].get()], nodeToVertex[endpoints[1].get()], loopGraph);
        loopGraph[e.first].length = edge->getLength();
        addedEdges.emplace_back(endpoints[0].get(), endpoints[1].get());
        addedEdges.emplace_back(endpoints[1].get(), endpoints[0].get());
    });
}

void label_loops_and_trees(AreaGraph& graph)
{
    LoopGraph loopGraph;
    construct_loop_graph(graph, loopGraph);
    label_loops_and_trees(loopGraph);
}

void label_loops_and_trees(LoopGraph& graph)
{
    std::vector<Vertex> predecessors(num_vertices(graph), num_vertices(graph));
    // storage for extracting the loops
    std::vector<Vertex> sourcePath;
    std::vector<Vertex> targetPath;
    LoopLabellingVisitor vis(predecessors, sourcePath, targetPath);
    predecessors[0] = 0;   // point root to itself

    breadth_first_search(graph, vertex(0, graph), visitor(vis));
    //     undirected_dfs(graph, visitor(vis).edge_color_map(get(&LoopGraphEdge::color, graph)));
}

void compute_node_distances(AreaGraph& graph, const LoopGraph& loopGraph)
{
    DistMatrix distances(graph.sizeNodes());
    DistMap distMap(distances, loopGraph);
    johnson_all_pairs_shortest_paths(loopGraph, distMap, weight_map(get(&LoopGraphEdge::length, loopGraph)));

    for (std::size_t n = 0, end = graph.sizeNodes(); n < end; ++n) {
        AreaNode* startNode = loopGraph[n].node;

        for (std::size_t m = n + 1; m < end; ++m) {
            AreaNode* endNode = loopGraph[m].node;
            graph.setNodeDistance(startNode, endNode, distances[n][m]);
        }
    }
}

void LoopLabellingVisitor::initialize_vertex(Vertex v, const LoopGraph& graph)
{
    // For each vertex, ensure it doesn't have any loops specified
    graph[v].node->setLoop(false);
}

void LoopLabellingVisitor::tree_edge(Edge e, const LoopGraph& graph)
{
    predecessors[target(e, graph)] = source(e, graph);
}

void LoopLabellingVisitor::non_tree_edge(Edge e, const LoopGraph& graph)
{
    // A back edge was found to the target of the edge
    // Go through the predecessors until target the is found.
    // Each node along the way back should be marked as part of a Loop

    auto targetVertex = target(e, graph);

    // Ignore this case. It arises from the BFS just looking at all outedges in the undirected graph, so it
    // sees back to the parent in the search tree when expanded.
    if (predecessors[source(e, graph)] == targetVertex) {
        return;
    }

    auto prevVertex = predecessors[targetVertex];
    auto vertex = targetVertex;

    // Extract the source and target paths
    targetPath.clear();
    while (prevVertex != vertex) {
        targetPath.push_back(vertex);
        prevVertex = vertex;
        vertex = predecessors[vertex];
    }

    vertex = source(e, graph);
    prevVertex = predecessors[vertex];
    sourcePath.clear();
    while (prevVertex != vertex) {
        sourcePath.push_back(vertex);
        prevVertex = vertex;
        vertex = predecessors[vertex];
    }

    // Find the first shared vertex between the two
    auto targetIt = std::find_first_of(targetPath.begin(), targetPath.end(), sourcePath.begin(), sourcePath.end());
    assert(targetIt != targetPath.end());
    auto sourceIt = std::find(sourcePath.begin(), sourcePath.end(), *targetIt);
    assert(sourceIt != sourcePath.end());

    // Turn begin, targetIt into a valid half-open range for a for-loop
    std::size_t targetEnd = std::distance(targetPath.begin(), targetIt) + 1;
    std::size_t sourceEnd = std::distance(sourcePath.begin(), sourceIt) + 1;

    double loopDist = 0.0;
    for (std::size_t n = 1; n < targetEnd; ++n) {
        loopDist += graph[edge(targetPath[n - 1], targetPath[n], graph).first].length;
    }

    for (std::size_t n = 1; n < sourceEnd; ++n) {
        loopDist += graph[edge(sourcePath[n - 1], sourcePath[n], graph).first].length;
    }

#ifdef DEBUG_LOOPS
    std::cout << "DEBUG:label_loops_and_trees: Found a loop with distance " << loopDist << " : \n";
#endif

    for (std::size_t n = 0; n < targetEnd; ++n) {
        graph[targetPath[n]].node->setLoop(true);
        graph[targetPath[n]].node->setLoopDistance(loopDist);

#ifdef DEBUG_LOOPS
        std::cout << graph[targetPath[n]].node->getPosition() << '\n';
#endif   // DEBUG_LOOPS
    }

    for (std::size_t n = 0; n < sourceEnd; ++n) {
        graph[sourcePath[n]].node->setLoop(true);
        graph[sourcePath[n]].node->setLoopDistance(loopDist);

#ifdef DEBUG_LOOPS
        std::cout << graph[sourcePath[n]].node->getPosition() << '\n';
#endif   // DEBUG_LOOPS
    }
}

}   // namespace hssh
}   // namespace vulcan


//
//  Copyright Toon Knapen, Karl Meerbergen
//  Copyright Thomas Klimpel 2008
//
// Distributed under 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 "ublas_heev.hpp"

#include <boost/numeric/bindings/lapack/driver/heevx.hpp>
#include <boost/numeric/bindings/lapack/driver/syevx.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/mpl/if.hpp>

#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

template <typename T, typename W, typename UPLO>
int do_memory_uplo(int n, W& workspace)
{
  typedef typename bindings::remove_imaginary<T>::type real_type ;

  typedef ublas::matrix<T, ublas::column_major>     matrix_type ;
  typedef ublas::vector<real_type>                  vector_type ;

  typedef ublas::hermitian_adaptor<matrix_type, UPLO> hermitian_type;

  // Set matrix
  matrix_type a(n, n);
  a.clear();
  vector_type e1(n);
  vector_type e2(n);

  fill(a);
  matrix_type a2(a);
  matrix_type z(a);

  // Compute Schur decomposition.
  fortran_int_t m;
  ublas::vector<fortran_int_t> ifail(n);

  hermitian_type h_a(a);
  lapack::heevx('V', 'A', h_a, real_type(0.0), real_type(1.0), 2, n-1, real_type(1e-28), m,
                e1, z, ifail, workspace) ;

  if(check_residual(a2, e1, z)) return 255 ;

  hermitian_type h_a2(a2);
  lapack::heevx('N', 'A', h_a2, real_type(0.0), real_type(1.0), 2, n-1, real_type(1e-28), m,
                e2, z, ifail, workspace) ;
  if(norm_2(e1 - e2) > n * norm_2(e1) * std::numeric_limits< real_type >::epsilon()) return 255 ;

  // Test for a matrix range
  fill(a);
  a2.assign(a);

  typedef ublas::matrix_range< matrix_type > matrix_range ;
  typedef ublas::hermitian_adaptor<matrix_range, UPLO> hermitian_range_type;

  ublas::range r(1,n-1) ;
  matrix_range a_r(a, r, r);
  matrix_range z_r(z, r, r);
  ublas::vector_range< vector_type> e_r(e1, r);
  ublas::vector<fortran_int_t> ifail_r(n-2);

  hermitian_range_type h_a_r(a_r);
  lapack::heevx('V', 'A', h_a_r, real_type(0.0), real_type(1.0), 2, n-1, real_type(1e-28), m,
                e_r, z_r, ifail_r, workspace);

  matrix_range a2_r(a2, r, r);
  if(check_residual(a2_r, e_r, z_r)) return 255 ;

  return 0 ;
} // do_memory_uplo()

template <typename T, typename W>
int do_memory_type(int n, W workspace)
{
  std::cout << "  upper\n" ;
  if(do_memory_uplo<T,W,ublas::upper>(n, workspace)) return 255 ;
  std::cout << "  lower\n" ;
  if(do_memory_uplo<T,W,ublas::lower>(n, workspace)) return 255 ;
  return 0 ;
}

template <typename T>
struct Workspace
{
  typedef ublas::vector< T >                                       array_type ;
  typedef ublas::vector< fortran_int_t >                               int_array_type ;
  typedef lapack::detail::workspace2< array_type, int_array_type > type ;

  Workspace(size_t n)
    : work_(8*n)
    , iwork_(5*n)
  {}

  type operator()()
  {
    return lapack::workspace(work_, iwork_) ;
  }

  array_type work_ ;
  int_array_type iwork_ ;
};

template <typename T>
struct Workspace< std::complex<T> >
{
  typedef ublas::vector< std::complex<T> >                 complex_array_type ;
  typedef ublas::vector< T >                               real_array_type ;
  typedef ublas::vector< fortran_int_t >                       int_array_type ;
  typedef lapack::detail::workspace3<
  complex_array_type, real_array_type, int_array_type > type ;

  Workspace(size_t n)
    : work_(2*n)
    , rwork_(7*n)
    , iwork_(5*n)
  {}

  type operator()()
  {
    return lapack::workspace(work_, rwork_, iwork_) ;
  }

  complex_array_type work_ ;
  real_array_type    rwork_ ;
  int_array_type     iwork_ ;
};

template <typename T>
int do_value_type()
{
  const int n = 8 ;

  std::cout << " optimal workspace\n";
  if(do_memory_type<T,lapack::optimal_workspace>(n, lapack::optimal_workspace())) return 255 ;

  std::cout << " minimal workspace\n";
  if(do_memory_type<T,lapack::minimal_workspace>(n, lapack::minimal_workspace())) return 255 ;

  std::cout << " workspace array\n";
  Workspace<T> work(n);
  if(do_memory_type<T,typename Workspace<T>::type >(n, work())) return 255 ;
  return 0;
} // do_value_type()

int main()
{
  // Run tests for different value_types
  std::cout << "float\n" ;
  if(do_value_type< float >()) return 255;

  std::cout << "double\n" ;
  if(do_value_type< double >()) return 255;

  std::cout << "complex<float>\n" ;
  if(do_value_type< std::complex<float> >()) return 255;

  std::cout << "complex<double>\n" ;
  if(do_value_type< std::complex<double> >()) return 255;

  std::cout << "Regression test succeeded\n" ;
  return 0;
}