// Copyright (c) 2021 Graphcore Ltd. All rights reserved.

#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE BooleanOps
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(OrCase1) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(1);
  auto c = m.booleanOr(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(OrCase2) {
  Model m;
  auto a = m.addConstant(0);
  auto b = m.addConstant(0);
  auto c = m.booleanOr(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(OrCase3) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(0);
  auto c = m.booleanOr(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(OrCase4) {
  Model m;
  auto a = m.addConstant(0);
  auto b = m.addConstant(1);
  auto c = m.booleanOr(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(OrCase5) {
  Model m;
  auto a = m.addConstant(DataType::max());
  auto b = m.addConstant(DataType::max());
  auto c = m.booleanOr(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(AndCase1) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(1);
  auto c = m.booleanAnd(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(AndCase2) {
  Model m;
  auto a = m.addConstant(0);
  auto b = m.addConstant(0);
  auto c = m.booleanAnd(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(AndCase3) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.addConstant(0);
  auto c = m.booleanAnd(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(AndCase4) {
  Model m;
  auto a = m.addConstant(0);
  auto b = m.addConstant(1);
  auto c = m.booleanAnd(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{0});
}

BOOST_AUTO_TEST_CASE(AndCase5) {
  Model m;
  auto a = m.addConstant(DataType::max());
  auto b = m.addConstant(DataType::max());
  auto c = m.booleanAnd(a, b);
  auto s = m.minimize(c);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[c], DataType{1});
}

BOOST_AUTO_TEST_CASE(NotCase1) {
  Model m;
  auto a = m.addConstant(0);
  auto b = m.booleanNot(a);
  auto s = m.minimize(b);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[b], DataType{1});
}

BOOST_AUTO_TEST_CASE(NotCase2) {
  Model m;
  auto a = m.addConstant(1);
  auto b = m.booleanNot(a);
  auto s = m.minimize(b);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[b], DataType{0});
}

BOOST_AUTO_TEST_CASE(NotCase3) {
  Model m;
  auto a = m.addConstant(20000);
  auto b = m.booleanNot(a);
  auto s = m.minimize(b);
  BOOST_CHECK(s.validSolution());
  BOOST_CHECK_EQUAL(s[b], DataType{0});
}


//|
//|    Copyright (C) 2019 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
//|    Authors:  Konstantinos Chatzilygeroudis (maintainer)
//|              Bernardo Fichera
//|              Walid Amanhoud
//|    email:    costashatz@gmail.com
//|              bernardo.fichera@epfl.ch
//|              walid.amanhoud@epfl.ch
//|    Other contributors:
//|              Yoan Mollard (yoan@aubrune.eu)
//|    website:  lasa.epfl.ch
//|
//|    This file is part of iiwa_ros.
//|
//|    iiwa_ros is free software: you can redistribute it and/or modify
//|    it under the terms of the GNU General Public License as published by
//|    the Free Software Foundation, either version 3 of the License, or
//|    (at your option) any later version.
//|
//|    iiwa_ros is distributed in the hope that it will be useful,
//|    but WITHOUT ANY WARRANTY; without even the implied warranty of
//|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//|    GNU General Public License for more details.
//|
#include <Eigen/Dense>

#include <pluginlib/class_list_macros.hpp>

#include <iiwa_control/custom_effort_controller.hpp>

#include <Corrade/Containers/PointerStl.h>

#include <robot_controllers/CascadeController.hpp>
#include <robot_controllers/SumController.hpp>

namespace iiwa_control {
    template <class MatT>
    Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> pseudo_inverse(const MatT& mat, typename MatT::Scalar tolerance = typename MatT::Scalar{1e-4}) // choose appropriately
    {
        typedef typename MatT::Scalar Scalar;
        auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        const auto& singularValues = svd.singularValues();
        Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> singularValuesInv(mat.cols(), mat.rows());
        singularValuesInv.setZero();
        for (unsigned int i = 0; i < singularValues.size(); ++i) {
            if (singularValues(i) > tolerance) {
                singularValuesInv(i, i) = Scalar{1} / singularValues(i);
            }
            else {
                singularValuesInv(i, i) = Scalar{0};
            }
        }
        return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
    }

    std::vector<std::vector<std::string>> get_types(const std::string& input, const std::string& output)
    {
        std::vector<std::vector<std::string>> result(2);

        // Input
        std::string s = input;
        std::string delimiter = "|";

        size_t pos = 0;
        std::string token;
        while ((pos = s.find(delimiter)) != std::string::npos) {
            token = s.substr(0, pos);
            result[0].push_back(token);
            s.erase(0, pos + delimiter.length());
        }
        // add the last one to the list
        result[0].push_back(s);

        // Output
        s = output;
        delimiter = "|";

        pos = 0;
        token = "";
        while ((pos = s.find(delimiter)) != std::string::npos) {
            token = s.substr(0, pos);
            result[1].push_back(token);
            s.erase(0, pos + delimiter.length());
        }
        // add the last one to the list
        result[1].push_back(s);

        return result;
    }

    void set_types(CustomEffortController::ControllerPtr& ctrl, const std::vector<std::string>& input, const std::vector<std::string>& output)
    {
        robot_controllers::IOTypes input_type, output_type;
        for (auto& s : input) {
            if (s == "Position")
                input_type = input_type | robot_controllers::IOType::Position;
            else if (s == "Orientation")
                input_type = input_type | robot_controllers::IOType::Orientation;
            else if (s == "Velocity")
                input_type = input_type | robot_controllers::IOType::Velocity;
            else if (s == "AngularVelocity")
                input_type = input_type | robot_controllers::IOType::AngularVelocity;
            else if (s == "Acceleration")
                input_type = input_type | robot_controllers::IOType::Acceleration;
            else if (s == "AngularAcceleration")
                input_type = input_type | robot_controllers::IOType::AngularAcceleration;
            else if (s == "Force")
                input_type = input_type | robot_controllers::IOType::Force;
            else if (s == "Torque")
                input_type = input_type | robot_controllers::IOType::Torque;
        }

        for (auto& s : output) {
            if (s == "Position")
                output_type = output_type | robot_controllers::IOType::Position;
            else if (s == "Orientation")
                output_type = output_type | robot_controllers::IOType::Orientation;
            else if (s == "Velocity")
                output_type = output_type | robot_controllers::IOType::Velocity;
            else if (s == "AngularVelocity")
                output_type = output_type | robot_controllers::IOType::AngularVelocity;
            else if (s == "Acceleration")
                output_type = output_type | robot_controllers::IOType::Acceleration;
            else if (s == "AngularAcceleration")
                output_type = output_type | robot_controllers::IOType::AngularAcceleration;
            else if (s == "Force")
                output_type = output_type | robot_controllers::IOType::Force;
            else if (s == "Torque")
                output_type = output_type | robot_controllers::IOType::Torque;
        }

        ctrl->SetIOTypes(input_type, output_type);
    }

    void set_input_space(CustomEffortController::ControllerPtr& ctrl, size_t space_dim)
    {
        size_t input_dim = 0;

        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Position) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Force) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Torque) {
            input_dim += 3; // This is fixed to 3D
        }

        robot_controllers::RobotParams params = ctrl->GetParams();
        params.input_dim_ = input_dim;
        // TO-DO: We assume same input/output
        params.output_dim_ = input_dim;

        ctrl->SetParams(params);
    }

    CustomEffortController::CustomEffortController() {}

    CustomEffortController::~CustomEffortController() { sub_command_.shutdown(); }

    bool CustomEffortController::init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n)
    {
        // List of controlled joints
        std::string param_name = "joints";
        if (!n.getParam(param_name, joint_names_)) {
            ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
            return false;
        }
        n_joints_ = joint_names_.size();

        if (n_joints_ == 0) {
            ROS_ERROR_STREAM("List of joint names is empty.");
            return false;
        }

        // Get URDF
        urdf::Model urdf;
        if (!urdf.initParam("robot_description")) {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }

        // Get basic parameters
        n.param<std::string>("params/space", operation_space_, "joint"); // Default operation space is task-space

        // Check the operational space
        if (operation_space_ == "task") {
            space_dim_ = 3;

            // Get the URDF XML from the parameter server
            std::string urdf_string, full_param;
            std::string robot_description = "robot_description";
            std::string end_effector;

            // gets the location of the robot description on the parameter server
            if (!n.searchParam(robot_description, full_param)) {
                ROS_ERROR("Could not find parameter %s on parameter server", robot_description.c_str());
                return false;
            }

            // search and wait for robot_description on param server
            while (urdf_string.empty()) {
                ROS_INFO_ONCE_NAMED("CustomEffortController", "CustomEffortController is waiting for model"
                                                              " URDF in parameter [%s] on the ROS param server.",
                    robot_description.c_str());

                n.getParam(full_param, urdf_string);

                usleep(100000);
            }
            ROS_INFO_STREAM_NAMED("CustomEffortController", "Received urdf from param server, parsing...");

            // Get the end-effector
            n.param<std::string>("params/end_effector", end_effector, "iiwa_link_ee");

            // Initialize iiwa tools
            tools_.init_rbdyn(urdf_string, end_effector);
        }
        else
            space_dim_ = n_joints_;

        // Read Controllers from Params
        std::map<std::string, ControllerPtr> controllers;
        std::vector<std::string> ctrl_names;

        XmlRpc::XmlRpcValue symbols;

        n.getParam("controllers", symbols);

        assert(symbols.getType() == XmlRpc::XmlRpcValue::TypeStruct);
        for (XmlRpc::XmlRpcValue::iterator i = symbols.begin(); i != symbols.end(); ++i) {
            // ROS_WARN_STREAM(i->first << ": " << i->second.getType());
            std::string name = i->first;
            std::string type, input, output;
            std::vector<double> param_values;
            n.getParam("controllers/" + name + "/type", type);
            n.getParam("controllers/" + name + "/params", param_values);
            n.getParam("controllers/" + name + "/input", input);
            n.getParam("controllers/" + name + "/output", output);

            if (type.size() == 0) {
                ROS_WARN_STREAM("Could not find type of controller '" << name << "'. Skipping this controller!");
                continue;
            }

            auto ctrl = manager_.loadAndInstantiate(type);

            if (ctrl) {
                robot_controllers::RobotParams params;
                params.input_dim_ = space_dim_;
                params.output_dim_ = space_dim_;

                params.time_step_ = 0.01; // TO-DO: Get this from controller manager or yaml

                params.values_ = param_values;

                ctrl->SetParams(params);

                // TO-DO: Maybe separate input and output
                if (input.size() > 0 && output.size() > 0) {
                    std::vector<std::vector<std::string>> tt = get_types(input, output);
                    if (tt.size() == 2)
                        set_types(ctrl, tt[0], tt[1]);
                }

                set_input_space(ctrl, space_dim_);

                controllers[name] = std::move(ctrl);
                ctrl_names.push_back(name);
            }
        }

        XmlRpc::XmlRpcValue symbols_structure;
        n.getParam("structure", symbols_structure);

        if (symbols_structure.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
            for (XmlRpc::XmlRpcValue::iterator i = symbols_structure.begin(); i != symbols_structure.end(); ++i) {
                // ROS_WARN_STREAM(i->first << ": " << i->second.getType());
                std::string name = i->first;
                std::vector<std::string> sub;
                n.getParam("structure/" + name, sub);
                bool is_sum = false;
                if (name.find("Add") == 0) {
                    controllers[name] = ControllerPtr(new robot_controllers::SumController);
                    is_sum = true;
                }
                else if (name.find("Cascade") == 0) {
                    controllers[name] = ControllerPtr(new robot_controllers::CascadeController);
                }
                else {
                    ROS_WARN_STREAM("Cannot identify the type of the controller by the name: '" << name << "'. Ignoring!");
                    continue;
                }
                // std::cout << sub.size() << std::endl;
                for (size_t k = 0; k < sub.size(); k++) {
                    // ROS_WARN_STREAM("    " << sub[k]);
                    if (is_sum)
                        static_cast<robot_controllers::SumController*>(controllers[name].get())->AddController(std::move(controllers[sub[k]]));
                    else
                        static_cast<robot_controllers::CascadeController*>(controllers[name].get())->AddController(std::move(controllers[sub[k]]));
                    ctrl_names.erase(std::remove(ctrl_names.begin(), ctrl_names.end(), sub[k]), ctrl_names.end());
                    controllers.erase(sub[k]);
                }

                // Initialize parameters
                robot_controllers::RobotParams params;
                params.input_dim_ = space_dim_;
                params.output_dim_ = space_dim_;

                params.time_step_ = 0.01; // TO-DO: Get this from controller manager or yaml
                controllers[name]->SetParams(params);

                set_input_space(controllers[name], space_dim_);

                ctrl_names.push_back(name);
            }
        }

        null_space_control_ = false;
        if (operation_space_ == "task") {
            std::vector<double> joints;
            n.getParam("params/null_space/joints", joints);
            null_space_control_ = (joints.size() == n_joints_);

            if (null_space_control_) {
                null_space_joint_config_ = Eigen::VectorXd::Map(joints.data(), joints.size());

                null_space_Kp_ = 20.;
                null_space_Kd_ = 0.1;
                null_space_max_torque_ = 10.;

                n.getParam("params/null_space/Kp", null_space_Kp_);
                n.getParam("params/null_space/Kp", null_space_Kd_);
                n.getParam("params/null_space/max_torque", null_space_max_torque_);
            }
        }

        unsigned int ctrl_size = ctrl_names.size();

        if (ctrl_size == 0) {
            ROS_ERROR_STREAM("Could not load specified controllers! Exiting..!");
            return false;
        }

        if (ctrl_size == 1) {
            controller_ = std::move(controllers[ctrl_names[0]]);

            set_input_space(controller_, space_dim_);
        }
        else {
            controller_.reset(new robot_controllers::SumController);
            for (unsigned int i = 0; i < ctrl_size; i++) {
                static_cast<robot_controllers::SumController*>(controller_.get())->AddController(std::move(controllers[ctrl_names[i]]));
            }

            set_input_space(controller_, space_dim_);
        }

        // Initialize the controller(s)
        if (!controller_->Init()) {
            ROS_ERROR("Controllers could not be initialized! Exiting!");
            return false;
        }

        for (unsigned int i = 0; i < n_joints_; i++) {
            try {
                joints_.push_back(hw->getHandle(joint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException& e) {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

            urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
            if (!joint_urdf) {
                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                return false;
            }
            joint_urdfs_.push_back(joint_urdf);
        }

        // Get controller command size
        cmd_dim_ = 0;

        if (controller_->GetInput().GetType() & robot_controllers::IOType::Position) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Force) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Torque) {
            cmd_dim_ += 3; // This is fixed to 3D
        }

        std::vector<double> init_cmd(cmd_dim_, 0.0);
        has_orientation_ = false;
        if (operation_space_ == "task") {
            has_orientation_ = ((controller_->GetInput().GetType() & robot_controllers::IOType::Orientation)) ? true : false;
            bool has_position = ((controller_->GetInput().GetType() & robot_controllers::IOType::Position)) ? true : false;
            if (has_position || has_orientation_) {
                // if task space, we need to alter the initial command
                iiwa_tools::RobotState robot_state;
                robot_state.position.resize(n_joints_);
                robot_state.velocity.resize(n_joints_);

                for (size_t i = 0; i < n_joints_; i++) {
                    robot_state.position[i] = joints_[i].getPosition();
                    robot_state.velocity[i] = joints_[i].getVelocity();
                }

                auto ee_state = tools_.perform_fk(robot_state);
                Eigen::AngleAxisd aa(ee_state.orientation);
                Eigen::VectorXd o = aa.axis() * aa.angle();
                Eigen::VectorXd p = ee_state.translation;

                size_t offset = 0;
                if (has_orientation_)
                    offset = 3;

                for (size_t i = 0; i < 3; i++) {
                    if (has_orientation_)
                        init_cmd[i] = o(i);
                    if (has_position)
                        init_cmd[i + offset] = p(i);
                }
            }
        }

        ROS_INFO_STREAM("Initial command: " << Eigen::VectorXd::Map(init_cmd.data(), init_cmd.size()).transpose());

        commands_buffer_.writeFromNonRT(init_cmd);

        sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &CustomEffortController::commandCB, this);

        return true;
    }

    void CustomEffortController::update(const ros::Time& time, const ros::Duration& period)
    {
        std::vector<double>& commands = *commands_buffer_.readFromRT();

        Eigen::MatrixXd jac(6, n_joints_);
        Eigen::MatrixXd jac_deriv(6, n_joints_);
        Eigen::MatrixXd jac_t_pinv(n_joints_, 6);
        Eigen::VectorXd eef(6);

        if (operation_space_ == "task") {
            iiwa_tools::RobotState robot_state;
            robot_state.position.resize(n_joints_);
            robot_state.velocity.resize(n_joints_);

            for (size_t i = 0; i < n_joints_; i++) {
                robot_state.position[i] = joints_[i].getPosition();
                robot_state.velocity[i] = joints_[i].getVelocity();
            }

            std::tie(jac, jac_deriv) = tools_.jacobians(robot_state);
            jac_t_pinv = pseudo_inverse(Eigen::MatrixXd(jac.transpose()));
            auto ee_state = tools_.perform_fk(robot_state);
            Eigen::AngleAxisd aa(ee_state.orientation);
            eef.head(3) = aa.axis() * aa.angle();
            eef.tail(3) = ee_state.translation;
        }

        Eigen::VectorXd cmd(n_joints_);

        cmd = Eigen::VectorXd::Map(commands.data(), commands.size());

        robot_controllers::RobotState curr_state, robot_state;
        curr_state.position_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.velocity_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.acceleration_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.force_ = Eigen::VectorXd::Zero(n_joints_);

        for (unsigned int i = 0; i < n_joints_; i++) {
            curr_state.position_(i) = joints_[i].getPosition();
            curr_state.velocity_(i) = joints_[i].getVelocity();
            // curr_state.acceleration_(i) = joints_[i].getAcceleration();
            // TO-DO: Fill acceleration
            curr_state.force_(i) = joints_[i].getEffort();
        }

        if (operation_space_ == "task") {
            if (null_space_control_)
                robot_state = curr_state;

            Eigen::VectorXd pos = eef;
            Eigen::VectorXd vel = jac * curr_state.velocity_;
            Eigen::VectorXd acc = jac * curr_state.acceleration_ + jac_deriv * curr_state.velocity_;
            Eigen::VectorXd f = jac_t_pinv * curr_state.force_; // TO-DO: This is not perfect, but should be enough

            curr_state.position_ = pos.tail(3);
            curr_state.velocity_ = vel.tail(3);
            curr_state.acceleration_ = acc.tail(3);
            curr_state.force_ = f.tail(3);

            if (has_orientation_) {
                curr_state.orientation_ = pos.head(3);
                curr_state.angular_velocity_ = vel.head(3);
                curr_state.angular_acceleration_ = acc.head(3);
                curr_state.torque_ = f.head(3);
            }
        }

        // Update desired state in controller
        robot_controllers::RobotState desired_state;
        unsigned int size = curr_state.position_.size();
        unsigned int index = 0;
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            desired_state.orientation_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Position) {
            desired_state.position_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            desired_state.angular_velocity_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            desired_state.velocity_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            desired_state.angular_acceleration_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            desired_state.acceleration_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Torque) {
            desired_state.torque_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Force) {
            desired_state.force_ = cmd.segment(index, size);
            // index += size;
        }

        controller_->SetInput(desired_state);

        // Update control torques given current velocity
        controller_->Update(curr_state);

        Eigen::VectorXd output; // = Eigen::VectorXd::Zero(space_dim_);
        if (operation_space_ == "task") {
            output = Eigen::VectorXd::Zero(2 * space_dim_);
            if (controller_->GetOutput().GetType() & robot_controllers::IOType::Force)
                output.tail(3) = controller_->GetOutput().desired_.force_;
            if (controller_->GetOutput().GetType() & robot_controllers::IOType::Torque)
                output.head(3) = controller_->GetOutput().desired_.torque_;
        }
        else // regular controller
            output = controller_->GetOutput().desired_.force_;

        if (operation_space_ == "task") {
            // output.head(3) = Eigen::VectorXd::Zero(3);
            output = jac.transpose() * output;

            // Add null-space signal if wanted
            if (null_space_control_) {
                Eigen::VectorXd null_space_signal = null_space_Kp_ * (null_space_joint_config_ - robot_state.position_) - null_space_Kd_ * robot_state.velocity_;
                Eigen::VectorXd null_space_force = (Eigen::MatrixXd::Identity(n_joints_, n_joints_) - jac.transpose() * jac_t_pinv) * null_space_signal;
                for (int i = 0; i < null_space_force.size(); i++) {
                    if (null_space_force(i) > null_space_max_torque_)
                        null_space_force(i) = null_space_max_torque_;
                    else if (null_space_force(i) < -null_space_max_torque_)
                        null_space_force(i) = -null_space_max_torque_;
                }
                output = output + null_space_force;
            }
        }

        // ROS_INFO_STREAM("Effort: " << output.transpose());

        std::vector<double> commanded_effort(n_joints_, 0.);

        Eigen::VectorXd::Map(commanded_effort.data(), commanded_effort.size()) = output;

        for (unsigned int i = 0; i < n_joints_; i++) {
            enforceJointLimits(commanded_effort[i], i);
            joints_[i].setCommand(commanded_effort[i]);
        }
    }

    void CustomEffortController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
    {
        if (msg->data.size() != cmd_dim_) {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") is not correct! Not executing!");
            return;
        }

        commands_buffer_.writeFromNonRT(msg->data);
    }

    void CustomEffortController::enforceJointLimits(double& command, unsigned int index)
    {
        // Check that this joint has applicable limits
        if (command > joint_urdfs_[index]->limits->effort) // above upper limit
        {
            command = joint_urdfs_[index]->limits->effort;
        }
        else if (command < -joint_urdfs_[index]->limits->effort) // below lower limit
        {
            command = -joint_urdfs_[index]->limits->effort;
        }
    }
} // namespace iiwa_control

PLUGINLIB_EXPORT_CLASS(iiwa_control::CustomEffortController, controller_interface::ControllerBase)


#pragma once
#include <model.hpp>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <utils.hpp>
#include <exception>
#include <vector>
#include <iostream>
#include <fstream>

namespace dummyml
{

class gaussian_process : public Model
{
private:
    EigenMatrix _x;
    EigenMatrix _y;
    EigenMatrix _C_inv;
    double _alpha;
    std::unique_ptr<kernel> _kernel;
public:
    struct MetaData{
        Eigen::Index _n;
        Eigen::Index _f;
        kernel::type _t;
        double _a;
        Eigen::Index check_sum;
        MetaData() = default;
        MetaData(const gaussian_process& model):
            _n(model._x.rows()),
            _f(model._x.cols()),
            _t(model._kernel->_T),
            _a(model._alpha),
            check_sum(
                model._x.rows() ^
                model._x.cols() ^
                model._kernel->_T
            ){}
        bool is_check_sum_correct(){
            return (_n ^ _f ^ _t) == check_sum;
        }
    };
    gaussian_process(const char* filename){
        load(filename);
    }
    gaussian_process(
        double alpha = 0.2,
        kernel::type k_type = kernel::type::LinearKernel
    ): _alpha(alpha), _kernel(get_kernel(k_type)){}
    gaussian_process(
        nparray_d x,
        nparray_d y,
        double alpha = 0.2,
        kernel::type k_type = kernel::type::LinearKernel
    ): _alpha(alpha), _kernel(get_kernel(k_type)){
        fit(x, y);
    }
    double run_kernel(double x, double y){
        return (*_kernel)(x,y);
    }
    void load(const char* file_name){
        std::fstream fin_bin(file_name,std::ios_base::in | std::ios_base::binary);
        if(fin_bin.fail()){
            throw std::runtime_error(
                "[ERROR] gaussian_process load: failed to load model."
            );
        }
        MetaData meta;
        fin_bin.read(
            dummy_cast<char*,MetaData*>(&meta),
            sizeof(MetaData)
        );
        if(!meta.is_check_sum_correct()){
            throw std::runtime_error(
                "[ERROR] gaussian_process load: MetaData mismatch."
            );
        }
        _alpha = meta._a;
        _kernel = get_kernel(meta._t);
        _x = EigenMatrix(meta._n, meta._f);
        _y = EigenMatrix(meta._n, 1);
        _C_inv = EigenMatrix(meta._n, meta._n);
        fin_bin.read(
            dummy_cast<char*,double*>(_x.data()),
            sizeof(double) * _x.size()
        ).read(
            dummy_cast<char*,double*>(_y.data()),
            sizeof(double) * _y.size()
        ).read(
            dummy_cast<char*,double*>(_C_inv.data()),
            sizeof(double) * _C_inv.size()
        );
        return;
    }
    void save(const char* file_name){
        std::fstream fout_bin(file_name,std::ios_base::out | std::ios_base::binary);
        if(fout_bin.fail()){
            throw std::runtime_error(
                "[ERROR] gaussian_process save: failed to save model."
            );
        }
        MetaData meta(*this);
        fout_bin.write(
            dummy_cast<char*,MetaData*>(&meta),
            sizeof(MetaData)
        ).write(
            dummy_cast<char*,double*>(_x.data()),
            sizeof(double) * _x.size()
        ).write(
            dummy_cast<char*,double*>(_y.data()),
            sizeof(double) * _y.size()
        ).write(
            dummy_cast<char*,double*>(_C_inv.data()),
            sizeof(double) * _C_inv.size()
        );
        return;
    }
    void fit(nparray_d x, nparray_d y){
        auto x_buf_info = x.request();
        auto y_buf_info = y.request();
        if(x_buf_info.shape[0] != y_buf_info.shape[0]){
            throw std::length_error(
                "[ERROR] gaussian_process fit: data & label counts mismatch."
            );
        }
        size_t dataset_size = x_buf_info.shape[0];
        size_t feature_size = x_buf_info.shape[1];
        double* x_ptr = (double*)x_buf_info.ptr;
        double* y_ptr = (double*)y_buf_info.ptr;

        // copy x,y to _x,_y
        _x.resize(dataset_size, feature_size);
        _y.resize(dataset_size, 1);
        _C_inv.resize(dataset_size, dataset_size);
        memcpy(_x.data(), x_ptr, dataset_size * feature_size * sizeof(double));
        memcpy(_y.data(), y_ptr, dataset_size                * sizeof(double));

        // calculate _C_inv
        for(size_t row = 0;row < dataset_size; ++row)
            for(size_t col = row;col < dataset_size; ++col)
                _C_inv(col, row) = _C_inv(row, col) = (*_kernel)(_x.row(row), _x.row(col));
        for(size_t diag = 0;diag < dataset_size; ++diag)
            _C_inv(diag ,diag) += _alpha;
        _C_inv = _C_inv.inverse();
        return;
    }
    nparray_d operator()(nparray_d x){
        auto x_buf_info = x.request();
        size_t dataset_size = _x.rows();
        size_t feature_size = _x.cols();
        if(x_buf_info.shape[0] != feature_size){
            throw std::length_error(
                "[ERROR] gaussian_process operator(): x size & feature size mismatch."
            );
        }
        EigenVector x_vec(feature_size);
        EigenMatrix kt_vec(1, dataset_size);
        memcpy(x_vec.data(), x_buf_info.ptr, _x.cols()*sizeof(double));

        for(size_t i = 0;i < dataset_size;++i)
            kt_vec(0, i) = (*_kernel)(_x.row(i), x_vec);
        EigenMatrix ktC_inv = kt_vec * _C_inv;
        nparray_d result(2);
        double* result_ptr = (double*)result.request().ptr;
        result_ptr[0] = (ktC_inv * _y)(0);
        result_ptr[1] =
            ((*_kernel)(x_vec, x_vec) + _alpha) -  // c
            (ktC_inv * kt_vec.transpose())(0);// kt * C^-1 * k
        return result;
    }
    void set_alpha(double alpha){
        _alpha = alpha;
    }
};

} // namespace dummyml

void export_gaussian_process(py::module_ &);


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP

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

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename RadiusType,
    typename CalculationType = void
>
class vincenty
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef RadiusType radius_type;

    inline vincenty()
    {}

    explicit inline vincenty(geometry::detail::ellipsoid<RadiusType> const& e)
        : m_ellipsoid(e)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return calculate<typename calculation_type<Point1, Point2>::type>
            (
                get_as_radian<0>(point1), get_as_radian<1>(point1),
                get_as_radian<0>(point2), get_as_radian<1>(point2)
            );
    }

    inline geometry::detail::ellipsoid<RadiusType> ellipsoid() const
    {
        return m_ellipsoid;
    }

    inline RadiusType radius() const
    {
        // For now return the major axis. It is used in distance_cross_track, from point-to-line
        return m_ellipsoid.a();
    }

private :
    geometry::detail::ellipsoid<RadiusType> m_ellipsoid;

    template <typename CT, typename T>
    inline CT calculate(T const& lon1,
                T const& lat1,
                T const& lon2,
                T const& lat2) const
    {
        CT const c2 = 2;
        CT const pi = geometry::math::pi<CT>();
        CT const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        CT L = lon2 - lon1;
        CT lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return CT(0);
        }

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const c1 = 1;
        CT const one_min_f = c1 - m_ellipsoid.f();

        CT const U1 = atan(one_min_f * tan(lat1)); // above (1)
        CT const U2 = atan(one_min_f * tan(lat2)); // above (1)

        CT const cos_U1 = cos(U1);
        CT const cos_U2 = cos(U2);
        CT const sin_U1 = sin(U1);
        CT const sin_U2 = sin(U2);

        // alpha: azimuth of the geodesic at the equator
        CT cos2_alpha;
        CT sin_alpha;

        // sigma: angular distance p1,p2 on the sphere
        // sigma1: angular distance on the sphere from the equator to p1
        // sigma_m: angular distance on the sphere from the equator to the midpoint of the line
        CT sigma;
        CT sin_sigma;
        CT cos2_sigma_m;

        CT previous_lambda;

        CT const c3 = 3;
        CT const c4 = 4;
        CT const c6 = 6;
        CT const c16 = 16;

        CT const c_e_12 = 1e-12;

        do
        {
            previous_lambda = lambda; // (13)
            CT sin_lambda = sin(lambda);
            CT cos_lambda = cos(lambda);
            sin_sigma = sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            CT cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos2_sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)

            CT C = m_ellipsoid.f()/c16 * cos2_alpha * (c4 + m_ellipsoid.f() * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * m_ellipsoid.f() * sin_alpha *
                (sigma + C * sin_sigma * ( cos2_sigma_m + C * cos_sigma * (-c1 + c2 * math::sqr(cos2_sigma_m)))); // (11)

        } while (geometry::math::abs(previous_lambda - lambda) > c_e_12
                && geometry::math::abs(lambda) < pi);

        CT sqr_u = cos2_alpha * (math::sqr(m_ellipsoid.a()) - math::sqr(m_ellipsoid.b())) / math::sqr(m_ellipsoid.b()); // above (1)

        // Oops getting hard here
        // (again, problem is that ttmath cannot divide by doubles, which is OK)
        CT const c47 = 47;
        CT const c74 = 74;
        CT const c128 = 128;
        CT const c256 = 256;
        CT const c175 = 175;
        CT const c320 = 320;
        CT const c768 = 768;
        CT const c1024 = 1024;
        CT const c4096 = 4096;
        CT const c16384 = 16384;

        CT A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
        CT B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
        CT delta_sigma = B * sin_sigma * ( cos2_sigma_m + (B/c4) * (cos(sigma)* (-c1 + c2 * cos2_sigma_m)
                - (B/c6) * cos2_sigma_m * (-c3 + c4 * math::sqr(sin_sigma)) * (-c3 + c4 * cos2_sigma_m))); // (6)

        return m_ellipsoid.b() * A * (sigma - delta_sigma); // (19)
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename RadiusType, typename CalculationType>
struct tag<vincenty<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<RadiusType, CalculationType>, P1, P2>
    : vincenty<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<vincenty<RadiusType, CalculationType> >
{
    typedef vincenty<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<vincenty<RadiusType, CalculationType> >
{
    static inline vincenty<RadiusType, CalculationType> apply(vincenty<RadiusType, CalculationType> const& input)
    {
        return input;
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<RadiusType, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<RadiusType, CalculationType>, P1, P2>::type
        apply(vincenty<RadiusType, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP


#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <Eigen/Dense>
#include <pcl/visualization/pcl_visualizer.h>
#include "kuri_mbzirc_challenge_2_exploration/panel_searching.h"

// Cluster the input pointcloud(raw input)
void panel_searching::pointcloud_tree_clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered, std::vector<pcl::PointIndices>& cluster_indices)
{
  std::cout << "PointCloud before filtering has: " << input_pointcloud->points.size () << " data points." << std::endl; //*

  // Create the filtering object: downsample the dataset using a leaf size of 1cm

  pcl::VoxelGrid<pcl::PointXYZ> vg;
  vg.setInputCloud (input_pointcloud);
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  // If the cluster tolerance is too small the algorithm will fail to find anything or divid one object into multiple laysers
  // Since the Velodyne has a lower resolution between layers
  ec.setClusterTolerance (0.2); // 20cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);
  std::cout<<"cluster no. :"<<cluster_indices.size()<<std::endl;
}

// Fit a plane model to each cluster
void panel_searching::plane_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud, int& plane_id,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& pc_vector)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new      pcl::PointCloud<pcl::PointXYZ>);
  // Create the segmentation object for the planar model and set all the parameters
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::PCDWriter writer;
  seg.setOptimizeCoefficients (true);
  // vertial plane fitting
  seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
  seg.setAxis(Eigen::Vector3f(0,0,1));
  seg.setEpsAngle (0.4);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.02);
  int id=0;
  int nr_points = (int) input_pointcloud->points.size ();
  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> local_pc_vector;
  while (input_pointcloud->points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (input_pointcloud);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }
    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (input_pointcloud);
    extract.setIndices (inliers);
    extract.setNegative (false);

    // Get the points associated with the planar surface
    extract.filter (*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
    ++plane_id;
    // save the pc in the local pc vector
    local_pc_vector.push_back(cloud_plane);
    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *input_pointcloud = *cloud_f;
  }

  if (local_pc_vector.size()<3)
  {
    std::cout << "plane size"<<std::endl;
    std::cout << local_pc_vector.size()<<std::endl;
    std::cout << "plane appended"<<std::endl;
    pc_vector.insert(pc_vector.end(), local_pc_vector.begin(), local_pc_vector.end());

  }
}

//Find the bounding box for each planar point cloud in order to identify the: Dimension, centroid, and density.
void panel_searching::bounding_box_detection(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& pc_vector,std::vector<Eigen::Vector3f>& dimension_list, std::vector<Eigen::Vector4f>& centroid_list)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::PCDWriter writer;

 int id=0;

  // initiate vectors for centroids, dimensions, and densities
  //  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pc_vector;

std::vector<double> density_list;
Eigen::Vector3f one_dimension;
 for (std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iterator = pc_vector.begin(), end = pc_vector.end(); iterator != end; ++iterator)
  {
     cloud_plane=*iterator;
    ++id;
    //detect bounding box
    std::cout<<"Start finding bounding box"<<std::endl;
    // find bounding box
    // Compute principal directions
    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud_plane, pcaCentroid);

    Eigen::Matrix3f covariance;
    computeCovarianceMatrixNormalized(*cloud_plane, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));

    // Transform the original cloud to the origin where the principal components correspond to the axes.
    Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
    projectionTransform.block<3,3>(0,0) = eigenVectorsPCA.transpose();
    projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * pcaCentroid.head<3>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud_plane, *cloudPointsProjected, projectionTransform);
    // Get the minimum and maximum points of the transformed cloud.
    pcl::PointXYZ minPoint, maxPoint;

    pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
    const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());

    // Final transform
    const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/  watch?v=mHVwd8gYLnI
    const Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();
    // save the centroid into the centroid vector
    centroid_list.push_back(pcaCentroid);
    // save dimenstion into dimension list
    one_dimension(0,0)=maxPoint.x - minPoint.x;
    one_dimension(1,0)=maxPoint.y - minPoint.y;
    one_dimension(2,0)=maxPoint.z - minPoint.z;
    dimension_list.push_back(one_dimension);
    // save density into density list
    density_list.push_back(cloud_plane->points.size()/(one_dimension(0,0)*one_dimension(1,0)*one_dimension(2,0)));
  }
}

