//
// Created by cc on 2020/8/5.
//

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>

using namespace Eigen;

Vector3d current_p;
mavros_msgs::State current_state;
ros::Publisher pva_pub;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    /// ENU frame to NWU
    current_p << msg->pose.position.y, -msg->pose.position.x, msg->pose.position.z;
}

void stateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

void setPVA(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    pva_setpoint.positions.push_back(p(0)); //x
    pva_setpoint.positions.push_back(p(1)); //y
    pva_setpoint.positions.push_back(p(2)); //z
    pva_setpoint.positions.push_back(yaw);

    pva_setpoint.velocities.push_back(v(0));
    pva_setpoint.velocities.push_back(v(1));
    pva_setpoint.velocities.push_back(v(2));

    pva_setpoint.accelerations.push_back(a(0));
    pva_setpoint.accelerations.push_back(a(1));
    pva_setpoint.accelerations.push_back(a(2));

    pva_pub.publish(pva_setpoint);

//    ROS_INFO_THROTTLE(1.0, "P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO_THROTTLE(1.0, "V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO_THROTTLE(1.0, "A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);

    ROS_INFO("P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
    ROS_INFO("V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
    ROS_INFO("A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
}

/** This function is to generate state to state trajectory **/
void motion_primitives(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0,
                       Eigen::Vector3d pf, Eigen::Vector3d vf, Eigen::Vector3d af, double v_max, double delt_t,
                       Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    // % Choose the time as running in average velocity
    // double decay_parameter = 0.5;
    // double T = 0.2;

    double j_limit = 5;
    double a_limit = 3;
    double v_limit = v_max;

//    double T1 = fabs(af(0)-a0(0))/j_limit > fabs(af(1)-a0(1))/j_limit ? fabs(af(0)-a0(0))/j_limit : fabs(af(1)-a0(1))/j_limit;
//    T1 = T1 > fabs(af(2)-a0(2))/j_limit ? T1 : fabs(af(2)-a0(2))/j_limit;
    double T2 = fabs(vf(0)-v0(0))/a_limit > fabs(vf(1)-v0(1))/a_limit ? fabs(vf(0)-v0(0))/a_limit : fabs(vf(1)-v0(1))/a_limit;
    T2 = T2 > fabs(vf(2)-v0(2))/a_limit ? T2 : fabs(vf(2)-v0(2))/a_limit;
    double T3 = fabs(pf(0)-p0(0))/v_limit > fabs(pf(1)-p0(1))/v_limit ? fabs(pf(0)-p0(0))/v_limit : fabs(pf(1)-p0(1))/v_limit;
    T3 = T3 > fabs(pf(2)-p0(2))/v_limit ? T3 : fabs(pf(2)-p0(2))/v_limit;

//    double T = T1 > T2 ? T1 : T2;
//    T = T > T3 ? T : T3;
    double T = T2;
    T = T > T3 ? T : T3;
    T = T < 0.3 ? 0.3 : T;

//    ROS_INFO_THROTTLE(2, "T=%lf", T);

    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller
    for(int ii=0; ii<3; ii++)
    {
        double delt_a = af(ii) - a0(ii);
        double delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        double delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        //%  if vf is not free
        double alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        double beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        double gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            double tt = (jj + 1)*delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);
        }
    }
}

void compute_circular_traj(const double r, const double vel, const Eigen::Vector3d p0, const double t,
                           Eigen::Vector3d &p, Eigen::Vector3d &v, Eigen::Vector3d &a)
//@requires r > 0 && vel > 0 && t >= 0;
{
    const double theta = vel*t/r;

    p(0) = r*cos(theta) + p0(0) - r;
    p(1) = r*sin(theta) + p0(1);
    p(2) = p0(2);

    v(0) = -vel*sin(theta);
    v(1) = vel*cos(theta);
    v(2) = 0;

    a(0) = -vel*vel/r*cos(theta);
    a(1) = -vel*vel/r*sin(theta);
    a(2) = 0;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "control");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, stateCallback);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, positionCallback);

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    const int LOOPRATE = 40;
    ros::Rate loop_rate(LOOPRATE);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        loop_rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    /// Take off with constant acceleration
    double delt_t = 1.0 / LOOPRATE;
    double yaw_target = M_PI / 2.0;

    // Vector3d recorded_position = current_p;
    // while(ros::ok()){
    //     if(current_state.mode != "OFFBOARD" || !current_state.armed){
    //         setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
    //         recorded_position = current_p;
    //     }else{
    //         setPVA(recorded_position, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    Vector3d circle_p0 = current_p;
    while(ros::ok()){
        if(current_state.mode != "OFFBOARD" || !current_state.armed){
            setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
            circle_p0 = current_p;
        }else{
            /** Accelerate period **/
            double circle_speed = 0.8;
            double circle_radius = 0.6;
            double acc_t_total = 2 * circle_radius / circle_speed;
            int acc_times = acc_t_total / delt_t;
            double acc_a_value = circle_speed * circle_speed / 2 / circle_radius;
            circle_p0 = current_p;

            for(int i=0; i<acc_times; i++){
                Eigen::Vector3d p, v ,a;

                p << circle_p0(0), circle_p0(1) + 0.5 * acc_a_value * (i * delt_t) * (i * delt_t), circle_p0(2);
                v << 0.0, acc_a_value * i * delt_t, 0.0;
                a << 0.0, acc_a_value, 0.0;
                setPVA(p, v, a, yaw_target);

                loop_rate.sleep();
                ros::spinOnce();
            }
            ROS_WARN("Accelerate Complete!");

            /** Now draw circle **/
            double init_t = 0.0;
            Vector3d circle_start_point = circle_p0;
            circle_start_point(1) += circle_radius;

            while(ros::ok()){

                Eigen::Vector3d p, v ,a;
                compute_circular_traj(circle_radius, circle_speed, circle_start_point, init_t, p, v, a);
                setPVA(p, v, a, yaw_target);//a_t.row(last_index));
                init_t += delt_t;

                if(current_state.mode != "OFFBOARD" || !current_state.armed){
                    break;
                }

                loop_rate.sleep();
                ros::spinOnce();
            }

        }
        loop_rate.sleep();
        ros::spinOnce();
    }

    // ROS_INFO("Arm and takeoff");
    // while(ros::ok()){
    //     if( current_state.mode != "OFFBOARD" &&
    //         (ros::Time::now() - last_request > ros::Duration(5.0))){
    //         if( set_mode_client.call(offb_set_mode) &&
    //             offb_set_mode.response.mode_sent){
    //             ROS_INFO("Offboard enabled");
    //         }
    //         last_request = ros::Time::now();
    //     } else {
    //         if( !current_state.armed &&
    //             (ros::Time::now() - last_request > ros::Duration(5.0))){
    //             if( arming_client.call(arm_cmd) &&
    //                 arm_cmd.response.success){
    //                 ROS_INFO("Vehicle armed");
    //             }
    //             last_request = ros::Time::now();
    //         }
    //     }

    //     trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    //     if(current_state.mode != "OFFBOARD" || !current_state.armed){
    //         setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), M_PI);
    //     }else{
    //         counter ++;
    //         double z_sp, vz_sp;
    //         if(counter < take_off_send_times / 2){
    //             z_sp = 0.5*take_off_acc*counter*delt_t*counter*delt_t;
    //             vz_sp = counter*delt_t*take_off_acc;
    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), z_sp);
    //             Vector3d v_sp(0, 0, vz_sp);
    //             setPVA(p_sp, v_sp, Vector3d::Zero(), M_PI);

    //         }else if(counter < take_off_send_times){
    //             double t_this = (counter-take_off_send_times/2)*delt_t;
    //             z_sp = take_off_send_times/2*delt_t*take_off_acc*t_this - 0.5*take_off_acc*t_this*t_this;
    //             vz_sp = take_off_send_times/2*delt_t*take_off_acc - take_off_acc*t_this;

    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), z_sp);
    //             Vector3d v_sp(0, 0, vz_sp);
    //             setPVA(p_sp, v_sp, Vector3d::Zero(), M_PI);

    //         }else{
    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), take_off_height);
    //             setPVA(p_sp, Vector3d::Zero(), Vector3d::Zero(), M_PI);
    //             counter --;
    //         }
    //     }

    //     if(current_p(2) > take_off_height-0.05){
    //         ROS_WARN("Takeoff Complete!");
    //         break;
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // /** Take off complete. Go to a point with minimum jerk trajectory **/
    // double circle_radius = 2;

    // MatrixXd p_t, v_t, a_t;
    // Eigen::VectorXd t_vector;
    // Vector3d v0(0.0, 0.0, 0.0);
    // Vector3d a0(0.0, 0.0, 0.0);

    // Vector3d pf(circle_radius, 0, take_off_height);
    // Vector3d vf(0, 0, 0);
    // Vector3d af(0, 0, 0);

    // motion_primitives(current_p, v0, a0, pf, vf, af, 3.0, delt_t, p_t, v_t, a_t, t_vector);

    // for(int i=0; i<t_vector.size(); i++)
    // {
    //     setPVA(p_t.row(i), v_t.row(i), Vector3d::Zero());// a_t.row(i));
    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // while(ros::ok())
    // {
    //     setPVA(p_t.row(t_vector.size()-1), v_t.row(t_vector.size()-1), Vector3d::Zero());// a_t.row(i));
    //     Vector3d last_sp_p = p_t.row(t_vector.size()-1);
    //     Vector3d delt_p = last_sp_p - current_p;
    //     if(delt_p.norm() < 0.2){
    //         ROS_WARN("Align Complete!");
    //         break;
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // /** Now draw circle **/
    // Vector3d circle_p0 = current_p;
    // double init_t = 0.0;
    // while(ros::ok()){
    //     Eigen::Vector3d p, v ,a;
    //     compute_circular_traj(circle_radius, 5.0, circle_p0, init_t, p, v, a);
    //     setPVA(p, v, a);//a_t.row(last_index));

    //     init_t += delt_t;
    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    return 0;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout; using std::endl; using std::cerr;

//[policy_eg_9

/*`
The previous example was all well and good, but the custom error handlers
didn't really do much of any use.  In this example we'll implement all
the custom handlers and show how the information provided to them can be
used to generate nice formatted error messages.

Each error handler has the general form:

   template <class T>
   T user_``['error_type]``(
      const char* function,
      const char* message,
      const T& val);

and accepts three arguments:

[variablelist
[[const char* function]
   [The name of the function that raised the error, this string
   contains one or more %1% format specifiers that should be
   replaced by the name of real type T, like float or double.]]
[[const char* message]
   [A message associated with the error, normally this
   contains a %1% format specifier that should be replaced with
   the value of ['value]: however note that overflow and underflow messages
   do not contain this %1% specifier (since the value of ['value] is
   immaterial in these cases).]]
[[const T& value]
   [The value that caused the error: either an argument to the function
   if this is a domain or pole error, the tentative result
   if this is a denorm or evaluation error, or zero or infinity for
   underflow or overflow errors.]]
]

As before we'll include the headers we need first:

*/