//Decide if a planar pointcloud could be the goal of not with the following criteria:
//1. Dimension in x and y. 2. Ratio of x and y
void panel_searching::assembler(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& pc_vector, pcl::PointCloud<pcl::PointXYZ>::Ptr plane_assemble,std::vector<Eigen::Vector3f>& dimension_list, std::vector<Eigen::Vector4f>& centroid_list,std::vector<Eigen::Vector4f>& sifted_centroid_list)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  Eigen::Vector3f one_dimension;
  Eigen::Vector4f one_centroid;
  int idx=0;
  int pass_count=0;

  for (std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iterator = pc_vector.begin(), end = pc_vector.end(); iterator != end; ++iterator)
  {
    cloud_plane=*iterator;
    //filtered with plane dimension
    one_dimension=dimension_list[idx];
    one_centroid=centroid_list[idx];
    std::cout<<one_dimension<<std::endl;
    std::cout<<"inside"<<std::endl;
    std::cout<<one_dimension[0,1]<<std::endl;
    std::cout<<one_dimension[0,2]<<std::endl;
    if( one_dimension[0,1]<1.5  && one_dimension[0,2]<1.5 && one_dimension[0,2]/one_dimension[0,1]<2&&one_dimension[0,1]>0.5)
    {
      //append centroid list to the sifted_centroid_list

      sifted_centroid_list.push_back(one_centroid);
      //append points from single plane to the output pointcloud
      for (size_t i = 0; i < cloud_plane->points.size(); ++i)
      {
        //create single point
        pcl::PointXYZ p;
        p.x = cloud_plane->points[i].x;
        p.y = cloud_plane->points[i].y;
        p.z = cloud_plane->points[i].z;
        //assign pointer value
        plane_assemble->points.push_back(p);
        ++plane_assemble->width;
      }
      ++idx;
    }
  }
}

// Main function. The whole workflow is:
//1. Cluster the input raw pointcloud data. 2. Fit vertical plane to each cluster. 3. Find bounding box for each planar pointcloud. 4. Decide if a planar pointcloud could be the goal of not with pre-defined criteria
void panel_searching::panel_extraction(pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud, std::vector<pcl::PointIndices> cluster_indices, pcl::PointCloud<pcl::PointXYZ>::Ptr plane_assemble,std::vector<Eigen::Vector4f>& sifted_centroid_list)
{
  pcl::PCDWriter writer;
  //create a list of point cloud for bounding box detection
  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pc_vector;

  int plane_id=0;
  int j = 0;
  int id=0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
      cloud_cluster->points.push_back (input_pointcloud->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;
    id++;
    j++;
    // fit planer
    plane_fitting(cloud_cluster, plane_id,pc_vector);
  }
  // find bounding box
  std::vector<Eigen::Vector3f> dimension_list;
  std::vector<Eigen::Vector4f> centroid_list;
  bounding_box_detection(input_pointcloud, pc_vector, dimension_list, centroid_list);
  Eigen::Vector3f one_dimension;
  Eigen::Vector4f one_centroid;
  one_centroid=centroid_list[0];
  assembler(pc_vector, plane_assemble,dimension_list, centroid_list, sifted_centroid_list );
}


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