#include <boost/math/special_functions.hpp>

/*`
Next we'll implement our own error handlers for each type of error,
starting with domain errors:
*/

namespace boost{ namespace math{
namespace policies
{

template <class T>
T user_domain_error(const char* function, const char* message, const T& val)
{
   /*`
   We'll begin with a bit of defensive programming in case function or message are empty:
   */
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Cause unknown with bad argument %1%";
   /*`
   Next we'll format the name of the function with the name of type T, perhaps double:
   */
   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();
   /*`
   Then likewise format the error message with the value of parameter /val/,
   making sure we output all the potentially significant digits of /val/:
   */
   msg += ": \n";
   int prec = 2 + (std::numeric_limits<T>::digits * 30103UL) / 100000UL;
   // int prec = std::numeric_limits<T>::max_digits10; //  For C++0X Standard Library
   msg += (boost::format(message) % boost::io::group(std::setprecision(prec), val)).str();
   /*`
   Now we just have to do something with the message, we could throw an
   exception, but for the purposes of this example we'll just dump the message
   to std::cerr:
   */
   std::cerr << msg << std::endl;
   /*`
   Finally the only sensible value we can return from a domain error is a NaN:
   */
   return std::numeric_limits<T>::quiet_NaN();
}

/*`
Pole errors are essentially a special case of domain errors,
so in this example we'll just return the result of a domain error:
*/

template <class T>
T user_pole_error(const char* function, const char* message, const T& val)
{
   return user_domain_error(function, message, val);
}

/*`
Overflow errors are very similar to domain errors, except that there's
no %1% format specifier in the /message/ parameter:
*/
template <class T>
T user_overflow_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is too large to represent";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is an infinity, just return it:
   return val;
}

/*`
Underflow errors are much the same as overflow:
*/

template <class T>
T user_underflow_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is too small to represent";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is zero, just return it:
   return val;
}

/*`
Denormalised results are much the same as underflow:
*/

template <class T>
T user_denorm_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is denormalised";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is denormalised, just return it:
   return val;
}

/*`
Which leaves us with evaluation errors: these occur when an internal
error occurs that prevents the function being fully evaluated.
The parameter /val/ contains the closest approximation to the result
found so far:
*/

template <class T>
T user_evaluation_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "An internal evaluation error occurred with "
                  "the best value calculated so far of %1%";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   int prec = 2 + (std::numeric_limits<T>::digits * 30103UL) / 100000UL;
   // int prec = std::numeric_limits<T>::max_digits10; // For C++0X Standard Library
   msg += (boost::format(message) % boost::io::group(std::setprecision(prec), val)).str();

   std::cerr << msg << std::endl;

   // What do we return here?  This is generally a fatal error, that should never occur,
   // so we just return a NaN for the purposes of the example:
   return std::numeric_limits<T>::quiet_NaN();
}

} // policies
}} // boost::math

/*`
Now we'll need to define a suitable policy that will call these handlers,
and define some forwarding functions that make use of the policy:
*/

namespace mymath
{ // unnamed.

using namespace boost::math::policies;

typedef policy<
   domain_error<user_error>,
   pole_error<user_error>,
   overflow_error<user_error>,
   underflow_error<user_error>,
   denorm_error<user_error>,
   evaluation_error<user_error>
> user_error_policy;

BOOST_MATH_DECLARE_SPECIAL_FUNCTIONS(user_error_policy)

} // unnamed namespace

/*`
We now have a set of forwarding functions, defined in namespace mymath,
that all look something like this:

``
template <class RealType>
inline typename boost::math::tools::promote_args<RT>::type
   tgamma(RT z)
{
   return boost::math::tgamma(z, user_error_policy());
}
``

So that when we call `mymath::tgamma(z)` we really end up calling
`boost::math::tgamma(z, user_error_policy())`, and any
errors will get directed to our own error handlers:
*/

int main()
{
   // Raise a domain error:
   cout << "Result of erf_inv(-10) is: "
      << mymath::erf_inv(-10) << std::endl << endl;
   // Raise a pole error:
   cout << "Result of tgamma(-10) is: "
      << mymath::tgamma(-10) << std::endl << endl;
   // Raise an overflow error:
   cout << "Result of tgamma(3000) is: "
      << mymath::tgamma(3000) << std::endl << endl;
   // Raise an underflow error:
   cout << "Result of tgamma(-190.5) is: "
      << mymath::tgamma(-190.5) << std::endl << endl;
   // Unfortunately we can't predicably raise a denormalised
   // result, nor can we raise an evaluation error in this example
   // since these should never really occur!
} // int main()

/*`

Which outputs:

[pre
Error in function boost::math::erf_inv<double>(double, double):
Argument outside range \[-1, 1\] in inverse erf function (got p=-10).
Result of erf_inv(-10) is: 1.#QNAN

Error in function boost::math::tgamma<long double>(long double):
Evaluation of tgamma at a negative integer -10.
Result of tgamma(-10) is: 1.#QNAN

Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too large to represent.
Error in function boost::math::tgamma<double>(double):
Result of function is too large to represent
Result of tgamma(3000) is: 1.#INF

Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too large to represent.
Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too small to represent.
Result of tgamma(-190.5) is: 0
]

Notice how some of the calls result in an error handler being called more
than once, or for more than one handler to be called: this is an artefact
of the fact that many functions are implemented in terms of one or more
sub-routines each of which may have it's own error handling.  For example
`tgamma(-190.5)` is implemented in terms of `tgamma(190.5)` - which overflows -
the reflection formula for `tgamma` then notices that it is dividing by
infinity and so underflows.
*/

//] //[/policy_eg_9]


#ifndef TINYMT64SEARCH_HPP
#define TINYMT64SEARCH_HPP
/**
 * @file tinymt64search.hpp
 *
 * @brief Tiny Mersenne Twister only 127 bit internal state,
 * this class is used by tinymt32dc.
 *
 * This file is important. Users should not change this file,
 * except they are experts in random number generation.
 * This file is used for parameter searching.
 * For random number generation, use tinymt64c.c and tinymt64c.h.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */

#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cerrno>
#include <sstream>
#include <unistd.h>
#include <NTL/GF2X.h>
#include <search_temper.hpp>

namespace tinymt {
    using namespace MTToolBox;
    using namespace NTL;
    using namespace std;

    class tinymt64;
    //typedef search_temper<tinymt64, uint64_t, 64, 1, 32, 9> st64;
    /**
     * tempering parameter search algorithm
     * - generator is tinymt64
     * - generator's output type is uint64_t
     * - bits in output is 64
     * - size of tempering parameter array is 1
     * - tempering 52 bit from MSB
     * - tempering parameter is searched every 6 bits
     */

    typedef search_temper<tinymt64, uint64_t, 64, 1, 52, 6> st64;
    /**
     * tempering parameter search algorithm
     * - generator is tinymt64
     * - generator's output type is uint64_t
     * - bits in output is 64
     * - size of tempering parameter array is 1
     * - tempering 12 bit from LSB
     * - tempering parameter is searched every 6 bits
     */
    typedef search_temper<tinymt64, uint64_t, 64, 1, 12, 6, true> stlsb64;

    /**
     * @class tinymt64_param
     * @brief a class keeping parameters of tinymt64
     *
     * This class keeps parameters of tinymt64, and has some
     * method for outputting parameters.
     */
    class tinymt64_param {
    public:
	uint32_t id;
	uint32_t mat1;
	uint32_t mat2;
	uint64_t tmat[1];
	/**
	 * This method is used in output.hpp.
	 * @return header line of output.
	 */
	string get_header() {
	    return "id, mat1, mat2, tmat";
	};

	/**
	 * This method is used in output.hpp.
	 * @return string of parameters
	 */
	string get_string() {
	    stringstream ss;
	    ss << dec << id << ",";
	    ss << hex << setw(8) << setfill('0') << mat1 << ",";
	    ss << hex << setw(8) << setfill('0') << mat2 << ",";
	    ss << hex << setw(16) << setfill('0') << tmat[0] << ",";
	    string s;
	    ss >> s;
	    return s;
	};

	/**
	 * This method is used for DEBUG.
	 * @return string of parameters.
	 */
	string get_debug_string() {
	    stringstream ss;
	    ss << "id:" << dec << id << endl;
	    ss << "mat1:" << hex << setw(8) << setfill('0') << mat1 << endl;
	    ss << "mat2:" << hex << setw(8) << setfill('0') << mat2 << endl;
	    ss << "tmat:" << hex << setw(16) << setfill('0') << tmat[0] << endl;
	    string s;
	    ss >> s;
	    return s;
	};
    };

    /**
     * @class tinymt64
     * @brief 64-bit tinymt generator class used for dynamic creation
     *
     * This class is one of the main class of tinymt dynamic creator.
     * This class is designed to be called from programs in MTToolBox,
     * but is not a subclass of some abstract class.
     * Instead, this class is passed to them as template parameters.
     */
    class tinymt64 {
    public:
	/**
	 * Constructor by id.
	 * @param id The number specified by a user.
	 */
	tinymt64(uint32_t id) {
	    param.id = id;
	    param.mat1 = 0;
	    param.mat2 = 0;
	    param.tmat[0] = 0;
	    reverse_bit_flag = false;
	};

	/**
	 * The copy constructor.
	 * @param src The origin of copy.
	 */
	tinymt64(const tinymt64& src) :
	    param(src.param) {
	    reverse_bit_flag = false;
	};

	/**
	 * Constructor by parameter.
	 * @param src_param
	 */
	tinymt64(const tinymt64_param& src_param) : param(src_param) {
	    reverse_bit_flag = false;
	};

	/**
	 * This method is called by the functions in simple_shortest_basis.hpp
	 */
	void make_zero_status() {
	    status[0] = 0;
	    status[1] = 0;
	};

	/**
	 * This method always returns 127
	 * @return always 127
	 */
	int get_mexp() const {
	    return mexp;
	};

	/**
	 * This method always returns 2
	 * @return always 2
	 */
	int get_status_size() const {
	    return status_size;
	};

	/**
	 * This method initialize internal state.
	 * This initialization is simple.
	 * @param seed seed for initialization
	 */
	void seeding(uint64_t seed) {
	    status[0] = 0;
	    status[1] = seed;
	};

	/**
	 * Important state transition function.
	 */
	void next_state() {
	    uint64_t x;
	    status[0] &= UINT64_C(0x7fffffffffffffff);
	    x = status[0] ^ status[1];
	    x ^= x << sh0;
	    x ^= x >> 32;
	    x ^= x << 32;
	    x ^= x << sh1;
	    status[0] = status[1];
	    status[1] = x;
	    if (x & 1) {
		status[0] ^= static_cast<uint64_t>(param.mat1);
		status[1] ^= static_cast<uint64_t>(param.mat2) << 32;
	    }
	};

	/**
	 * get a part of internal state without tempering
	 * @return
	 */
	uint64_t get_uint() {
	    return status[0];
	};

	/**
	 * getter of status
	 * @param index index of internal status
	 * @return element of internal status at \b index
	 */
	uint64_t get_status(int index) {
	    return status[index];
	};

	/**
	 * setter of status
	 * @param value new status value
	 */
	void set_status(uint64_t value[2]) {
	    status[0] = value[0];
	    status[1] = value[1];
	};

	/**
	 * Important method, generate new random number
	 * @return new pseudo random number
	 */
	uint64_t generate() {
	    next_state();
	    return temper();
	};

	/**
	 * This method is called by the functions in simple_shortest_basis.hpp
	 * This method returns \b bit_len bits of MSB of generated numbers
	 * If reverse_bit_flag is set, bits are taken from LSB
	 * @param bit_len bit length from MSB or LSB
	 * @return generated numbers of bit_len
	 */
	uint64_t generate(int bit_len) {
	    uint64_t u;
	    if (reverse_bit_flag) {
		u = reverse_bit(generate());
	    } else {
		u = generate();
	    }
	    uint64_t mask = 0;
	    mask = (~mask) << (64 - bit_len);
	    return u & mask;
	};

	/**
	 * make parameters from given sequential number and
	 * internal id
	 * @param num sequential number
	 */
	void setup_param(uint32_t num) {
	    uint32_t work = num ^ (num << 15) ^ (num << 23);
	    param.mat1 = (work & 0xffff0000) | (param.id & 0xffff);
	    param.mat2 = (work & 0xffff) | (param.id & 0xffff0000);
	    param.mat1 ^= param.mat1 >> 19;
	    param.mat2 ^= param.mat2 << 18;
	    param.tmat[0] = 0;
	};

	/**
	 * getter of parameter
	 * @return parameter
	 */
	const tinymt64_param& get_param() const {
	    return param;
	};

	/**
	 * This method gives information to the functions in the file
	 * search_temper.hpp
	 * @return always 1
	 */
	int get_temper_param_num() const {
	    return 1;
	};

	/**
	 * This method is called by the functions in the file
	 * simple_shortest_basis.hpp
	 * @return true if all elements of status is zero
	 */
	bool is_zero() {
	    return (status[0] == 0) && (status[1] == 0);
	};

	/**
	 * This is important,
	 * This function tries to improve output quality of randomness.
	 * One important point of tinymt is this tempering function,
	 * which is not GF(2)-linear.
	 * But in calculating parameter phase, NON_LINEAR is never defined.
	 * @return improved random number
	 */
	uint64_t temper() {
#if defined(NO_TEMPER)
	    return status[0];
#else
	    uint64_t x;
#if defined(NON_LINEAR)
	    x = status[0] + status[1];
#else
	    x = status[0] ^ status[1];
#endif
	    x ^= status[0] >> sh8;
	    if (x & 1) {
		x ^= param.tmat[0];
	    }
	    return x;
#endif
	};

	/**
	 * This method is called by functions in the file search_temper.hpp
	 * @param mask available bits of pattern
	 * @param pattern bit pattern
	 * @param src_bit only 0 is allowed
	 */
        void set_temper_pattern(uint64_t mask, uint64_t pattern, int src_bit) {
            param.tmat[src_bit] &= ~mask;
            param.tmat[src_bit] |= pattern & mask;
        };

	/**
	 * This method is called by functions in the file
	 * simple_shortest_basis.hpp addition of internal state as
	 * GF(2) vector is possible when state transition function and
	 * output function is GF(2)-linear.
	 * @param that tinymt generator added to this generator
	 */
	void add(tinymt64& that) {
	    status[0] ^= that.status[0];
	    status[1] ^= that.status[1];
	};

	/**
	 * This method is called by functions in the file search_temper.hpp
	 * Do not remove this.
	 */
	void setup_temper() {
	};

	/**
	 * setter of parameter
	 * @param src new parameter
	 */
	void set_param(const tinymt64_param& src) {
	    param = src;
	};

	/**
	 * output parameters
	 * @param out output stream
	 */
	void out_param(ostream& out) {
	    string s = param.get_debug_string();
	    out << s << endl;
	};

	/**
	 * This method is called by the functions in search_temper.hpp
	 * to calculate the equidistribution properties from LSB
	 */
	void set_reverse_bit() {
	    reverse_bit_flag = true;
	};

	/**
	 * This method is called by the functions in search_temper.hpp
	 * to reset the reverse_bit_flag
	 */
	void reset_reverse_bit() {
	    reverse_bit_flag = false;
	};
    private:
	enum {sh0 = 12, sh1 = 11, sh8=8, status_size = 4, mexp = 127};
	uint64_t status[2];
	tinymt64_param param;
	bool reverse_bit_flag;
    };
}

#endif


#include <Eigen/Core>
#include <tuple>
#include <libv/core/time.hpp>
#include <libv/lma/ttt/traits/unroll1.hpp>
#include <libv/lma/ttt/traits/unroll2.hpp>

template<class A, class B, class C> void compute(A& a, const B& b, const C& c)
{
  a = b * c;
}

template<class ... Args> void compute(std::tuple<Args...> tuple)
{
  compute(std::get<0>(tuple),std::get<1>(tuple),std::get<2>(tuple));
}

template<int I, int J> auto statique()
{
  typedef Eigen::Matrix<double,I,I> MatII;
  typedef Eigen::Matrix<double,I,J> MatIJ;
  typedef Eigen::Matrix<double,J,I> MatJI;
  return std::tuple<MatII,MatIJ,MatJI>(MatII::Zero(),MatIJ::Random(),MatJI::Random());
}

template<int I, int J> auto dynamique()
{
  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatII;
  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatIJ;
  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatJI;
  return std::tuple<MatII,MatIJ,MatJI>(MatII::Zero(I,I),MatIJ::Random(I,J),MatJI::Random(J,I));
}

struct Test
{
  template<int I, int J> void operator()() const
  {
    size_t n = 100000;

    double t = v::now();
    for(size_t i = 0 ; i < n ; ++i)
      compute(statique<I+1,J+1>());
    t = v::now() - t;
    std::cout << " Statique <" << I+1 << "," << J+1 << ">  : " << t << std::endl;

    t = v::now();
    for(size_t i = 0 ; i < n ; ++i)
      compute(dynamique<I+1,J+1>());
    t = v::now() - t;
    std::cout << " Dynamique<" << I+1 << "," << J+1 << ">  : " << t << std::endl;
    std::cout << std::endl;
  }

  template<size_t I> void operator()(ttt::Int<I>) const
  {
    static const int J = I;
    size_t n = 100000;

    double t = v::now();
    for(size_t i = 0 ; i < n ; ++i)
      compute(statique<I,J>());
    t = v::now() - t;
    std::cout << " Statique <" << I << "," << J << ">  : " << t << std::endl;

    t = v::now();
    for(size_t i = 0 ; i < n ; ++i)
      compute(dynamique<I,J>());
    t = v::now() - t;
    std::cout << " Dynamique<" << I << "," << J << ">  : " << t << std::endl;
    std::cout << std::endl;
  }

};

using namespace ttt;

int main()
{
  //double_unroll(Int<16>{},Int<16>{},Test{});
  unroll(Test{},Int<1>{},Int<32>{});
  return 0;
}


#ifndef NTFMT_FLOAT_HPP_
#define NTFMT_FLOAT_HPP_

#include "ntfmt_fwd.hpp"
#include "ntfmt.hpp"

#ifndef NTFMT_PRINT_FLOAT_BUFFER_SIZE
#ifdef BOOST_PLATFORM_CONFIG
#define NTFMT_PRINT_FLOAT_BUFFER_SIZE std::numeric_limits<T>::max_exponent
#else
#define NTFMT_PRINT_FLOAT_BUFFER_SIZE 24
#endif
#endif

#include <string.h>
#include <float.h>
#include <cmath>
#ifndef NTFMT_NO_BOOST
#include <boost/integer.hpp>
#include <boost/type_traits/integral_promotion.hpp>
#endif

namespace ntfmt {
	namespace detail {
#ifdef BOOST_DINKUMWARE_STDLIB
		using ::fabs;
		using ::floor;
		using ::ceil;
		using ::fmod;
		using ::log;
		using ::log10;
		using ::pow;
		inline bool isinf(double d) { return !::_finite(d); }
		inline bool isinf(long double d) { return !::_finite((double)d); }
		inline bool isnan(double d) { return !!::_isnan(d); }
		inline bool isnan(long double d) { return !!::_isnan((double)d); }
		inline double nextafter(double x, double y) { return ::_nextafter(x, y); }
		inline long double nextafter(long double x, long double y) { return ::_nextafter((double)x, (double)y); }
		inline double copysign(double x) { return ::_copysign(x); }
		inline long double copysign(long double x) { return ::_copysign((double)x); }
#else
		using std::fabs;
		using std::floor;
		using std::ceil;
		using std::isinf;
		using std::isnan;
		using std::fmod;
		using std::log;
		using std::log10;
		using std::pow;
		using ::copysign;
		using ::nextafter;
#endif

		template <typename charT>
		inline charT *gstrncpy(charT *const, charT const *const, size_t);
		template <>
		inline char *gstrncpy<char>(char *const dst, char const *const src, size_t lim) { return strncpy(dst, src, lim); }
		template <>
		inline wchar_t *gstrncpy<wchar_t>(wchar_t *const dst, wchar_t const *const src, size_t lim) { return wcsncpy(dst, src, lim); }

		template <typename T>
		inline T get_predecessor(T const &v) { return nextafter(v, -numeric_limits<T>::infinity()); }
		template <typename T>
		inline T get_successor(T const &v) { return nextafter(v, +numeric_limits<T>::infinity()); }

		template <typename charT, typename FloatT>
		inline charT to_hexstr(FloatT const v, unsigned const base, bool const capital, typename enable_if< is_floating_point<typename decay<FloatT>::type> >::type * = 0) {
			return hexstr<charT>::str(capital)[static_cast<unsigned>(fmod(v, static_cast<FloatT>(base)))];
		}