#include "theia/sfm/estimators/estimate_similarity_transformation_2d_3d.h"

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

#include <limits>
#include <memory>
#include <vector>

#include "theia/sfm/camera/camera.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/feature.h"
#include "theia/sfm/similarity_transformation.h"
#include "theia/sfm/transformation/gdls_similarity_transform.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

inline void TransformCamera(const SimilarityTransformation& sim_transform,
                            Camera* camera) {
  const Eigen::Vector3d old_position = camera->GetPosition();
  const Eigen::Vector3d new_position =
      sim_transform.scale * sim_transform.rotation *
      old_position + sim_transform.translation;
  camera->SetPosition(new_position);

  const Eigen::Matrix3d old_orientation =
      camera->GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d new_orientation =
      old_orientation * sim_transform.rotation.transpose();
  camera->SetOrientationFromRotationMatrix(new_orientation);
}

// An estimator for computing the similarity transformation from 4 2D-3D
// correspondences using the gDLS algorithm to minimize reprojection error.
class GdlsSimilarityTransformationEstimator
    : public Estimator<CameraAndFeatureCorrespondence2D3D,
                       SimilarityTransformation> {
 public:
  GdlsSimilarityTransformationEstimator() {}

  // 3 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<CameraAndFeatureCorrespondence2D3D>& correspondences,
      std::vector<SimilarityTransformation>* similarity_transformations) const {
    std::vector<Eigen::Vector3d> ray_origins(4), ray_directions(4),
        world_points(4);
    for (int i = 0; i < 4; i++) {
      ray_origins[i] = correspondences[i].camera.GetPosition();
      ray_directions[i] = correspondences[i].camera.PixelToUnitDepthRay(
          correspondences[i].observation).normalized();
      world_points[i] = correspondences[i].point3d.hnormalized();
    }

    // Compute the similarity transformation. Note that this function computes
    // R, t, and s such that:
    //
    //   s * c_i + alpha_i * x_i = R * X_i + t
    //
    // where c_i is the camera position, alpha_i is the depth of the feature,
    // x_i is the unit-norm feature observation, and X_i is the 3D point.
    std::vector<Eigen::Quaterniond> rotations;
    std::vector<Eigen::Vector3d> translations;
    std::vector<double> scales;
    GdlsSimilarityTransform(ray_origins,
                            ray_directions,
                            world_points,
                            &rotations,
                            &translations,
                            &scales);

    // Aggregate the solutions, modifying the output so that R, t, s are of the
    // more useful form of:
    //
    //   s * R * (c_i + alpha_i * x_i) + t = X_i
    //
    // which transforms only the camera coordinate system so that it is aligned
    // with the 3D points.
    for (int i = 0; i < rotations.size(); i++) {
      SimilarityTransformation similarity_transformation;
      similarity_transformation.rotation =
          rotations[i].toRotationMatrix().transpose();
      similarity_transformation.translation =
          similarity_transformation.rotation * -translations[i];
      similarity_transformation.scale = scales[i];
      similarity_transformations->emplace_back(similarity_transformation);
    }
    return similarity_transformations->size() > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(
      const CameraAndFeatureCorrespondence2D3D& correspondence,
      const SimilarityTransformation& similarity_transformation) const {
    // Apply the similarity transformation to the camera.
    Camera transformed_camera = correspondence.camera;
    TransformCamera(similarity_transformation, &transformed_camera);

    Eigen::Vector2d reprojection;
    // If the point is reprojected behind the camera, return the maximum
    // possible error.
    if (transformed_camera.ProjectPoint(correspondence.point3d, &reprojection) <
        0) {
      return std::numeric_limits<double>::max();
    }

    // Return the squared reprojection error.
    return (correspondence.observation - reprojection).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(GdlsSimilarityTransformationEstimator);
};

}  // namespace