		template <typename charT> struct nan_str;
		template <>
		struct nan_str<char> {
			static char const (&str(bool const capital))[4] {
				static char const s[][4] = { "nan", "NAN" };
				return s[capital];
			}
		};
		template <>
		struct nan_str<wchar_t> {
			static wchar_t const (&str(bool const capital))[4] {
				static wchar_t const s[][4] = { L"nan", L"NAN" };
				return s[capital];
			}
		};
		template <typename charT> struct inf_str;
		template <>
		struct inf_str<char> {
			static char const (&str(bool const capital))[4] {
				static char const s[][4] = { "inf", "INF" };
				return s[capital];
			}
		};
		template <>
		struct inf_str<wchar_t> {
			static wchar_t const (&str(bool const capital))[4] {
				static wchar_t const s[][4] = { L"inf", L"INF" };
				return s[capital];
			}
		};

		template <typename charT>
		inline void inc_strnum(charT *const strnum, unsigned const base, unsigned const col, bool const capital) {
			if (strnum[col]++ == to_hexstr<charT>(base-1, base, capital)) {
				strnum[col] = to_hexstr<charT>(0, base, capital);
				inc_strnum(strnum, base, col - 1, capital);
			}
		}

		template <typename T>
		inline T ipow(unsigned const base, int e) {
			return pow(static_cast<T>(base), e);
		}

		template <typename T>
		inline int ilog(unsigned const base, T v) {
			if (isinf(v)) return ilog(base, std::numeric_limits<T>::max());
			if (v==0) return 0;
			return static_cast<int>(floor( (base==10) ? log10(v) : (log(v)/log(static_cast<T>(base))) )) + 1;
		}

		namespace detail {
#ifndef NTFMT_NO_BOOST
			using boost::int_max_value_t;
			using boost::int_min_value_t;
			using boost::integral_promotion;
#else
			template <unsigned long long N>
			struct int_max_value_t {
				typedef
					typename if_c<(N<=UCHAR_MAX), unsigned char,
					typename if_c<(N<=USHRT_MAX), unsigned short,
					typename if_c<(N<=UINT_MAX), unsigned int,
					typename if_c<(N<=ULONG_MAX), unsigned long,
					typename if_c<(N<=ULONG_LONG_MAX), unsigned long long,
				void>::type>::type>::type>::type>::type type;
				typedef type least;
				typedef type fast;
			};
			template <long long N>
			struct int_min_value_t {
				typedef
					typename if_c<(N>=SCHAR_MIN), signed char,
					typename if_c<(N>=SHRT_MIN), signed short,
					typename if_c<(N>=INT_MIN), signed int,
					typename if_c<(N>=LONG_MIN), signed long,
					typename if_c<(N>=LONG_LONG_MIN), signed long long,
				void>::type>::type>::type>::type>::type type;
				typedef type least;
				typedef type fast;
			};
			template <typename T>
			struct integral_promotion {
				typedef decltype((T)0+0) type;
			};
#endif
		}
		template <typename T>
		struct dtoa_traits {
			typedef typename detail::int_max_value_t<numeric_limits<T>::max_exponent>::least higher_type;
			typedef typename detail::int_min_value_t<numeric_limits<T>::min_exponent>::least lower_type;
			typedef typename detail::integral_promotion<typename select_larger_type<higher_type, lower_type>::type>::type return_type;
		};
		template <typename charT, typename T, size_t N>
		typename dtoa_traits<T>::return_type dtoa(charT (&out)[N], T const v, int prec, flags_t const &flags) {
			typedef typename dtoa_traits<T>::return_type return_type;
			unsigned const base = flags.radix;
			bool const capital = flags.capital;
			T const value = fabs(v);
			return_type const k = ilog(base, get_successor(value));

			prec = ref_clip(0, prec, static_cast<int>(N-3));
			out[0] = NTFMT_CHR_ZERO;
			{
				T q = (k>0) ? (value / ipow<T>(base, k-1)) : (value * ipow<T>(base, 1-k));

				charT const *const end = array_end(out) - 1;
				for (charT *ptr = array_begin(out) + 1; ptr < end; ++ptr) {
					*ptr = to_hexstr<charT>(q, base, capital);
					q = fmod(q, base) * base;
				}
				unsigned f = static_cast<unsigned>(from_hexstr(out[prec+1], base, capital));
				if (f > base/2 || (f == base/2 && !(q == 0 && static_cast<unsigned>(from_hexstr(out[prec], base, capital))%2 == 0))) {
					inc_strnum(out, base, prec, capital);
				}
			}
			bool const carried = (out[0] != NTFMT_CHR_ZERO);
			if (!carried) std::copy(out+1, out+prec+1, out);
			out[prec+carried] = 0;
			return ref_min(k+carried, static_cast<return_type>(N-3));
		}
		template <typename charT, typename T>
		void float_printer(sink_fn_t<charT> &fn, T const &value, const flags_t &flags) {
			using namespace std;
			if (ntfmt_unlikely(isnan(value))) {
				fn(nan_str<charT>::str(flags.capital));
				return;
			}
			charT head[6] = { };
			charT *phead = head;
			if (copysign(static_cast<T>(1), value) < 0) *phead++ = NTFMT_CHR_MINUS;
			else if (flags.plus) *phead++ = NTFMT_CHR_PLUS;
			else if (flags.space) *phead++ = NTFMT_CHR_SPACE;
			if (ntfmt_unlikely(isinf(value))) {
				fn(head);
				fn(inf_str<charT>::str(flags.capital));
				return;
			}
			int const prec = flags.prec_enable ? flags.precision : 6;
			int const l = ilog(flags.radix, fabs(value));
			bool const exponential = flags.exponential || ( !flags.fixed && ((l < -4) || (l > prec)));
			charT buf[NTFMT_PRINT_FLOAT_BUFFER_SIZE + 8] = { };
			if (exponential) {
				int const e = dtoa<charT, T>(buf, value, prec + flags.exponential, flags);
				if (!flags.exponential && !flags.alter) {
					charT *p = buf + gstrlen(buf) - 1;
					while (*p == NTFMT_CHR_ZERO) *p-- = 0;
				}
				*phead++ = buf[0];
				buf[0] = buf[1] && (prec!=0 || flags.alter) ? NTFMT_CHR_DOT : 0;

				size_t const wid = gstrlen(head) + gstrlen(buf) + 5;
				if (!flags.minus) fill_chr_to(fn, NTFMT_CHR_SPACE, flags.width - wid);
				fn(head);
				fn(buf);
				fn(flags.capital ? NTFMT_CH_LIT('E') : NTFMT_CH_LIT('e'));
				flags_t ff = default_flags();
				ff.zero = 1;
				ff.prec_enable = 1;
				ff.precision = 3;
				ff.plus = 1;
				integer_printer(fn, static_cast<long>(e-1), ff);
				if (flags.minus) fill_chr_to(fn, NTFMT_CHR_SPACE, flags.width - wid);
			} else {
				int const e = dtoa<charT, T>(buf, value, prec + (flags.fixed ? l : 0), flags);
				if (e < 1) {
					*phead++ = NTFMT_CHR_ZERO;
					if (prec!=0) *phead++ = NTFMT_CHR_DOT;
					if (e <= -prec) buf[0] = 0;
				} else {
					if (prec!=0) {
						charT *const end = &buf[gstrlen(buf)]+1;
						std::rotate(&buf[e], end, end+1);
						buf[e] = NTFMT_CHR_DOT;
					}
					if (!flags.fixed && !flags.alter) {
						charT *p = buf + gstrlen(buf) - 1;
						while (*p == NTFMT_CHR_ZERO) *p-- = 0;
						if (*p == NTFMT_CHR_DOT) *p-- = 0;
					}
				}
				int const lim = e < -prec ? prec : -e;
				size_t const wid = gstrlen(head) + gstrlen(buf) + lim + (e>=1 ? e : 0);

				if (!flags.minus) fill_chr_to(fn, NTFMT_CHR_SPACE, flags.width - wid);
				fn(head);
				fill_chr_to(fn, NTFMT_CHR_ZERO, lim);
				fn(buf);
				if (flags.minus) fill_chr_to(fn, NTFMT_CHR_SPACE, flags.width - wid);
			}
		}
	}
}

#endif


/*
    Copyright (c) 2017 Mobile Robots Laboratory at Poznan University of Technology:
    -Jan Wietrzykowski name.surname [at] put.poznan.pl

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef INCLUDE_PLANESEG_HPP
#define INCLUDE_PLANESEG_HPP

#include <vector>

#include <Eigen/Eigen>

#include <pcl/impl/point_types.hpp>
#include <pcl/common/common_headers.h>

#include "UnionFind.h"
#include "Types.hpp"
#include "Serialization.hpp"

class PlaneSeg {
public:
    PlaneSeg()
            : id(-1),
              label(0),
              normAlignConsistent(true),
              points(new pcl::PointCloud<pcl::PointXYZRGB>()),
              normals(new pcl::PointCloud<pcl::Normal>())
    {}
    PlaneSeg(int iid,
             int ilabel,
             pcl::PointCloud<pcl::PointXYZRGB>::Ptr ipoints,
             pcl::PointCloud<pcl::Normal>::Ptr inormals,
             const std::vector<int> &iorigPlaneSegs,
             const std::vector<int> &iadjSegs)
            : id(iid),
              label(ilabel),
              normAlignConsistent(true),
              points(ipoints),
              normals(inormals),
              origPlaneSegs(iorigPlaneSegs),
              adjSegs(iadjSegs)
    {
        calcSegProp();
    }

    PlaneSeg(const PlaneSeg &other);

    void setId(int id) {
        PlaneSeg::id = id;
    }
    void setPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr ipoints){
        points = ipoints;
    }

    void setNormals(pcl::PointCloud<pcl::Normal>::Ptr inormals){
        normals = inormals;
    }

    void setLabel(int ilabel){
        label = ilabel;
    }

    void setSegNormal(const Eigen::Vector3d &isegNormal){
        segNormal = isegNormal;
    }

    void setOrigPlaneSegs(const std::vector<int> &origPlaneSegs) {
        PlaneSeg::origPlaneSegs = origPlaneSegs;
    }

    void setSegCentroid(const Eigen::Vector3d &isegCentroid){
        segCentroid = isegCentroid;
    }

    void setAdjSegs(const std::vector<int> &iadjSegs) {
        adjSegs = iadjSegs;
    }

    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &getPoints() const {
        return points;
    }

    const pcl::PointCloud<pcl::Normal>::Ptr &getNormals() const {
        return normals;
    }

    const std::vector<int> &getOrigPlaneSegs() const {
        return origPlaneSegs;
    }

    int getId() const {
        return id;
    }

    int getLabel() const {
        return label;
    }

    const Eigen::Vector3d &getSegNormal() const {
        return segNormal;
    }

    double getSegNormalIntDiff() const {
        return segNormalIntDiff;
    }

    float getSegCurv() const {
        return segCurv;
    }

    const Eigen::Vector3d &getSegCentroid() const {
        return segCentroid;
    }

    const Eigen::Matrix3d &getSegCovar() const {
        return segCovar;
    }

    const vectorVector3d &getEvecs() const {
        return evecs;
    }

    const std::vector<double> &getEvals() const {
        return evals;
    }

    const std::vector<int> &getAdjSegs() const {
        return adjSegs;
    }

    float getAreaEst() const {
        return areaEst;
    }

    pcl::PointNormal getPointNormal() const {
        pcl::PointNormal pt;
        pt.x = segCentroid[0];
        pt.y = segCentroid[1];
        pt.z = segCentroid[2];
        pt.normal_x = segNormal[0];
        pt.normal_y = segNormal[1];
        pt.normal_z = segNormal[2];

        return pt;
    }

    const Eigen::Vector4d& getSegPlaneParams() const {
        return segPlaneParams;
    }

    bool isNormAlignConsistent(){
        return normAlignConsistent;
    }

    void addPointAndNormal(pcl::PointXYZRGB newPt, pcl::Normal newNorm){
        points->push_back(newPt);
        normals->push_back(newNorm);
    }

    void addPointsAndNormals(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr newPts,
                             pcl::PointCloud<pcl::Normal>::ConstPtr newNorms){
        points->insert(points->end(), newPts->begin(), newPts->end());
        normals->insert(normals->end(), newNorms->begin(), newNorms->end());
    }

    void addOrigPlaneSegs(const std::vector<int> &newOrigPlaneSegs){
        origPlaneSegs.insert(origPlaneSegs.end(), newOrigPlaneSegs.begin(), newOrigPlaneSegs.end());
    }

    void addAdjSeg(int newNh){
        adjSegs.push_back(newNh);
    }

    void addAdjSegs(const std::vector<int> &newAdjSegs){
        adjSegs.insert(adjSegs.end(), newAdjSegs.begin(), newAdjSegs.end());
    }

    void calcSegProp(bool filter = false);

    void transform(const Vector7d &transform);

    PlaneSeg merge(const PlaneSeg &planeSeg, UnionFind &sets);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    int id;
    int label;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr points;
    pcl::PointCloud<pcl::Normal>::Ptr normals;
    std::vector<int> origPlaneSegs;

    Eigen::Vector3d segNormal;
    double segNormalIntDiff;
    Eigen::Vector4d segPlaneParams;
    Eigen::Vector3d segCentroid;
    Eigen::Matrix3d segCovar;
    vectorVector3d evecs;
    std::vector<double> evals;
    float segCurv;
    bool normAlignConsistent;
    float areaEst;

    std::vector<int> adjSegs;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & id;
        ar & label;
        ar & points;
        ar & normals;
        ar & origPlaneSegs;
        ar & segNormal;
        ar & segNormalIntDiff;
        ar & segPlaneParams;
        ar & segCentroid;
        ar & segCovar;
        ar & evecs;
        ar & evals;
        ar & segCurv;
        ar & normAlignConsistent;
        ar & areaEst;
        ar & adjSegs;
    }
};

#endif //INCLUDE_PLANESEG_HPP


#include <iostream>
#include <stdexcept>
#include <parallel/algorithm>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <glog/logging.h>
#include "calign.h"
#include "image-io.h"

namespace calign {
    using namespace std;

    class Histogram: public vector<uint16_t> {
    public:
        // Sample pixels from a list of images, at most max_image pixels from
        // each image.
        // Then sort all the pixels.
        Histogram (vector<string> const &files, size_t max_image = 100000) {
            reserve(files.size() * max_image);
            boost::progress_display progress(files.size(), cout);
#pragma omp parallel
            {
                std::default_random_engine rng;
#pragma omp for
                for (unsigned i = 0; i < files.size(); ++i) {
                    cv::Mat im = load_image(files[i]);
                    //cout << path << " " << im.total() << endl;
                    uint16_t *ptr = im.ptr<uint16_t>(0);
                    size_t sz = im.total();
                    if (sz > max_image) {
                        shuffle(ptr, ptr +  sz, rng);
                        sz = max_image;
                    }
#pragma omp critical
                    {
                        insert(end(), ptr, ptr + sz);
                        ++progress;
                    }
                }
            }
            __gnu_parallel::sort(begin(), end());
        }
    };

    void CAlign::fit (vector<string> const &from_files,
                      vector<string> const &to_files) {
        Histogram from(from_files);
        Histogram to(to_files);
        unsigned last_from_v = 0;
        float last_to_v = 0;
        lookup.resize(from.back() + 1);
        lookup[last_from_v] = last_to_v;
        unsigned b = 0;
        for (;;) {
            if (b >= from.size()) break;
            unsigned e = b + 1;
            while (e < from.size() && from[b] == from[e]) ++e;
            unsigned from_v = from[b];
            unsigned to_index = (b + e - 1) * to.size() / (2 * from.size());
            if (to_index >= to.size()) to_index = to.size() - 1;
            float to_v = to[to_index];
            cout << from_v << " => "  << to_v << endl;
            if (from_v == 0) {
                last_from_v = from_v;
                last_to_v = to_v;
                lookup[last_from_v] = last_to_v;
            }
            else {
                for (unsigned x = last_from_v + 1; x <= from_v; ++x) {
                    //     y - last_to_v          x - last_from_v
                    //   ----------------- ==== --------------------
                    //   to_v - last_to_v       from_v - last_from_v
                    //
                    float y =  1.0 * (x - last_from_v) * (to_v - last_to_v) / (from_v - last_from_v)
                            + last_to_v;
                    lookup[x] = y;
                }
            }
            b = e;
        }
    }
}

using namespace std;
namespace fs = boost::filesystem;

void load_list (fs::path const &path, vector<string> *list) {
    fs::ifstream is(path);
    string line;
    list->clear();
    while (is >> line) {
        list->emplace_back(std::move(line));
    }
}

int main (int argc, char *argv[]) {
    fs::path from_path;
    fs::path to_path;
    fs::path model_path;
    {
        int verbose;
        namespace po = boost::program_options;
        po::options_description desc_visible("Allowed options");
        desc_visible.add_options()
            ("help,h", "produce help message.")
            ("from", po::value(&from_path), "")
            ("to", po::value(&to_path), "")
            ("model", po::value(&model_path), "")
            ;

        po::options_description desc("Allowed options");
        desc.add(desc_visible);

        po::positional_options_description p;
        p.add("from", 1);
        p.add("to", 1);
        p.add("model", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).
                         options(desc).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help") || from_path.empty() || to_path.empty() || model_path.empty()) {
            cout << "Usage:" << endl;
            cout << desc;
            cout << endl;
            return 0;
        }
    }
    setup_dicom(argv[0]);

    vector<string> from;
    vector<string> to;
    load_list(from_path, &from);
    load_list(to_path, &to);
    calign::CAlign align;
    align.fit(from, to);
    align.save(model_path.native());
    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Introduction to Flight, Fifth edition, Appendix A, John D. Anderson Jr., McGraw Hill, 2005.
 *      US Standard Atmosphere 1976,
 *          http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770009539_1977009539.pdf.
 *      Forget, François, et al. "Improved general circulation models of the Martian atmosphere from the surface to above 80 km."
 *          Journal of Geophysical Research: Planets 104.E10 (1999): 24155-24175.
 *      Millour, E., et al. "The Mars Climate Database (MCD version 5.2)." European Planetary Science Congress 2015. Vol. 10. EPSC, 2015.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/astro/aerodynamics/tabulatedAtmosphere.h"
#include "tudat/io/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_tabulated_atmosphere )

// Summary of tests.
// Test 1: Test tabulated atmosphere at sea level.
// Test 2: Test tabulated atmosphere at 10.0 km altitude including passing arbitrary longitude
//         and latitude.
// Test 3: Test tabulated atmosphere at 10.05 km altitude when just passing the altitude.
// Test 4: Test tabulated atmosphere at 1000 km altitude with table.
// Test 5: Test if the atmosphere file can be read multiple times.
// Test 6: Test if the position-independent functions work.

//! Check if the atmosphere is calculated correctly at sea level.
// Values from (US Standard Atmosphere, 1976).
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereAtSeaLevel )
{
    // Create a tabulated atmosphere object.
    std::string tabulatedAtmosphereFile = paths::getAtmosphereTablesPath( ) +
            "/USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFile );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    const double altitude = 0.0;
    BOOST_CHECK_CLOSE_FRACTION( 1.225, tabulatedAtmosphere.getDensity( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 101325.0, tabulatedAtmosphere.getPressure( altitude ), 1.0e-4 );
    BOOST_CHECK_CLOSE_FRACTION( 288.15, tabulatedAtmosphere.getTemperature( altitude ), tolerance );
}

//! Test tabulated atmosphere at 10km altitude including passing arbitrary longitude and latitude.
// The given value for pressure was obtained from table in book.
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereAt10km )
{
    // Create a tabulated atmosphere object.
    std::string tabulatedAtmosphereFile = paths::getAtmosphereTablesPath( ) +
            "/USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFile );
    const double altitude = 10.0e3;
    const double longitude = 0.0;
    const double latitude = 0.0;
    const double time = 0.0;

    BOOST_CHECK_SMALL( 0.41351 - tabulatedAtmosphere.getDensity( altitude,  longitude, latitude, time ), 1.0e-4 );
    BOOST_CHECK_SMALL( 26500.0 - tabulatedAtmosphere.getPressure( altitude,  longitude, latitude, time ), 1.0 );
    BOOST_CHECK_SMALL( 223.26 - tabulatedAtmosphere.getTemperature( altitude,  longitude, latitude, time ), 1.0e-2 );
}

//! Test tabulated atmosphere at 10.05 km altitude when just passing the altitude.
// Check whether the atmosphere is calculated correctly at 10.05 km.
// The values are linear interpolated values based on book values.
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereAt10p5km )
{
    // Create a tabulated atmosphere object.
    std::string tabulatedAtmosphereFile = paths::getAtmosphereTablesPath( ) +
            "/USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFile );
    const double altitude = 10.05e3;

    BOOST_CHECK_SMALL( 0.4110 - tabulatedAtmosphere.getDensity( altitude ), 1.0e-3 );
    BOOST_CHECK_SMALL( 26299.0 - tabulatedAtmosphere.getPressure( altitude ), 1.0 );
    BOOST_CHECK_SMALL( 222.9350 - tabulatedAtmosphere.getTemperature( altitude ), 2.0e-2 );
}

//! Test tabulated atmosphere at 1000 km altitude.
// Check whether the atmosphere is calculated correctly at 1000 km. Compared with the input table.
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereAt1000kmtab )
{
    // Create a tabulated atmosphere object.
    std::string tabulatedAtmosphereFiles = paths::getAtmosphereTablesPath( ) +
            "/USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles );
    const double altitude = 1.0e6;

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    BOOST_CHECK_CLOSE_FRACTION( 3.5618e-15, tabulatedAtmosphere.getDensity( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 7.5158e-9, tabulatedAtmosphere.getPressure( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1000.0, tabulatedAtmosphere.getTemperature( altitude ), tolerance );
}

//! Check if the atmosphere is calculated correctly when the variables are shuffled
// Values from (US Standard Atmosphere, 1976).
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereDependentVariables )
{
    // Create vector of dependent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables;
    dependentVariables.push_back( aerodynamics::pressure_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::density_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::temperature_dependent_atmosphere );

    // Create a tabulated atmosphere object.
    std::string tabulatedAtmosphereFile = paths::getAtmosphereTablesPath( ) +
            "/USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFile, dependentVariables );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    const double altitude = 0.0;
    // Pressure and Density are switched
    BOOST_CHECK_CLOSE_FRACTION( 101325.0, tabulatedAtmosphere.getDensity( altitude ), 1.0e-4 );
    BOOST_CHECK_CLOSE_FRACTION( 1.225, tabulatedAtmosphere.getPressure( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 288.15, tabulatedAtmosphere.getTemperature( altitude ), tolerance );
}

//! Check if the atmosphere is calculated correctly when all the possible dependent variables are used and shuffled
// Values from Mars Climate Database Web Interface (http://www-mars.lmd.jussieu.fr/mcd_python/), avaraged over time, latitude and longitude.
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereAllDependentVariables )
{
    // Create vector of dependent and independent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables;
    dependentVariables.push_back( aerodynamics::pressure_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::molar_mass_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::specific_heat_ratio_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::gas_constant_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::temperature_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::density_dependent_atmosphere );
    std::vector< aerodynamics::AtmosphereIndependentVariables > independentVariables = { aerodynamics::latitude_dependent_atmosphere };

    // Create a tabulated atmosphere object.
    std::map< int, std::string > tabulatedAtmosphereFiles;
    tabulatedAtmosphereFiles[ 0 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphere.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles, independentVariables, dependentVariables );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    const double latitude = 5e4;
    BOOST_CHECK_CLOSE_FRACTION( 4.3604106892e-02, tabulatedAtmosphere.getDensity( 0.0, 0.0, latitude ), 1.0e-4 );
    BOOST_CHECK_CLOSE_FRACTION( 7.6178752157e-05, tabulatedAtmosphere.getPressure( 0.0, 0.0, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.3794210386e+00, tabulatedAtmosphere.getTemperature( 0.0, 0.0, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.9068065814e+02, tabulatedAtmosphere.getSpecificGasConstant( 0.0, 0.0, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.6104994141e+02, tabulatedAtmosphere.getRatioOfSpecificHeats( 0.0, 0.0, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 2.3477457225e+00, tabulatedAtmosphere.getMolarMass( 0.0, 0.0, latitude ), 1.0e-4 );
    BOOST_CHECK_CLOSE_FRACTION( 205.817372408135, tabulatedAtmosphere.getSpeedOfSound( 0.0, 0.0, latitude ), 10.0 * tolerance );
}

//! Check if the atmosphere is calculated correctly when more than one independent variable is used, and when
//! dependent variables are shuffled.
// Values from Mars Climate Database Web Interface (http://www-mars.lmd.jussieu.fr/mcd_python/), avaraged over time.
BOOST_AUTO_TEST_CASE( testMultiDimensionalTabulatedAtmosphere )
{
    // Create vector of dependent and independent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables = {
        aerodynamics::density_dependent_atmosphere, aerodynamics::pressure_dependent_atmosphere,
        aerodynamics::temperature_dependent_atmosphere};
    std::vector< aerodynamics::AtmosphereIndependentVariables > independentVariables = {
        aerodynamics::longitude_dependent_atmosphere, aerodynamics::latitude_dependent_atmosphere,
        aerodynamics::altitude_dependent_atmosphere };

    // Create a tabulated atmosphere object.
    std::map< int, std::string > tabulatedAtmosphereFiles;
    tabulatedAtmosphereFiles[ 0 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/density.dat";
    tabulatedAtmosphereFiles[ 1 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/pressure.dat";
    tabulatedAtmosphereFiles[ 2 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/temperature.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles, independentVariables, dependentVariables );

    // Declare tolerance used for Boost tests.
    const double tolerance = 1e-7;

    // Define independent variable conditions
    const double altitude = 5.0e4;
    const double longitude = unit_conversions::convertDegreesToRadians( -180.0 );
    const double latitude = unit_conversions::convertDegreesToRadians( -90.0 );

    // Check that values matches with MATLAB interpolation (note that dependent variables are shuffled)
    BOOST_CHECK_CLOSE_FRACTION( 5.2805275e-05, tabulatedAtmosphere.getDensity( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.6627685, tabulatedAtmosphere.getPressure( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 151.544, tabulatedAtmosphere.getTemperature( altitude, longitude, latitude ), 1e2 * tolerance );
}

//! Check if the atmosphere is calculated correctly when more than one independent variable is used, and when both independent
//! and dependent variables are shuffled. Also tests for linear interpolation.
// Values from Mars Climate Database Web Interface (http://www-mars.lmd.jussieu.fr/mcd_python/), avaraged over time.
BOOST_AUTO_TEST_CASE( testMultiDimensionalTabulatedAtmosphereWithInterpolationAndShuffledVariables )
{
    // Create vector of dependent and independent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables = {
        aerodynamics::specific_heat_ratio_dependent_atmosphere, aerodynamics::temperature_dependent_atmosphere,
        aerodynamics::density_dependent_atmosphere, aerodynamics::pressure_dependent_atmosphere,
        aerodynamics::gas_constant_dependent_atmosphere };
    std::vector< aerodynamics::AtmosphereIndependentVariables > independentVariables = {
        aerodynamics::longitude_dependent_atmosphere, aerodynamics::latitude_dependent_atmosphere,
        aerodynamics::altitude_dependent_atmosphere };

    // Create a tabulated atmosphere object.
    std::map< int, std::string > tabulatedAtmosphereFiles;
    tabulatedAtmosphereFiles[ 0 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/specificHeatRatio.dat";
    tabulatedAtmosphereFiles[ 1 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/temperature.dat";
    tabulatedAtmosphereFiles[ 2 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/density.dat";
    tabulatedAtmosphereFiles[ 3 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/pressure.dat";
    tabulatedAtmosphereFiles[ 4 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/gasConstant.dat";
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles, independentVariables, dependentVariables );

    // Declare tolerance used for Boost tests.
    const double tolerance = 1e-5;

    // Define independent variable conditions
    const double altitude = 236.9862e3;
    const double longitude = unit_conversions::convertDegreesToRadians( 72.98632 );
    const double latitude = unit_conversions::convertDegreesToRadians( -65.9762 );

    // Check that values matches with MATLAB interpolation (note that dependent variables are shuffled)
    BOOST_CHECK_CLOSE_FRACTION( 5.50924276619674e-13, tabulatedAtmosphere.getDensity( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 5.05433422347663e-08, tabulatedAtmosphere.getPressure( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 174.841221295332, tabulatedAtmosphere.getTemperature( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 536.101455389577, tabulatedAtmosphere.getSpecificGasConstant( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.66646490380313, tabulatedAtmosphere.getRatioOfSpecificHeats( altitude, longitude, latitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 395.224168752852, tabulatedAtmosphere.getSpeedOfSound( altitude, longitude, latitude ), tolerance );
}

//! Check if the atmosphere is calculated correctly when default extrapolation values are used.
// Values from Mars Climate Database Web Interface (http://www-mars.lmd.jussieu.fr/mcd_python/), avaraged over time.
BOOST_AUTO_TEST_CASE( testMultiDimensionalTabulatedAtmosphereDefaultExtrapolation )
{
    // Create vector of dependent and independent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables = {
        aerodynamics::pressure_dependent_atmosphere, aerodynamics::temperature_dependent_atmosphere,
        aerodynamics::specific_heat_ratio_dependent_atmosphere, aerodynamics::density_dependent_atmosphere };
    std::vector< aerodynamics::AtmosphereIndependentVariables > independentVariables = {
        aerodynamics::longitude_dependent_atmosphere, aerodynamics::latitude_dependent_atmosphere,
        aerodynamics::altitude_dependent_atmosphere };

    // Create a tabulated atmosphere object.
    std::map< int, std::string > tabulatedAtmosphereFiles;
    tabulatedAtmosphereFiles[ 0 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/pressure.dat";
    tabulatedAtmosphereFiles[ 1 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/temperature.dat";
    tabulatedAtmosphereFiles[ 2 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/specificHeatRatio.dat";
    tabulatedAtmosphereFiles[ 3 ] = paths::getAtmosphereTablesPath( ) + "/MCDMeanAtmosphereTimeAverage/density.dat";

    // Test 1: check with single vector of default values
    {
        std::vector< interpolators::BoundaryInterpolationType > boundaryHandling = { interpolators::throw_exception_at_boundary,
                                                                                interpolators::use_boundary_value_with_warning,
                                                                                interpolators::use_default_value_with_warning };
        std::vector< double > defualtExtrapolationValues = { 100, 250, 1.3, 0 };
        aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles, independentVariables, dependentVariables,
                                                               boundaryHandling, defualtExtrapolationValues );

        // Declare tolerance used for Boost tests.
        const double tolerance = 1e-7;

        // Define independent variable conditions
        const std::vector< double > longitude = { unit_conversions::convertDegreesToRadians( 0.0 ),
                                                  unit_conversions::convertDegreesToRadians( 0.0 ) + 10.0,
                                                  unit_conversions::convertDegreesToRadians( 0.0 ) };
        const std::vector< double > latitude = { unit_conversions::convertDegreesToRadians( 0.0 ),
                                                 unit_conversions::convertDegreesToRadians( 0.0 ),
                                                 unit_conversions::convertDegreesToRadians( 0.0 ) + 10.0 };
        const std::vector< double > altitude = { 5.0e7, 5.0e4, 5.0e4 };

        // Expected results
        std::vector< double > boundaryValues = { 0.877653865782941, 163.354129386099, 1.37774305607899, 2.82975546661156e-05 };
        const std::vector< bool > expectedException = { false, true, false };
        std::vector< double > expectedResult;
        bool exception;

        // Check that values matches with MATLAB interpolation (note that dependent variables are shuffled)
        for ( unsigned int i = 0; i < 3; i++ )
        {
            exception = false;
            expectedResult = ( i == 0 ) ? defualtExtrapolationValues : boundaryValues;
            try
            {
                BOOST_CHECK_CLOSE_FRACTION( expectedResult.at( 0 ),
                                            tabulatedAtmosphere.getPressure( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                            tolerance );
                BOOST_CHECK_CLOSE_FRACTION( expectedResult.at( 1 ),
                                            tabulatedAtmosphere.getTemperature( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                            tolerance );
                BOOST_CHECK_CLOSE_FRACTION( expectedResult.at( 2 ),
                                            tabulatedAtmosphere.getRatioOfSpecificHeats( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                            tolerance );
                BOOST_CHECK_CLOSE_FRACTION( expectedResult.at( 3 ),
                                            tabulatedAtmosphere.getDensity( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                            tolerance );
            }
            catch( std::runtime_error const& )

            {
                exception = true;
            }
            BOOST_CHECK_EQUAL( exception, expectedException.at( i ) );
        }
    }

    // Test 2: check with full set of default values
    {
        std::vector< interpolators::BoundaryInterpolationType > boundaryHandling =
        std::vector< interpolators::BoundaryInterpolationType >( independentVariables.size( ), interpolators::use_default_value );
        std::vector< std::vector< std::pair< double, double > > > defualtExtrapolationValues =
        {
            std::vector< std::pair< double, double > >( independentVariables.size( ), std::make_pair( -100.0, 100.0 ) ),
            std::vector< std::pair< double, double > >( independentVariables.size( ), std::make_pair( 250.0, 0.08652 ) ),
            std::vector< std::pair< double, double > >( independentVariables.size( ), std::make_pair( 1.4, 1.3 ) ),
            std::vector< std::pair< double, double > >( independentVariables.size( ), std::make_pair( 1e-3, 0.0 ) )
        };
        aerodynamics::TabulatedAtmosphere tabulatedAtmosphere( tabulatedAtmosphereFiles, independentVariables, dependentVariables,
                                                               boundaryHandling, defualtExtrapolationValues );

        // Declare tolerance used for Boost tests.
        const double tolerance = 1e-7;

        // Define independent variable conditions
        const std::vector< double > longitude = { unit_conversions::convertDegreesToRadians( 0.0 ) - 10.0,
                                                  unit_conversions::convertDegreesToRadians( 0.0 ) + 10.0,
                                                  unit_conversions::convertDegreesToRadians( 0.0 ),
                                                  unit_conversions::convertDegreesToRadians( 0.0 ),
                                                  unit_conversions::convertDegreesToRadians( 0.0 ),
                                                  unit_conversions::convertDegreesToRadians( 0.0 ) };
        const std::vector< double > latitude = { unit_conversions::convertDegreesToRadians( 0.0 ),
                                                 unit_conversions::convertDegreesToRadians( 0.0 ),
                                                 unit_conversions::convertDegreesToRadians( 0.0 ) - 10.0,
                                                 unit_conversions::convertDegreesToRadians( 0.0 ) + 10.0,
                                                 unit_conversions::convertDegreesToRadians( 0.0 ),
                                                 unit_conversions::convertDegreesToRadians( 0.0 ) };
        const std::vector< double > altitude = { 5.0e5, 5.0e5, 5.0e5, 5.0e5, 0.0, 5.0e10 };

        // Check that values matches default values
        for ( unsigned int i = 0; i < altitude.size( ); i++ )
        {
            std::vector< double > expectedValue;
            for ( unsigned int j = 0; j < dependentVariables.size( ); j++ )
            {
                if ( ( i % 2 ) == 0.0 )
                {
                    expectedValue.push_back( defualtExtrapolationValues.at( j ).at( std::floor( i / 2.0 ) ).first );
                }
                else
                {
                    expectedValue.push_back( defualtExtrapolationValues.at( j ).at( std::floor( i / 2.0 ) ).second );
                }
            }

            BOOST_CHECK_CLOSE_FRACTION( expectedValue.at( 0 ),
                                        tabulatedAtmosphere.getPressure( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                        tolerance );
            BOOST_CHECK_CLOSE_FRACTION( expectedValue.at( 1 ),
                                        tabulatedAtmosphere.getTemperature( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                        tolerance );
            BOOST_CHECK_CLOSE_FRACTION( expectedValue.at( 2 ),
                                        tabulatedAtmosphere.getRatioOfSpecificHeats( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                        tolerance );
            BOOST_CHECK_CLOSE_FRACTION( expectedValue.at( 3 ),
                                        tabulatedAtmosphere.getDensity( altitude.at( i ), longitude.at( i ), latitude.at( i ) ),
                                        tolerance );
        }
    }
}

//! Check if the atmosphere is calculated correctly when heat ratio and gas constants are added.
// Values from (US Standard Atmosphere, 1976).
BOOST_AUTO_TEST_CASE( testTabulatedAtmosphereExtraVariables )
{
    // Create vector of dependent variables
    std::vector< aerodynamics::AtmosphereDependentVariables > dependentVariables;
    dependentVariables.push_back( aerodynamics::density_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::pressure_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::temperature_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::specific_heat_ratio_dependent_atmosphere );
    dependentVariables.push_back( aerodynamics::gas_constant_dependent_atmosphere );

    // Create a tabulated atmosphere object.
    aerodynamics::TabulatedAtmosphere tabulatedAtmosphere(
                paths::getAtmosphereTablesPath( ) + "/USSA1976Until100kmPer100mUntil1000kmPer1000m_wHR_GC.dat",
                dependentVariables );

    // Declare tolerance used for Boost tests.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    const double altitude = 0.0;
    BOOST_CHECK_CLOSE_FRACTION( 8.0, tabulatedAtmosphere.getSpecificGasConstant( altitude ), tolerance );
    BOOST_CHECK_CLOSE_FRACTION( 1.7, tabulatedAtmosphere.getRatioOfSpecificHeats( altitude ), 1.0e-4 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
// Sample code to perform I/O:

#include <iostream>

using namespace std;

int main() {
	int num;
	cin >> num;										// Reading input from STDIN
	cout << "Input number is " << num << endl;		// Writing output to STDOUT
}

// Warning: Printing unwanted or ill-formatted data to output will cause the test cases to fail
*/