bool EstimateSimilarityTransformation2D3D(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<CameraAndFeatureCorrespondence2D3D>& correspondences,
    SimilarityTransformation* similarity_transformation,
    RansacSummary* ransac_summary) {
  GdlsSimilarityTransformationEstimator similarity_transformation_estimator;
  std::unique_ptr <
      SampleConsensusEstimator<GdlsSimilarityTransformationEstimator> > ransac =
      CreateAndInitializeRansacVariant(ransac_type,
                                       ransac_params,
                                       similarity_transformation_estimator);
  // Estimate the absolute pose.
  return ransac->Estimate(correspondences,
                          similarity_transformation,
                          ransac_summary);
}

}  // namespace theia


#include <cmath>
#include <vector>
#include <string>
#include "multi_lidar_calib/common.h"
#include "multi_lidar_calib/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/passthrough.h>    ///直通滤波相关
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

 #include <ceres/ceres.h>
 #include "lidarFactor.hpp"

#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include<vector>

#include<mutex>

#define PI 3.1415926

using std::atan2;
using std::cos;
using std::sin;

double lidar_z_height = -1;

ros::Publisher  pubFittedPlane;

double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; // 激光雷达间相对位姿关系

Eigen::Map<Eigen::Quaterniond> q_to_be_optimized(parameters);
Eigen::Map<Eigen::Vector3d> t_to_be_optimized(parameters + 4);

void planeFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

}

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

    ros::init(argc, argv, "multi_lidar_calibration");
	ros::NodeHandle nh;
    pubFittedPlane = nh.advertise<sensor_msgs::PointCloud2>("/fitted_plane", 100);

    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::LocalParameterization *q_parameterization =
                new ceres::EigenQuaternionParameterization();
            ceres::Problem::Options problem_options;

            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters, 4, q_parameterization);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<36; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = "/media/mjy/Samsung_T5/linux/DX/data/0812forcalib/pcd_select/";
                std::string filename_pt1 = "/media/mjy/Samsung_T5/linux/DX/data/0812forcalib/pcd_select/";
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                planeFitting(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                planeFitting(cloud1,coefficients1);

                // 两平面法线的夹角作为一个损失值
                Eigen::Vector3d norm0(coefficients0->values[0], coefficients0->values[1], coefficients0->values[2]);
                Eigen::Vector3d norm1(coefficients1->values[0], coefficients1->values[1], coefficients1->values[2]);
                ceres::CostFunction *cost_function;
                cost_function = LidarNormFactor::Create(norm0, norm1);
                problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }

    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
    std::cout<<q_to_be_optimized.matrix()<<std::endl;

    return 0;
}


/*
   Copyright 2015 Ruben Moreno Montoliu <ruben3d at gmail dot com>

   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.
*/

//
// DiskAreaLight.cpp - Created on 2010.02.21
//

#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>
#include "DiskAreaLight.h"
#include "Object.h"
#include "Polar3.h"
#include "Random.h"
#include "Ray.h"
#include "Disk.h"
#include "Material.h"

unsigned int DiskAreaLight::GlobalCounter = 0;

DiskAreaLight::DiskAreaLight(const Transform& tr, const double intensity, const double exponent, const Color& color,
								const double radius, const bool show, const Attenuation attenuation)
		: Light(tr, intensity, color), LightHasPosition(tr(Point(0,0,0))), LightHasAttenuation(attenuation),
		LightHasGeometry(show), LightHasDirection(tr(Vector(0,0,1)),exponent), m_radius(radius),
		m_radiusSq(radius*radius), m_minSamples(16), m_maxSamples(32)
{
	Light::addAttribute(LightHasPosition::ATTRIBUTE);
	Light::addAttribute(LightHasDirection::ATTRIBUTE);
	Light::addAttribute(LightHasAttenuation::ATTRIBUTE);
	Light::addAttribute(LightHasGeometry::ATTRIBUTE);

	m_id = ++GlobalCounter;
}

bool DiskAreaLight::computeSample(const Point& P, const std::list<Object*>& objects, unsigned int& haltonSeq,
									Color& I, Vector& L) const
{
	// Point at the light surface
	Point LP(0,0,0);

	// Generates a random sample.
	do
	{
		haltonSeq++;
		LP.x = Random::Instance().haltonSeq(haltonSeq,2)*m_radius;
		LP.y = Random::Instance().haltonSeq(haltonSeq,3)*m_radius;
	}
	while ((LP.x*LP.x + LP.y*LP.y)>m_radiusSq);

	// Ray from point of intersection to light surface
	L = LP - P;
	const double lightRayLength = L.length();
	L = L.normalize();
	Ray lightRay = T(Ray(P, L));	// <- Local to World
	lightRay.tmax = lightRayLength;

	// Test if the light ray is obstructed
	bool obstructed = false;
	std::list<Object*>::const_iterator obj;
	for (obj = objects.begin(); obj != objects.end(); obj++)
	{
		if (!(*obj)->castShadow()) continue;

		double t;
		const Ray lightRayT = (*obj)->T.applyInv(lightRay);	// World to object
		bool intersects = (*obj)->getGeometry()->intersects(lightRayT,t);
		if (intersects)
		{
			obstructed = true;
			break;
		}
	}

	I = obstructed ? Color(0,0,0) : getColor()*getIntensity()*getAttFactor(lightRayLength);
	return obstructed;
}

void DiskAreaLight::computeIntensity(const Vector&, const Point& P,
							const std::list<Object*>& objects,
							std::list<Color>& Is, std::list<Vector>& Ls) const
{
	if (getDirection().dot(P-getPosition()) <= 0.0) return;	// The normal is not facing the P.

	bool penumbra = false;
	unsigned int haltonSeq = Random::Instance().generate(1024);

	Point tP = T.applyInv(P); // World to local
	Color I;
	Vector L;
	bool shadow = computeSample(tP,objects,haltonSeq,I,L);
	Is.push_back(I);
	Ls.push_back(T(L));	// Local to world

	for (int i=1; i<m_minSamples; i++)
	{
		Color I;
		Vector L;
		bool blocked = computeSample(tP,objects,haltonSeq,I,L);
		Is.push_back(I);
		Ls.push_back(T(L));	// Local to world

		if (blocked != shadow) penumbra = true;
	}

	if (penumbra)
	{
		for (int n=0; n<m_maxSamples; n++)
		{
			Color I;
			Vector L;
			computeSample(tP,objects,haltonSeq,I,L);
			Is.push_back(I);
			Ls.push_back(T(L));	// Local to world
		}
	}
}

Geometry* DiskAreaLight::getGeometry() const
{
	return new Disk(m_radius);
}

Material* DiskAreaLight::getMaterial() const
{
	Material *m = new Material(std::string("_dal_")+boost::lexical_cast<std::string>(m_id), getColor());
	m->setEmission(getIntensity());
	m->setSpecularIntensity(0.0);
	m->setSpecularGlossiness(0.0);
	return m;
}


#include "Tillotson.hpp"
#include <assert.h>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/minima.hpp>
#include <iostream>
#include "../../misc/universal_error.hpp"
#include "../../misc/utils.hpp"

Tillotson::Tillotson(double a, double b, double A, double B, double rho0, double E0, double EIV, double ECV, double
	alpha, double beta, bool negative_pressure,size_t e_index) :
	a_(a), b_(b), A_(A), B_(B), rho0_(rho0), E0_(E0), EIV_(EIV), ECV_(ECV), alpha_(alpha), beta_(beta),
	negative_pressure_(negative_pressure), temp_d_(0), temp_p_(0), e_index_(e_index){}

double Tillotson::dp2EI(double d, double p) const
{
	double eta = d / rho0_;
	double c = E0_ * eta*eta;
	double AB = (A_ - 2 * B_)*eta + (B_ - A_) + B_ * eta*eta;
	double A_B = A_ - B_;
	double sqr = std::sqrt(d*c * 2 * (a_ - b_)*(p - (A_B + eta * B_)*(eta - 1)) + d * d*c*c*(a_ + b_)*(a_ + b_) + (p - (eta - 1)*(A_B + eta * B_))
		*(p - (eta - 1)*(A_B + eta * B_)));
	double first_part = p - AB - a_ * c*d - b_ * c*d;
	double E = (first_part + sqr) / (2 * a_*d);
	if (E < 0)
	{
		E = p * 0.001 / d;
	}
	assert(E > 0);
	return E;
}

double Tillotson::dp2EIV(double d, double p) const
{
	double eta = d / rho0_;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
	double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
	double eta3 = beta_ * (rho0_ / d - 1);
	double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
	double b = b_ * exp_alpha;
	double AB = exp_alpha * exp_beta;
	double E = (p - AB - a_ * c*d - b * c*d + sqrt(4 * a_*c*d*(p - AB) + std::pow(AB + (a_ + b)*c*d - p, 2))) /
		(2 * a_*d);
	if (E < 0)
	{
		//std::cout << "d " << d << " p " << p << std::endl;
		E = p * 0.001 / d;
	}
	assert(E > 0);
	return E;
}

double Tillotson::de2pI(double d, double e)const
{
	double eta = d / rho0_;
	double c = E0_ * eta*eta;
	double AB = (A_ - 2 * B_)*eta + (B_ - A_) + B_ * eta*eta;
	double res = 0;
	//if (negative_pressure_)
		//res = (a_ + b_ / (e / c + 1))*d*e + AB;
	//else
	res = std::max((a_ + b_ / (e / c + 1))*d*e + AB, a_*d*e*1e-7);
	return res;
}

double Tillotson::de2pII(double d, double e)const
{
	double P2 = de2pI(d, EIV_);
	double P3 = de2pIV(d, ECV_);
	return std::max(((e - EIV_)*P3 + (ECV_ - e)*P2) / (ECV_ - EIV_), a_*d*e*1e-7);
}

double Tillotson::de2pIV(double d, double e)const
{
	double eta = d / rho0_;
	if (alpha_ > 100 * eta*eta)
		return a_ * d*e;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
	double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
	double eta3 = beta_ * (rho0_ / d - 1);
	double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
	//if (negative_pressure_)
		return a_ * d*e + exp_alpha * (b_*d*e / (e / c + 1) + exp_beta);
	//else
	//	return std::max(a_*d*e + exp_alpha * (b_*d*e / (e / c + 1) + exp_beta), a_ *d*e*1e-7);
}

double Tillotson::dep2cI(double d, double e, double p) const
{
	double eta = d / rho0_;
	double w0 = e / (E0_*eta*eta) + 1;
	double gamma = a_ + b_ / w0;
	double res = (gamma + 1)*p / d + (A_ + B_ * (eta*eta - 1)) / d + b_ * (w0 - 1)*(2 * e - p / d) / (w0*w0);
	res = std::max(res, 1e-10*E0_);
	return res;
}

double Tillotson::dep2cIV(double d, double e, double p) const
{
	double eta = d / rho0_;
	double w0 = e / (E0_*eta*eta) + 1;
	double z = 1.0 / eta - 1.0;
	double afactor = (alpha_*z*z > 100) ? 0 : std::exp(-alpha_ * z*z);
	double res0 = p * (a_ + b_ * afactor / w0 + 1) / d;
	double bfactor = (beta_*z > 100) ? 0 : std::exp(-beta_ * z);
	double res1 = A_ * bfactor*afactor*(1 + (eta - 1)*(beta_ + 2 * alpha_*z - eta) / (eta*eta)) / rho0_;
	res1 += b_ * d*e*afactor*(2 * alpha_*z*w0 / rho0_ + (p / d - 2 * e) / (E0_*d)) / (w0*w0*eta*eta);
	double res = std::max(res0 + res1, 1e-10*E0_);
	return res;
}

struct dp2eII
{
	dp2eII(Tillotson const& eos) : eos_(eos)
	{}

	double operator()(double e)
	{
		double res = std::abs(1.0 - eos_.de2pII(eos_.temp_d_, e) / eos_.temp_p_);
		return res;
	}
private:
	Tillotson const& eos_;
};