// Write your code here
#include <iostream>
#include <vector>
#include <limits.h>
//#include <boost/multiprecision/long long int.hpp>

using namespace std;
//using namespace boost::multiprecision;

struct Coordinate
{
    long long int x;
    long long int y;

};

class Rectangle
{
    long long int px;
    long long int py;
    long long int width;
    long long int height;
    long long int limXinf;
    long long int limXsup;
    long long int limYinf;
    long long int limYsup;

    public:
    vector<Coordinate> pointsSet;

    Rectangle(long long int,long long int,long long int,long long int);
    vector<Coordinate> returnInternalRectangle(const Rectangle&);
};

Rectangle::Rectangle(long long int cx,long long int cy,long long int w,long long int h)
{
    this->px = cx;
    this->py = cy;
    this->width = w;
    this->height = h;

    this->limXinf = cx;
    this->limXsup = cx + w;
    this->limYinf = cy;
    this->limYsup = cy + h;

    Coordinate bc;

    bc.x = cx;
    bc.y = cy;
    pointsSet.push_back(bc);

    bc.x = cx + w;
    bc.y = cy;
    pointsSet.push_back(bc);

    bc.x = cx + w;
    bc.y = cy + h;
    pointsSet.push_back(bc);

    bc.x = cx;
    bc.y = cy + h;
    pointsSet.push_back(bc);
}