double Tillotson::dp2e(double d, double p, tvector const & tracers, vector<string> const & tracernames) const
{
	if(tracernames.size() > 0)
		return tracers[e_index_];

	double eta = d / rho0_;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	if (d >= rho0_)
	{
		return dp2EI(d, p);
	}
	else
	{
		double e4 = dp2EIV(d, p);
		double p4 = de2pI(d, e4);
		double e1 = dp2EI(d, p);
		double p1 = de2pI(d, e1);
		//double p1 = de2pI(d, e1);
		if (e1 < EIV_ && p1 > 0)
			return dp2EI(d, p);
		if (p4<0 || e4>=ECV_)
			return e4;

		temp_d_ = d;
		temp_p_ = p;
		boost::uintmax_t it = 50;
		std::pair<double, double> res,res2;
		try
		{
			//res = boost::math::tools::toms748_solve(dp2eII(*this), EIV_, ECV_, boost::math::tools::eps_tolerance<double>(30), it);
			res=boost::math::tools::brent_find_minima(dp2eII(*this), EIV_, ECV_, 30, it);
			//res2 = boost::math::tools::brent_find_minima(dp2eII(*this), EIV_, 0.5*(EIV_+ECV_), 30, it);
		}
		catch (boost::exception const& eo)
		{
			double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
			double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
			double eta3 = beta_ * (rho0_ / d - 1);
			double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
			double PIV = de2pI(d, EIV_);
			double PCV = 0;
			if (negative_pressure_)
				PCV = a_ * d*ECV_ + exp_alpha * (b_*d*ECV_ / (ECV_ / c + 1) + exp_beta);
			else
				PCV = std::max(a_*d*ECV_ + exp_alpha * (b_*d*ECV_ / (ECV_ / c + 1) + exp_beta), (a_ + b_)*d*ECV_*1e-7);
			std::cout << " EIV_ " << EIV_ << " ECV_ " << ECV_ << " density " << d << " pressure " << p << " PIV " << PIV << " PCV " << PCV << std::endl;
		}
		double result = res.first;
		//double result = res.second > res2.second ? res2.first : res.first;
		if (result > EIV_ && result < ECV_)
		{
			double newp = de2p(d, result);
			if (newp > p*0.001)
			{
				if (std::abs(p - newp) > 0.001*std::abs(p))
				{
					UniversalError eo("No dp2e convergence");
					eo.AddEntry("Density", d);
					eo.AddEntry("Pressure", p);
					eo.AddEntry("New Pressure", newp);
					eo.AddEntry("EIV", EIV_);
					eo.AddEntry("ECV", ECV_);
					eo.AddEntry("First energy", res.first);
					eo.AddEntry("Second energy", res.second);
					eo.AddEntry("First pressure", de2p(d, res.first));
					eo.AddEntry("Second pressure", de2p(d, res.second));
					throw eo;
				}
			}
			assert(result > 0);
			return result;
		}
		return e4;
	}
}

double Tillotson::de2p(double d, double e, tvector const& /*tracers*/, vector<string> const& /*tracernames*/) const
{
	if (d >= rho0_)
	{
		return de2pI(d, e);
	}
	else
	{
		if (e <= EIV_)
			return de2pI(d, e);
		if (e >= ECV_)
			return de2pIV(d, e);
		return de2pII(d, e);
	}
}

double Tillotson::de2c(double d, double e, tvector const & tracers, vector<string> const & tracernames) const
{
	double p = de2p(d, e, tracers, tracernames);
	return dp2c(d, p, tracers, tracernames);
}

double Tillotson::dp2c(double d, double p, tvector const & tracers, vector<string> const & tracernames) const
{
	double e = dp2e(d, p, tracers, tracernames);
	if (d >= rho0_)
	{
		double res = dep2cI(d, e, p);
		assert(res > 0);
		return std::sqrt(res);
	}
	else
	{
		if (e <= EIV_)
		{
			double res = dep2cI(d, e, p);
			assert(res > 0);
			return std::sqrt(res);
		}
		//double e4 = dp2EIV(d, p);
		if (e >= ECV_)
		{
			double res = dep2cIV(d, e, p);
			assert(res > 0);
			return std::sqrt(res);
		}
		else
		{
			double c1 = dep2cI(d, e, p);
			double c4 = dep2cIV(d, e, p);
			double res = std::sqrt((c1*(ECV_ - e) + c4 * (e - EIV_)) / (ECV_ - EIV_));
			assert(res > 0);
			return res;
		}
	}
}

double Tillotson::dp2s(double /*d*/, double /*p*/, tvector const & /*tracers*/, vector<string> const & /*tracernames*/) const
{
	std::cout << "dp2s not implemented" << std::endl;
	assert(false);
	return 0;
}

double Tillotson::sd2p(double /*s*/, double /*d*/, tvector const & /*tracers*/, vector<string> const & /*tracernames*/) const
{
	std::cout << "sd2p not implemented" << std::endl;
	assert(false);
	return 0;
}

Tillotson::~Tillotson(void)
{}


#include "cnn/nodes.h"
#include "cnn/cnn.h"
#include "cnn/training.h"
#include "cnn/timing.h"
#include "cnn/rnn.h"
#include "cnn/lstm.h"
#include "cnn/dict.h"
#include "cnn/expr.h"
#include "cnn/rnnem.h"

#include <iostream>
#include <fstream>
#include <sstream>

#ifdef WIN32
#include <process.h>
#endif
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

using namespace std;
using namespace cnn;
using namespace cnn::expr;

/** this implements the following model
@inproceedings{Seq2Seq,
title = {Recurrent neural networks with external memory},
author  = {K. Yao, B. Peng and C. Dyer},
year  = 2016,
}
*/

//parameters
unsigned LAYERS = 3;
unsigned INPUT_DIM = 500;
unsigned HIDDEN_DIM = 500;
unsigned VOCAB_SIZE_SRC = 0;
unsigned VOCAB_SIZE_TGT = 0;

cnn::Dict sd, td;
int kSRC_SOS;
int kSRC_EOS;
int kTGT_SOS;
int kTGT_EOS;

typedef vector<int> Sentence;
typedef pair<Sentence, Sentence> SentencePair;
typedef vector<SentencePair> Corpus;

Corpus read_corpus(const string &filename)
{
  ifstream in(filename);
  assert(in);
  Corpus corpus;
  string line;
  int lc = 0, stoks = 0, ttoks = 0;
  while(getline(in, line)) {
    ++lc;
    Sentence source, target;
    ReadSentencePair(line, &source, &sd, &target, &td);
    corpus.push_back(SentencePair(source, target));
    stoks += source.size();
    ttoks += target.size();

    if ((source.front() != kSRC_SOS && source.back() != kSRC_EOS) ||
	(target.front() != kTGT_SOS && target.back() != kTGT_EOS)) {
      cerr << "Sentence in " << filename << ":" << lc << " didn't start or end with <s>, </s>\n";
      abort();
    }
  }
  cerr << lc << " lines, " << stoks << " & " << ttoks << " tokens (s & t), " << sd.size() << " & " << td.size() << " types\n";
  return corpus;
}

template <class Builder>
struct EncoderDecoder {
  LookupParameters* p_c;
  LookupParameters* p_ec;  // map input to embedding (used in fwd and rev models)
  Parameters* p_R;
  Parameters* p_bias;
  Builder dec_builder;
  Builder enc_builder;

  explicit EncoderDecoder(Model& model,
			  long layers,
			  long vocab_size_src,
			  long vocab_size_tgt,
			  long input_dim,
			  long hidden_dim) :
      dec_builder(layers, input_dim, hidden_dim, &model),
      enc_builder(layers, input_dim, hidden_dim, &model) {

    p_c = model.add_lookup_parameters(vocab_size_src, {input_dim});
    p_ec = model.add_lookup_parameters(vocab_size_src, {input_dim});
    p_R = model.add_parameters({vocab_size_tgt, hidden_dim});
    p_bias = model.add_parameters({vocab_size_tgt});
  }