vector<Coordinate> Rectangle::returnInternalRectangle(const Rectangle& param)
{
    vector<Coordinate> vectorP;
    Coordinate point;
    Coordinate fabricated_point;

    for(long long int ii = 0;ii<4;ii++)
    {
        point = param.pointsSet[ii];
        //cout<<"px = "<< point.x<<", py = "<<point.y<<endl;
        if( ( point.x >= (this->limXinf) ) && ( point.x  <= (this->limXsup) ) &&
            ( point.y >= (this->limYinf) ) && ( point.y  <= (this->limYsup) ))
        {//point is inside the rectangle which made the call.
            //cout<<"px = "<< point.x<<", py = "<<point.y<<endl;
            vectorP.push_back(point);
        }else{
            if( ( point.x >= (this->limXinf) ) && ( point.x  <= (this->limXsup) ))
            {
                if( point.y  > (this->limYsup) )
                {
                    fabricated_point.x = point.x;
                    fabricated_point.y = this->limYsup;
                }
                if( point.y  < (this->limYinf) )
                {
                    fabricated_point.x = point.x;
                    fabricated_point.y = this->limYinf;
                }
                vectorP.push_back(fabricated_point);
            }

            if(( point.y >= (this->limYinf) ) && ( point.y  <= (this->limYsup) ))
            {
                if( point.x > (this->limXsup))
                {
                    fabricated_point.x = limXsup;
                    fabricated_point.y = point.y;
                }
                if( point.x < (this->limXinf))
                {
                    fabricated_point.x = limXinf;
                    fabricated_point.y = point.y;
                }
                vectorP.push_back(fabricated_point);
            }

        }
    }
    return vectorP;
}

int main()
{
	long long int cx = 0;
	long long int cy = 0;
	long long int w = 0;
	long long int h = 0;

    long long int maxX = LLONG_MIN;
    long long int minX = LLONG_MAX;
    long long int maxY = LLONG_MIN;
    long long int minY = LLONG_MAX;

    vector<Coordinate> vectt1;
    vector<Coordinate> vectt2;

    vector<Coordinate> vecttpointsInter;

	cin >> cx >> cy >>  w >>  h;
    Rectangle rect1(cx,cy,w,h);

	cin >> cx >> cy >>  w >>  h;
    Rectangle rect2(cx,cy,w,h);

    vectt2 = rect2.returnInternalRectangle(rect1);
    vectt1 = rect1.returnInternalRectangle(rect2);
/*
    cout<<"POints...\n";

    for(int aa = 0;aa<vectt1.size();aa++)
    {
        Coordinate pp;
        pp = vectt1[aa];
        cout<<"pp = "<< pp.x<<", pp = "<<pp.y<<endl;
    }
    cout<<endl;

    for(int aa = 0;aa<vectt2.size();aa++)
    {
        Coordinate pp;
        pp = vectt2[aa];
        cout<<"pp = "<< pp.x<<", pp = "<<pp.y<<endl;
    }
    cout<<endl;

    cout<<"confrontation...\n";
*/
    (vectt1.size()>vectt2.size())? vecttpointsInter = vectt1: vecttpointsInter = vectt2;

    if(vectt1.size()==4) vecttpointsInter = vectt1;
    if(vectt2.size()==4) vecttpointsInter = vectt2;

    for(int aa = 0;aa<vecttpointsInter.size();aa++)
    {
        Coordinate pp;
        pp = vecttpointsInter[aa];
        //cout<<"pp = "<< pp.x<<", pp = "<<pp.y<<endl;
        if(pp.x<minX) minX = pp.x;
        if(pp.y<minY) minY = pp.y;
        if(pp.x>maxX) maxX = pp.x;
        if(pp.y>maxY) maxY = pp.y;
    }
    cout<<(maxX-minX)*(maxY-minY) <<endl;
}


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

/* Test_General.cpp - A general test program that uses a mix of operations over four ciphertexts.
 */
#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include "ArgMap.h"

NTL_CLIENT

//#define DEBUG_PRINTOUT

#ifdef DEBUG_PRINTOUT
#include "debugging.h"
#define debugCompare(ea,sk,p,c) {\
  PlaintextArray pp(ea);\
  ea.decrypt(c, sk, pp);\
  if (!equals(ea, pp, p)) { \
    std::cout << "oops:\n"; std::cout << p << "\n"; \
    std::cout << pp << "\n"; \
    exit(0); \
  }}
#else
#define debugCompare(ea,sk,p,c)
#endif

/**************

1. c1.multiplyBy(c0)
2. c0 += random constant
3. c2 *= random constant
4. tmp = c1
5. ea.shift(tmp, random amount in [-nSlots/2, nSlots/2])
6. c2 += tmp
7. ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
8. c1.negate()
9. c3.multiplyBy(c2)
10. c0 -= c3

**************/

static bool noPrint = true;

void  TestIt(long R, long p, long r, long d, long c, long k, long w,
               long L, long m, const Vec<long>& gens, const Vec<long>& ords)
{
  char buffer[32];
  if (!noPrint) {
    std::cout << "\n\n******** TestIt" << (isDryRun()? "(dry run):" : ":");
    std::cout << " R=" << R
	      << ", p=" << p
	      << ", r=" << r
	      << ", d=" << d
	      << ", c=" << c
	      << ", k=" << k
	      << ", w=" << w
	      << ", L=" << L
	      << ", m=" << m
	      << ", gens=" << gens
	      << ", ords=" << ords
	      << endl;
  }
  vector<long> gens1, ords1;
  convert(gens1, gens);
  convert(ords1, ords);

  FHEcontext context(m, p, r, gens1, ords1);
  buildModChain(context, L, c);

  ZZX G;
  if (d == 0)
    G = context.alMod.getFactorsOverZZ()[0];
  else
    G = makeIrredPoly(p, d);

  if (!noPrint) {
    context.zMStar.printout();
    std::cout << endl;

    std::cout << "security=" << context.securityLevel()<<endl;
    std::cout << "# small primes = " << context.smallPrimes.card() << "\n";
    std::cout << "# ctxt primes = " << context.ctxtPrimes.card() << "\n";
    std::cout << "# bits in ctxt primes = "
	 << long(context.logOfProduct(context.ctxtPrimes)/log(2.0) + 0.5) << "\n";
    std::cout << "# special primes = " << context.specialPrimes.card() << "\n";
    std::cout << "# bits in special primes = "
	 << long(context.logOfProduct(context.specialPrimes)/log(2.0) + 0.5) << "\n";
    std::cout << "G = " << G << "\n";
  }

  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(); // A +-1/0 secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need

  EncryptedArray ea(context, G);
  long nslots = ea.size();
#ifdef DEBUG_PRINTOUT
  dbgKey = &secretKey;
  dbgEa  = &ea;
#endif

  PlaintextArray p0(ea);
  PlaintextArray p1(ea);
  PlaintextArray p2(ea);
  PlaintextArray p3(ea);

  random(ea, p0);
  random(ea, p1);
  random(ea, p2);
  random(ea, p3);

  Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  ea.encrypt(c0, publicKey, p0);
  // {ZZX ppp0; ea.encode(ppp0, p0); c0.DummyEncrypt(ppp0);} // dummy encryption
  ea.encrypt(c1, publicKey, p1); // real encryption
  ea.encrypt(c2, publicKey, p2); // real encryption
  ea.encrypt(c3, publicKey, p3); // real encryption

  resetAllTimers();

  FHE_NTIMER_START(Circuit);

  for (long i = 0; i < R; i++) {

    if (!noPrint) std::cout << "*** round " << i << "..."<<endl;

     long shamt = RandomBnd(2*(nslots/2) + 1) - (nslots/2);
                  // random number in [-nslots/2..nslots/2]
     long rotamt = RandomBnd(2*nslots - 1) - (nslots - 1);
                  // random number in [-(nslots-1)..nslots-1]

     // two random constants
     PlaintextArray const1(ea);
     PlaintextArray const2(ea);
     random(ea, const1);
     random(ea, const2);

     ZZX const1_poly, const2_poly;
     ea.encode(const1_poly, const1);
     ea.encode(const2_poly, const2);

     mul(ea, p1, p0);     // c1.multiplyBy(c0)
     c1.multiplyBy(c0);
     if (!noPrint) CheckCtxt(c1, "c1*=c0");
     debugCompare(ea,secretKey,p1,c1);

     add(ea, p0, const1); // c0 += random constant
     c0.addConstant(const1_poly);
     if (!noPrint) CheckCtxt(c0, "c0+=k1");
     debugCompare(ea,secretKey,p0,c0);

     mul(ea, p2, const2); // c2 *= random constant
     c2.multByConstant(const2_poly);
     if (!noPrint) CheckCtxt(c2, "c2*=k2");
     debugCompare(ea,secretKey,p2,c2);

     PlaintextArray tmp_p(p1); // tmp = c1
     Ctxt tmp(c1);
     sprintf(buffer, "tmp=c1>>=%d", (int)shamt);
     shift(ea, tmp_p, shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
     ea.shift(tmp, shamt);
     if (!noPrint) CheckCtxt(tmp, buffer);
     debugCompare(ea,secretKey,tmp_p,tmp);

     add(ea, p2, tmp_p);  // c2 += tmp
     c2 += tmp;
     if (!noPrint) CheckCtxt(c2, "c2+=tmp");
     debugCompare(ea,secretKey,p2,c2);

     sprintf(buffer, "c2>>>=%d", (int)rotamt);
     rotate(ea, p2, rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
     ea.rotate(c2, rotamt);
     if (!noPrint) CheckCtxt(c2, buffer);
     debugCompare(ea,secretKey,p2,c2);

     ::negate(ea, p1); // c1.negate()
     c1.negate();
     if (!noPrint) CheckCtxt(c1, "c1=-c1");
     debugCompare(ea,secretKey,p1,c1);

     mul(ea, p3, p2); // c3.multiplyBy(c2)
     c3.multiplyBy(c2);
     if (!noPrint) CheckCtxt(c3, "c3*=c2");
     debugCompare(ea,secretKey,p3,c3);

     sub(ea, p0, p3); // c0 -= c3
     c0 -= c3;
     if (!noPrint) CheckCtxt(c0, "c0=-c3");
     debugCompare(ea,secretKey,p0,c0);

  }

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  FHE_NTIMER_STOP(Circuit);

  if (!noPrint) {
    std::cout << endl;
    printAllTimers();
    std::cout << endl;
  }
  resetAllTimers();
  FHE_NTIMER_START(Check);

  PlaintextArray pp0(ea);
  PlaintextArray pp1(ea);
  PlaintextArray pp2(ea);
  PlaintextArray pp3(ea);

  ea.decrypt(c0, secretKey, pp0);
  ea.decrypt(c1, secretKey, pp1);
  ea.decrypt(c2, secretKey, pp2);
  ea.decrypt(c3, secretKey, pp3);

  if (equals(ea, pp0, p0) && equals(ea, pp1, p1)
      && equals(ea, pp2, p2) && equals(ea, pp3, p3))
       std::cout << "GOOD\n";
  else std::cout << "BAD\n";

  FHE_NTIMER_STOP(Check);

  if (!noPrint) {
    printAllTimers();
    std::cout << endl;
  }
}

/* A general test program that uses a mix of operations over four ciphertexts.
 * Usage: Test_General_x [ name=value ]...
 *   R       number of rounds  [ default=1 ]
 *   p       plaintext base  [ default=2 ]
 *   r       lifting  [ default=1 ]
 *   d       degree of the field extension  [ default=1 ]
 *              d == 0 => factors[0] defines extension
 *   c       number of columns in the key-switching matrices  [ default=2 ]
 *   k       security parameter  [ default=80 ]
 *   L       # of bits in the modulus chain  [ default=heuristic ]
 *   s       minimum number of slots  [ default=0 ]
 *   repeat  number of times to repeat the test  [ default=1 ]
 *   m       use specified value as modulus
 *   mvec    use product of the integers as  modulus
 *              e.g., mvec='[5 3 187]' (this overwrite the m argument)
 *   gens    use specified vector of generators
 *              e.g., gens='[562 1871 751]'
 *   ords    use specified vector of orders
 *              e.g., ords='[4 2 -4]', negative means 'bad'
 */
int main(int argc, char **argv)
{
  setTimersOn();

  ArgMap amap;

  bool dry=false;
  amap.arg("dry", dry, "dry=1 for a dry-run");

  long R=1;
  amap.arg("R", R, "number of rounds");

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long d=1;
  amap.arg("d", d, "degree of the field extension");
  amap.note("d == 0 => factors[0] defines extension");

  long c=2;
  amap.arg("c", c, "number of columns in the key-switching matrices");

  long k=80;
  amap.arg("k", k, "security parameter");

  long L=500;
  amap.arg("L", L, "# of bits in the modulus chain");

  long s=0;
  amap.arg("s", s, "minimum number of slots");

  long repeat=1;
  amap.arg("repeat", repeat,  "number of times to repeat the test");

  long chosen_m=0;
  amap.arg("m", chosen_m, "use specified value as modulus", NULL);

  Vec<long> mvec;
  amap.arg("mvec", mvec, "use product of the integers as  modulus", NULL);
  amap.note("e.g., mvec='[5 3 187]' (this overwrite the m argument)");

  Vec<long> gens;
  amap.arg("gens", gens, "use specified vector of generators", NULL);
  amap.note("e.g., gens='[562 1871 751]'");

  Vec<long> ords;
  amap.arg("ords", ords, "use specified vector of orders", NULL);
  amap.note("e.g., ords='[4 2 -4]', negative means 'bad'");

  long seed=0;
  amap.arg("seed", seed, "PRG seed");

  long nt=1;
  amap.arg("nt", nt, "num threads");

  amap.arg("noPrint", noPrint, "suppress printouts");

  amap.parse(argc, argv);

  SetSeed(ZZ(seed));
  SetNumThreads(nt);

  long w = 64; // Hamming weight of secret key

  if (mvec.length()>0)
    chosen_m = computeProd(mvec);
  long m = FindM(k, L, c, p, d, s, chosen_m, !noPrint);

  setDryRun(dry);
  for (long repeat_cnt = 0; repeat_cnt < repeat; repeat_cnt++) {
    TestIt(R, p, r, d, c, k, w, L, m, gens, ords);
  }
}