  // build graph and return Expression for total loss
  Expression BuildGraph(const Sentence& insent, const Sentence& osent, ComputationGraph& cg) {
    // forward encoder
    enc_builder.new_graph(cg);

    enc_builder.start_new_sequence();
    for (int t = insent.size() - 1; t >= 0; --t) {
      Expression i_x_t = lookup(cg, p_ec, insent[t]);
      enc_builder.add_input(i_x_t);
    }

    cg.incremental_forward();

    // encoder -> decoder transformation
    vector<Expression> i_combined;
    for (auto s_l : enc_builder.final_s()) i_combined.push_back(s_l);

    //    cerr << "dim= " << i_combined.size() << endl;

    dec_builder.new_graph(cg);
    dec_builder.start_new_sequence(i_combined);

    // decoder
    Expression i_R = parameter(cg,p_R);
    Expression i_bias = parameter(cg,p_bias);
    vector<Expression> errs;

    const unsigned oslen = osent.size() - 1;
    for (unsigned t = 0; t < oslen; ++t) {
    	Expression i_x_t = lookup(cg, p_c, osent[t]);
    	Expression i_y_t = dec_builder.add_input(i_x_t);
    	Expression i_r_t = i_bias + i_R * i_y_t;
    	Expression i_ydist = log_softmax(i_r_t);
    	errs.push_back(pick(i_ydist,osent[t+1]));
    }
    Expression i_nerr = sum(errs);
    return -i_nerr;
  }
};

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);
  if (argc != 3 && argc != 4) {
    cerr << "Usage: " << argv[0] << " corpus.txt dev.txt [model.params]\n";
    return 1;
  }
  kSRC_SOS = sd.Convert("<s>");
  kSRC_EOS = sd.Convert("</s>");
  kTGT_SOS = td.Convert("<s>");
  kTGT_EOS = td.Convert("</s>");

  vector<SentencePair> training, dev;
  string line;
  cerr << "Reading training data from " << argv[1] << "...\n";

  ifstream in(argv[1]);
  training = read_corpus(argv[1]);

  VOCAB_SIZE_SRC = sd.size();
  VOCAB_SIZE_TGT = td.size();

  cerr << "vocab size src " << VOCAB_SIZE_SRC << "vocab size targt " << VOCAB_SIZE_TGT << endl;

  sd.Freeze(); // no new word types allowed
  td.Freeze();

  cerr << "Reading dev data from " << argv[2] << "...\n";
  dev = read_corpus(argv[2]);

  ostringstream os;
  os << "bilm"
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "Parameters will be written to: " << fname << endl;
  cnn::real best = 9e+99;

  Model model;
  bool use_momentum = false;
  Trainer* sgd = nullptr;
  if (use_momentum)
    sgd = new MomentumSGDTrainer(&model);
  else
    sgd = new SimpleSGDTrainer(&model);

  //RNNBuilder rnn(LAYERS, INPUT_DIM, HIDDEN_DIM, &model);
  //EncoderDecoder<SimpleRNNBuilder> lm(model);
//  EncoderDecoder<LSTMBuilder> lm(model, LAYERS, VOCAB_SIZE_SRC, VOCAB_SIZE_TGT, INPUT_DIM, HIDDEN_DIM);
  EncoderDecoder<RNNEMBuilder> lm(model, LAYERS, VOCAB_SIZE_SRC, VOCAB_SIZE_TGT, INPUT_DIM, HIDDEN_DIM);
  if (argc == 4) {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }

  unsigned report_every_i = 50;
  unsigned dev_every_i_reports = 2;
  unsigned si = training.size();
  vector<unsigned> order(training.size());
  for (unsigned i = 0; i < order.size(); ++i) order[i] = i;
  bool first = true;
  int report = 0;
  unsigned lines = 0;
  while(1) {
    Timer iteration("completed in");
    cnn::real loss = 0;
    unsigned chars = 0;
    for (unsigned i = 0; i < report_every_i; ++i) {
      if (si == training.size()) {
	si = 0;
	if (first) { first = false; } else { sgd->update_epoch(); }
	cerr << "**SHUFFLE\n";
	random_shuffle(order.begin(), order.end());
      }

      // build graph for this instance
      ComputationGraph cg;
      auto& sent = training[order[si]];
      Sentence src_sent = sent.first;
      Sentence tgt_sent = sent.second;

      chars += tgt_sent.size() - 1;
      ++si;
      lm.BuildGraph(src_sent, tgt_sent, cg);
      //cg.PrintGraphviz();
      loss += as_scalar(cg.forward());
      cg.backward();
      sgd->update();
      ++lines;
    }
    sgd->status();
    cerr << " E = " << (loss / chars) << " ppl=" << exp(loss / chars) << ' ';

#if 0
    lm.RandomSample();
#endif

    // show score on dev data?
    report++;
    if (report % dev_every_i_reports == 0) {
      cnn::real dloss = 0;
      int dchars = 0;
      for (auto& sent : dev) {
	ComputationGraph cg;
	Sentence src_sent = sent.first;
	Sentence tgt_sent = sent.second;

	lm.BuildGraph(src_sent, tgt_sent, cg);
	dloss += as_scalar(cg.forward());
	dchars += tgt_sent.size() - 1;
      }
      if (dloss < best) {
	best = dloss;
	ofstream out(fname);
	boost::archive::text_oarchive oa(out);
	oa << model;
      }
      cerr << "\n***DEV [epoch=" << (lines / (cnn::real)training.size()) << "] E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
    }
  }
  delete sgd;
}


#pragma once

#include <vector>
#include <cppsim/circuit_builder.hpp>
#include <cppsim/type.hpp>
#include <cppsim/simulator.hpp>
#include <cppsim/utility.hpp>
#include "problem.hpp"
#include "optimizer.hpp"
#include "differential.hpp"
#include <functional>
#include <Eigen/Dense>

class QuantumCircuitEnergyMinimizationSolver {
private:
    ParametricQuantumCircuit* _circuit;
    const std::function<ParametricQuantumCircuit* (UINT, UINT)>* _circuit_construction;
    UINT _param_count;
    std::vector<double> _parameter;
    double loss;
public:
    bool verbose;
    QuantumCircuitEnergyMinimizationSolver(const std::function<ParametricQuantumCircuit*(UINT,UINT)>* circuit_generator, UINT param_count = 0) {
        _circuit_construction = circuit_generator;
        _param_count = param_count;
        _circuit = NULL;
        verbose = false;
    };
    virtual ~QuantumCircuitEnergyMinimizationSolver() {
    }

    virtual void solve(
        EnergyMinimizationProblem* instance,
        UINT max_iteration = 100,
        std::string optimizer_name = "GD",
        std::string differentiation_method = "HalfPi"
    ) {
        if (_circuit != NULL) {
            delete _circuit;
            _circuit = NULL;
        }
        _circuit = (*_circuit_construction)(instance->get_qubit_count(), _param_count);
        auto _simulator = new ParametricQuantumCircuitSimulator(_circuit);
        _param_count = _simulator->get_parametric_gate_count();

        _parameter = std::vector<double>(_param_count, 0.);
        std::vector<double> gradient(_param_count);
        Random random;
        random.set_seed(0);
        for (auto& val : _parameter) val = random.uniform()*acos(0.0)*4;

        GradientBasedOptimizer* optimizer;
        QuantumCircuitGradientDifferentiation* differentiation;

        if (optimizer_name == "Adam") {
            optimizer = new AdamOptimizer(_param_count);
        }
        else if (optimizer_name == "GD") {
            optimizer = new GradientDecentOptimizer(_param_count);
        }
        else return;
        if (differentiation_method == "HalfPi") {
            differentiation = new GradientByHalfPi();
        }
        std::vector<double> old_param;
        for (UINT iteration = 0; iteration < max_iteration; ++iteration) {
            loss = differentiation->compute_gradient(_simulator, instance, _parameter, &gradient);

            if(verbose){
                std::cout << " *** epoch " << iteration << " *** " << std::endl;
                std::cout << " * loss = " << loss << std::endl;
                old_param = _parameter;
            }

            optimizer->apply_gradient(&_parameter, gradient);

            if (verbose) {
                for (UINT i = 0; i < _param_count; ++i) {
                    std::cout << " ** id " << i << " para = " << old_param[i] << " -> " << _parameter[i] << " , grad = " << gradient[i] << std::endl;
                }
            }
        }
        delete optimizer;
        delete differentiation;
        delete _simulator;
    }
    virtual double get_loss() { return loss; }
    virtual std::vector<double> get_parameter() { return _parameter; }
    ParametricQuantumCircuitSimulator* get_quantum_circuit_simulator() {
        return new ParametricQuantumCircuitSimulator(_circuit);
    }
};

class DiagonalizationEnergyMinimizationSolver {
private:
    ParametricQuantumCircuit* _circuit;
    const std::function<ParametricQuantumCircuit* (UINT, UINT)>* _circuit_construction;
    UINT _param_count;
    std::vector<double> _parameter;
    double loss;
public:
    bool verbose;
    DiagonalizationEnergyMinimizationSolver() {
        verbose = false;
    };
    virtual ~DiagonalizationEnergyMinimizationSolver() {
    }

    virtual void solve(
        EnergyMinimizationProblem* instance
    ) {
        const UINT qubit_count = instance->get_qubit_count();
        const UINT term_count = instance->get_term_count();
        const ITYPE matrix_dim = 1ULL << qubit_count;

        ComplexMatrix observable_matrix = ComplexMatrix::Zero(matrix_dim, matrix_dim);
        for (UINT term_index = 0; term_index < term_count; ++term_index) {
            auto Pauli_operator = instance->get_Pauli_operator(term_index);
            double coef = Pauli_operator->get_coef();
            auto target_index_list = Pauli_operator->get_index_list();
            auto pauli_id_list = Pauli_operator->get_pauli_id_list();

            std::vector<UINT> whole_pauli_id_list(qubit_count, 0);
            for (UINT i = 0; i < target_index_list.size(); ++i) {
                whole_pauli_id_list[target_index_list[i]] = pauli_id_list[i];
            }

            ComplexMatrix pauli_matrix;
            get_Pauli_matrix(pauli_matrix,whole_pauli_id_list);
            observable_matrix += coef*pauli_matrix;
        }

        observable_matrix.eigenvalues();
        Eigen::SelfAdjointEigenSolver<ComplexMatrix> eigen_solver(observable_matrix);
        loss = eigen_solver.eigenvalues()[0];

        if (verbose)    std::cout << "Eigenvalues : " << std::endl << eigen_solver.eigenvalues() << std::endl;
    }
    virtual double get_loss() { return loss; }
};