//----------------------------//
// This file is part of RaiSim//
// Copyright 2020, RaiSim Tech//
//----------------------------//

#pragma once

#include <algorithm>
#include <boost/algorithm/clamp.hpp>
#include <functional>
#include <set>
#include <stdlib.h>

#include "RaisimGymEnv.hpp"
#include "include/delta_dynamics.h"
#include "include/impedance_control_module.h"
#include "include/sensors.h"

namespace raisim {

class ENVIRONMENT : public RaisimGymEnv {

public:
  explicit ENVIRONMENT(const std::string &resourceDir, const Yaml::Node &cfg,
                       bool visualizable)
      : RaisimGymEnv(resourceDir, cfg), visualizable_(visualizable),
        unifDistPlusMinusOne_(-1.0, 1.0), controller_(cfg["controller"]) {
    render_ = cfg["render"].template As<bool>();

    /// create world
    world_ = std::make_unique<raisim::World>();
    /// add objects
    ouzel_ =
        world_->addArticulatedSystem(resourceDir_ + "/ouzel_delta/temp.urdf");

    ouzel_->setName("ouzel_delta");
    ouzel_->setControlMode(raisim::ControlMode::FORCE_AND_TORQUE);
    world_->addGround(-10.0); // we don't need a ground for the drone
    baseLink_ = ouzel_->getBodyIdx("ouzel/base_link");

    /// get robot data
    gcDim_ = ouzel_->getGeneralizedCoordinateDim();
    gvDim_ = ouzel_->getDOF();
    nJoints_ = gvDim_ - 6;

    /// initialize containers
    gc_.setZero(gcDim_);
    gc_init_.setZero(gcDim_);
    gv_.setZero(gvDim_);
    gv_init_.setZero(gvDim_);

    control_dt_ = cfg["control_dt"].template As<float>();
    simulation_dt_ = cfg["simulation_dt"].template As<float>();

    /// initialize delta
    delta_sym_ =
        new delta_dynamics::DeltaController(cfg_["deltaArm"], control_dt_);
    if (render_) {
      delta_eef_ = world_->addCylinder(0.05, 0.008, 0.01, "default",
                                       raisim::COLLISION(15));
      delta_eef_->setPosition(2, 2, 2);
      delta_eef_->setBodyType(raisim::BodyType::STATIC);
    }
    ee_vel_prev_ = Eigen::Vector3d::Zero();
    B_om_WB_prev_ = Eigen::Vector3d::Zero();
    pos_offset_BD_ =
        Eigen::Vector3d(cfg["deltaArm"]["p_BO"]["x"].template As<float>(),
                        cfg["deltaArm"]["p_BO"]["y"].template As<float>(),
                        cfg["deltaArm"]["p_BO"]["z"].template As<float>());
    ang_offset_BD_ =
        Eigen::Quaterniond(cfg["deltaArm"]["q_BO"]["w"].template As<float>(),
                           cfg["deltaArm"]["q_BO"]["x"].template As<float>(),
                           cfg["deltaArm"]["q_BO"]["y"].template As<float>(),
                           cfg["deltaArm"]["q_BO"]["z"].template As<float>())
            .normalized();

    /// Initialisation Parameters
    initialDistanceOffset_ =
        cfg["initialisation"]["distanceOffset"].template As<float>();
    initialAngularOffset_ =
        cfg["initialisation"]["angleOffsetDeg"].template As<float>() / 180.0 *
        M_PI;
    initialLinearVel_ = cfg["initialisation"]["linearVel"].template As<float>();
    initialAngularVel_ =
        cfg["initialisation"]["angularVelDeg"].template As<float>() / 180.0 *
        M_PI;

    resetInitialConditions();

    /// MUST BE DONE FOR ALL ENVIRONMENTS
    obDim_ = 36;
    actionDim_ = 12;
    ouzel_position_W_.setZero();
    ouzel_orientation_W_B_.setIdentity();
    ouzel_linear_vel_B_.setZero();
    ouzel_angular_vel_B_.setZero();

    /// Reward coefficients
    rewards_.initializeFromConfigurationFile(cfg["reward"]);
    terminalOOBRewardCoeff_ =
        cfg["termination"]["oob"]["reward"].template As<float>();
    terminalOOBWaypointDist_ =
        cfg["termination"]["oob"]["waypointDist"].template As<float>();
    terminalOOBAngleError_ =
        cfg["termination"]["oob"]["angleErrorDeg"].template As<float>() /
        180.0 * M_PI;
    terminalSuccessRewardCoeff_ =
        cfg["termination"]["success"]["reward"].template As<float>();
    terminalSuccessWaypointDist_ =
        cfg["termination"]["success"]["waypointDist"].template As<float>();
    terminalSuccessAngleError_ =
        cfg["termination"]["success"]["angleErrorDeg"].template As<float>() /
        180.0 * M_PI;
    terminalSuccessLinearVel_ =
        cfg["termination"]["success"]["linearVel"].template As<float>();
    terminalSuccessAngularVel_ =
        cfg["termination"]["success"]["angularVelDeg"].template As<float>() /
        180.0 * M_PI;
    min_time_in_success_state_ =
        cfg["termination"]["success"]["minTime"].template As<float>();

    // Add sensors
    auto *odometry_noise =
        new raisim_sensors::odometryNoise(control_dt_, cfg["odometryNoise"]);
    odometry_ = raisim_sensors::odometry(ouzel_, control_dt_, "ouzel",
                                         "ouzel/base_link", odometry_noise);

    delta_ouzel_ref_position_offset_previous_ = Eigen::Vector3d::Zero();
    ref_delta_joint_pos_previous_ = Eigen::Vector3d::Zero();
    time_in_success_state_ = 0.0;

    /// visualize if it is the first environment
    if (visualizable_) {
      server_ = std::make_unique<raisim::RaisimServer>(world_.get());
      server_->launchServer();
      server_->focusOn(ouzel_);
    }
  }

  void init() final {}

  void reset() final {
    resetInitialConditions();
    ouzel_->setState(gc_init_, gv_init_);
    time_in_success_state_ = 0.0;
    updateObservation();
  }

  float step(const Eigen::Ref<EigenVec> &action) final {
    controller_.setOdom(ouzel_position_W_, ouzel_orientation_W_B_,
                        ouzel_linear_vel_B_, ouzel_angular_vel_B_);

    auto actionD = action.cast<double>();
    Eigen::Vector3d delta_ouzel_ref_position_offset = actionD.segment(0, 3);
    Eigen::Quaterniond ref_ouzel_orientation_corr =
        QuaternionFromTwoVectors(actionD.segment(3, 3), actionD.segment(6, 3));
    controller_.adaptRefFromAction(delta_ouzel_ref_position_offset,
                                   ref_ouzel_orientation_corr);

    mav_msgs::EigenTorqueThrust wrench_command;
    controller_.calculateWrenchCommand(&wrench_command, control_dt_);

    // delta arm
    Eigen::Vector3d ref_delta_joint_pos(actionD.tail(3));
    Eigen::Vector3d ref_delta_joint_pos_clamped;
    ref_delta_joint_pos_clamped(0) = boost::algorithm::clamp(
        ref_delta_joint_pos(0), delta_min_joint_angle_, delta_max_joint_angle_);
    ref_delta_joint_pos_clamped(1) = boost::algorithm::clamp(
        ref_delta_joint_pos(1), delta_min_joint_angle_, delta_max_joint_angle_);
    ref_delta_joint_pos_clamped(2) = boost::algorithm::clamp(
        ref_delta_joint_pos(2), delta_min_joint_angle_, delta_max_joint_angle_);
    Eigen::VectorXd odometry_measurement_gt;
    Eigen::Vector3d force_B, torque_B;
    Eigen::Vector3d ee_pos, ee_vel, ee_acc, eef_pos_W;
    Eigen::Vector3d B_dv_WB, B_om_WB, B_dom_WB;
    for (int i = 0; i < int(control_dt_ / simulation_dt_ + 1e-10); i++) {
      odometry_.update();
      odometry_measurement_gt = odometry_.getMeasGT();
      ouzel_position_W_gt_ = odometry_measurement_gt.segment(0, 3);
      ouzel_orientation_W_B_gt_ = Eigen::Quaterniond(odometry_measurement_gt(3),
                                                     odometry_measurement_gt(4),
                                                     odometry_measurement_gt(5),
                                                     odometry_measurement_gt(6))
                                      .normalized();
      ouzel_linear_vel_B_gt_ = odometry_measurement_gt.segment(7, 3);
      ouzel_angular_vel_B_gt_ = odometry_measurement_gt.segment(10, 3);

      /// apply wrench on ouzel
      ouzel_->setExternalForce(baseLink_, ouzel_->BODY_FRAME,
                               wrench_command.thrust, ouzel_->WORLD_FRAME,
                               ouzel_->getCOM()); // set force in body frame
      ouzel_->setExternalTorqueInBodyFrame(baseLink_, wrench_command.torque);

      /// delta arm simulation
      delta_sym_->sendActuatorsCommand(ref_delta_joint_pos_clamped);

      delta_sym_->fwkinPosition(&ee_pos);
      delta_sym_->fwkinVel(&ee_vel, ee_pos);
      ee_acc = (ee_vel - ee_vel_prev_) / simulation_dt_;
      ee_vel_prev_ = ee_vel;

      B_dv_WB =
          ouzel_orientation_W_B_gt_.toRotationMatrix() * ouzel_linear_vel_B_gt_;
      B_om_WB = ouzel_orientation_W_B_gt_.toRotationMatrix() *
                ouzel_angular_vel_B_gt_;
      B_dom_WB = (B_om_WB - B_om_WB_prev_) / simulation_dt_;
      B_om_WB_prev_ = B_om_WB;

      /// Compute feed forward base wrench due to dynamics.
      delta_sym_->getBaseWrench(&force_B, &torque_B, ouzel_orientation_W_B_gt_,
                                B_om_WB, B_dom_WB, B_dv_WB, ee_pos, ee_vel,
                                ee_acc);
      ouzel_->setExternalForce(baseLink_, ouzel_->BODY_FRAME, force_B,
                               ouzel_->WORLD_FRAME, ouzel_->getCOM());
      ouzel_->setExternalTorqueInBodyFrame(baseLink_, torque_B);

      if (render_) {
        eef_pos_W = ouzel_position_W_gt_ + pos_offset_BD_ +
                    ouzel_orientation_W_B_gt_.matrix() *
                        ang_offset_BD_.matrix() * ee_pos;
        delta_eef_->setPosition(eef_pos_W(0), eef_pos_W(1), eef_pos_W(2));
        delta_eef_->setOrientation(ouzel_orientation_W_B_gt_);
      }

      if (server_)
        server_->lockVisualizationServerMutex();
      world_->integrate();
      if (server_)
        server_->unlockVisualizationServerMutex();
    }

    updateObservation();

    Eigen::Vector3d delta_ouzel_ref_position_offset_diff;
    if (delta_ouzel_ref_position_offset_previous_ == Eigen::Vector3d::Zero()) {
      delta_ouzel_ref_position_offset_diff = Eigen::Vector3d::Zero();
    } else {
      delta_ouzel_ref_position_offset_diff =
          delta_ouzel_ref_position_offset -
          delta_ouzel_ref_position_offset_previous_;
    }
    delta_ouzel_ref_position_offset_previous_ = delta_ouzel_ref_position_offset;

    Eigen::Vector3d ref_delta_joint_pos_diff;
    if (ref_delta_joint_pos_previous_ == Eigen::Vector3d::Zero()) {
      ref_delta_joint_pos_diff = Eigen::Vector3d::Zero();
    } else {
      ref_delta_joint_pos_diff =
          ref_delta_joint_pos - ref_delta_joint_pos_previous_;
    }
    ref_delta_joint_pos_previous_ = ref_delta_joint_pos_diff;

    // get rewards
    double waypoint_dist_delta, error_angle;
    computeErrorMetrics(waypoint_dist_delta, error_angle);
    Eigen::AngleAxisd ref_ouzel_orientation_corr_angle_axis(
        ref_ouzel_orientation_corr);
    rewards_.record("waypointDist", float(waypoint_dist_delta));
    rewards_.record("orientError", float(error_angle));
    rewards_.record("deltaOuzelRefPositionOffset",
                    float(delta_ouzel_ref_position_offset.squaredNorm()));
    rewards_.record("deltaOuzelRefPositionOffsetDiff",
                    float(delta_ouzel_ref_position_offset_diff.squaredNorm()));
    rewards_.record("orientRefCorr",
                    float(ref_ouzel_orientation_corr_angle_axis.angle()));
    rewards_.record("deltaJointAngles",
                    float(std::abs(ref_delta_joint_pos(0)) +
                          std::abs(ref_delta_joint_pos(1)) +
                          std::abs(ref_delta_joint_pos(2))));
    rewards_.record("deltaJointAnglesDiff",
                    float(ref_delta_joint_pos_diff.squaredNorm()));
    rewards_.record(
        "deltaJointAnglesClamp",
        float((ref_delta_joint_pos - ref_delta_joint_pos_clamped).norm()));
    return rewards_.sum();
  }

  void updateObservation() {
    odometry_.update();
    Eigen::VectorXd odometry_measurement = odometry_.getMeas();
    ouzel_position_W_ = odometry_measurement.segment(0, 3);
    ouzel_orientation_W_B_ =
        Eigen::Quaterniond(odometry_measurement(3), odometry_measurement(4),
                           odometry_measurement(5), odometry_measurement(6))
            .normalized();
    ouzel_linear_vel_B_ = odometry_measurement.segment(7, 3);
    ouzel_angular_vel_B_ = odometry_measurement.segment(10, 3);

    Eigen::VectorXd odometry_measurement_gt = odometry_.getMeasGT();
    ouzel_position_W_gt_ = odometry_measurement_gt.segment(0, 3);
    ouzel_orientation_W_B_gt_ = Eigen::Quaterniond(odometry_measurement_gt(3),
                                                   odometry_measurement_gt(4),
                                                   odometry_measurement_gt(5),
                                                   odometry_measurement_gt(6))
                                    .normalized();
    ouzel_linear_vel_B_gt_ = odometry_measurement_gt.segment(7, 3);
    ouzel_angular_vel_B_gt_ = odometry_measurement_gt.segment(10, 3);

    delta_joint_angle_ = delta_sym_->getqPos();
    delta_joint_angular_vel_ = delta_sym_->getqVel();

    Eigen::Vector3d end_effector_position_D;
    delta_sym_->fwkinPosition(&end_effector_position_D);

    delta_position_W_ = ouzel_position_W_ + pos_offset_BD_ +
                        ouzel_orientation_W_B_.matrix() *
                            ang_offset_BD_.matrix() * end_effector_position_D;
    delta_position_W_gt_ = ouzel_position_W_gt_ + pos_offset_BD_ +
                           ouzel_orientation_W_B_gt_.matrix() *
                               ang_offset_BD_.matrix() *
                               end_effector_position_D;

    ouzel_->getState(gc_, gv_);
    if (!Eigen::isfinite(gc_.array()).all()) {
      std::cout << "gc is nan!!" << std::endl;
      std::cout << "odometry : " << odometry_measurement << std::endl;
      std::cout << "gc : " << gc_ << std::endl;
      std::cout << "gv : " << gv_ << std::endl;
      raise(SIGTERM);
    }
  }

  void observe(Eigen::Ref<EigenVec> ob) final {
    /// convert it to float
    Eigen::Vector3d ref_delta_position_offset_W =
        delta_position_W_ - ref_delta_position_; // ref to current
    Eigen::Vector3d delta_ouzel_position_offet_W =
        ouzel_position_W_ - delta_position_W_; // delta to ouzel
    Eigen::Matrix3d ouzel_orientation_W_B_mat =
        ouzel_orientation_W_B_.toRotationMatrix();
    Eigen::Matrix3d ref_ouzel_orientation_mat =
        ref_ouzel_orientation_.toRotationMatrix();
    Eigen::VectorXd ob_double(obDim_);
    ob_double << ref_delta_position_offset_W, delta_ouzel_position_offet_W,
        ouzel_orientation_W_B_mat.col(0), ouzel_orientation_W_B_mat.col(1),
        ouzel_orientation_W_B_mat.col(2), ouzel_linear_vel_B_,
        ouzel_angular_vel_B_, ref_ouzel_orientation_mat.col(0),
        ref_ouzel_orientation_mat.col(1), ref_ouzel_orientation_mat.col(2),
        delta_joint_angle_, delta_joint_angular_vel_;
    ob = ob_double.cast<float>();
    if (!Eigen::isfinite(ob.array()).all()) {
      std::cout << "ob is nan: " << ob << std::endl;
    }
  }

  bool isTerminalState(float &terminalReward) final {
    double waypoint_dist_delta, error_angle;
    computeErrorMetrics(waypoint_dist_delta, error_angle);
    Eigen::VectorXd odometry_measurement_gt = odometry_.getMeasGT();
    Eigen::Vector3d lin_vel_gt = odometry_measurement_gt.segment(7, 3);
    Eigen::Vector3d ang_vel_gt = odometry_measurement_gt.segment(10, 3);
    if (waypoint_dist_delta > terminalOOBWaypointDist_ ||
        error_angle > terminalOOBAngleError_) {
      terminalReward = terminalOOBRewardCoeff_;
      return true;
    } else if (waypoint_dist_delta < terminalSuccessWaypointDist_ &&
               error_angle < terminalSuccessAngleError_ &&
               lin_vel_gt.norm() < terminalSuccessLinearVel_ &&
               ang_vel_gt.norm() < terminalSuccessAngularVel_) {
      time_in_success_state_ += control_dt_;
      if (time_in_success_state_ >= min_time_in_success_state_) {
        terminalReward = terminalSuccessRewardCoeff_;
        return true;
      }
      terminalReward = 0.f;
      return false;
    } else {
      terminalReward = 0.f;
      time_in_success_state_ = 0.0;
      return false;
    }
  }

  void curriculumUpdate(){};

  void setSeed(int seed) { gen_.seed(seed); }

private:
  void resetInitialConditions() {
    Eigen::Vector3d init_position(unifDistPlusMinusOne_(gen_),
                                  unifDistPlusMinusOne_(gen_),
                                  unifDistPlusMinusOne_(gen_));
    ouzel_position_W_ = init_position;

    Eigen::Vector3d init_orientation(unifDistPlusMinusOne_(gen_),
                                     unifDistPlusMinusOne_(gen_),
                                     unifDistPlusMinusOne_(gen_));
    init_orientation.normalize();
    double init_angle = unifDistPlusMinusOne_(gen_) * M_PI;
    Eigen::Quaterniond init_quaternion(
        Eigen::AngleAxisd(init_angle, init_orientation));
    init_quaternion.normalize();
    ouzel_orientation_W_B_ = init_quaternion;

    Eigen::Vector3d init_lin_vel_dir(unifDistPlusMinusOne_(gen_),
                                     unifDistPlusMinusOne_(gen_),
                                     unifDistPlusMinusOne_(gen_));
    Eigen::Vector3d init_lin_vel = init_lin_vel_dir.normalized() *
                                   initialLinearVel_ *
                                   unifDistPlusMinusOne_(gen_);
    ouzel_linear_vel_B_ = init_lin_vel;
    Eigen::Vector3d init_lin_vel_W =
        ouzel_orientation_W_B_.toRotationMatrix() * init_lin_vel;

    Eigen::Vector3d init_ang_vel_axis(unifDistPlusMinusOne_(gen_),
                                      unifDistPlusMinusOne_(gen_),
                                      unifDistPlusMinusOne_(gen_));
    Eigen::Vector3d init_ang_vel = init_ang_vel_axis.normalized() *
                                   initialAngularVel_ *
                                   unifDistPlusMinusOne_(gen_);
    ouzel_angular_vel_B_ = init_ang_vel;
    Eigen::Vector3d init_ang_vel_W =
        ouzel_orientation_W_B_.toRotationMatrix() * init_ang_vel;

    gc_init_ << ouzel_position_W_.x(), ouzel_position_W_.y(),
        ouzel_position_W_.z(), // position
        ouzel_orientation_W_B_.w(), ouzel_orientation_W_B_.x(),
        ouzel_orientation_W_B_.y(),
        ouzel_orientation_W_B_.z(), // orientation quaternion
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

    gv_init_ << init_lin_vel_W.x(), init_lin_vel_W.y(), init_lin_vel_W.z(),
        init_ang_vel_W.x(), init_ang_vel_W.y(), init_ang_vel_W.z(), 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

    // reset delta arm
    Eigen::Vector3d init_joint_angles(
        std::abs(unifDistPlusMinusOne_(gen_)) * 1.4,
        std::abs(unifDistPlusMinusOne_(gen_)) * 1.4,
        std::abs(unifDistPlusMinusOne_(gen_)) * 1.4);
    delta_sym_->setJointAngles(init_joint_angles);

    // reset reference
    Eigen::Vector3d ref_delta_position(unifDistPlusMinusOne_(gen_),
                                       unifDistPlusMinusOne_(gen_),
                                       unifDistPlusMinusOne_(gen_));
    ref_delta_position.normalize();
    ref_delta_position_ = ouzel_position_W_ + initialDistanceOffset_ *
                                                  unifDistPlusMinusOne_(gen_) *
                                                  ref_delta_position;

    Eigen::Vector3d ref_delta_orientation(unifDistPlusMinusOne_(gen_),
                                          unifDistPlusMinusOne_(gen_),
                                          unifDistPlusMinusOne_(gen_));
    ref_delta_orientation.normalize();
    double ref_delta_angle =
        unifDistPlusMinusOne_(gen_) * initialAngularOffset_;
    Eigen::Quaterniond ref_delta_quaternion(
        Eigen::AngleAxisd(ref_delta_angle, ref_delta_orientation));
    ref_delta_quaternion.normalize();
    Eigen::Quaterniond ref_quaternion =
        ouzel_orientation_W_B_ * ref_delta_quaternion;
    ref_quaternion.normalize();
    ref_ouzel_orientation_ = ref_quaternion;

    controller_.setRef(ref_delta_position_, ref_ouzel_orientation_);
  }

  void computeErrorMetrics(double &waypointDist, double &errorAngle) {
    waypointDist = (delta_position_W_gt_ - ref_delta_position_).norm();
    errorAngle =
        ouzel_orientation_W_B_gt_.angularDistance(ref_ouzel_orientation_);
  }

  Eigen::Quaterniond
  QuaternionFromTwoVectors(Eigen::Vector3d _orientation_vec_1,
                           Eigen::Vector3d _orientation_vec_2) {
    Eigen::Matrix3d orientation_mat;
    orientation_mat.setIdentity();
    if (_orientation_vec_1.norm() > 0 && _orientation_vec_2.norm() > 0 &&
        _orientation_vec_1.cross(_orientation_vec_2).norm() > 0) {
      /// Gram-Schmidt
      Eigen::Vector3d e1 = _orientation_vec_1 / _orientation_vec_1.norm();
      Eigen::Vector3d u2 = _orientation_vec_2 - e1.dot(_orientation_vec_2) * e1;
      Eigen::Vector3d e2 = u2 / u2.norm();
      orientation_mat.col(0) = e1;
      orientation_mat.col(1) = e2;
      orientation_mat.col(2) = e1.cross(e2);
    }
    return Eigen::Quaterniond(orientation_mat);
  }

  int gcDim_, gvDim_, nJoints_;
  bool visualizable_ = false;
  bool render_ = false;
  raisim::ArticulatedSystem *ouzel_;
  Eigen::VectorXd gc_init_, gv_init_, gc_, gv_, pTarget_, pTarget12_, vTarget_;
  double initialDistanceOffset_;
  double initialAngularOffset_;
  double initialLinearVel_;
  double initialAngularVel_;
  float terminalOOBRewardCoeff_;
  float terminalSuccessRewardCoeff_;
  double terminalOOBWaypointDist_;
  double terminalOOBAngleError_;
  double terminalSuccessWaypointDist_;
  double terminalSuccessAngleError_;
  double terminalSuccessLinearVel_;
  double terminalSuccessAngularVel_;
  Eigen::Vector3d ouzel_position_W_, ouzel_linear_vel_B_, ouzel_angular_vel_B_,
      delta_joint_angle_, delta_joint_angular_vel_;
  Eigen::Quaterniond ouzel_orientation_W_B_;
  Eigen::Vector3d ouzel_position_W_gt_, ouzel_linear_vel_B_gt_,
      ouzel_angular_vel_B_gt_;
  Eigen::Quaterniond ouzel_orientation_W_B_gt_;
  std::set<size_t> footIndices_;
  Eigen::Vector3d ref_delta_position_;
  Eigen::Quaterniond ref_ouzel_orientation_;
  Eigen::Vector3d delta_ouzel_ref_position_offset_previous_;
  Eigen::Vector3d ref_delta_joint_pos_previous_;

  rw_omav_controllers::ImpedanceControlModule controller_;

  raisim_sensors::odometry odometry_;

  delta_dynamics::DeltaController *delta_sym_;
  raisim::Cylinder *delta_eef_;
  Eigen::Vector3d ee_vel_prev_;
  Eigen::Vector3d B_om_WB_prev_;
  Eigen::Vector3d pos_offset_BD_;
  Eigen::Quaterniond ang_offset_BD_;
  double delta_min_joint_angle_ = 0.0;
  double delta_max_joint_angle_ = 1.5; // rad
  Eigen::Vector3d delta_position_W_;
  Eigen::Vector3d delta_position_W_gt_;

  int baseLink_;

  float time_in_success_state_;
  float min_time_in_success_state_;

  thread_local static std::mt19937 gen_;
  std::uniform_real_distribution<double> unifDistPlusMinusOne_;
};
thread_local std::mt19937 raisim::ENVIRONMENT::gen_;

} // namespace raisim


/**
 * \ file BlockLMSFilter.cpp
 */

#include <array>
#include <fstream>
#include <iostream>

#include <ATK/Adaptive/BlockLMSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

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

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

constexpr gsl::index PROCESSSIZE = 1200;

BOOST_AUTO_TEST_CASE(BlockLMSFilter_size_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_size(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_size_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_size(10);
  BOOST_CHECK_EQUAL(filter.get_size(), 10);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_build_throw_test)
{
  BOOST_CHECK_THROW(ATK::BlockLMSFilter<double> filter(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_blocksize_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_block_size(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_blocksize_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_block_size(10);
  BOOST_CHECK_EQUAL(filter.get_block_size(), 10);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_memory_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_memory_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_memory(0.5);
  BOOST_CHECK_EQUAL(filter.get_memory(), 0.5);
}

BOOST_AUTO_TEST_CASE( BlockLMSFilter_memory_positive1_test )
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_mu_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_mu(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_mu_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_mu(0.5);
  BOOST_CHECK_EQUAL(filter.get_mu(), 0.5);
}

BOOST_AUTO_TEST_CASE( BlockLMSFilter_mu_positive1_test )
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_mu(1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(BlockLMSFilter_learning_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  BOOST_CHECK_EQUAL(filter.get_learning(), true);
  filter.set_learning(false);
  BOOST_CHECK_EQUAL(filter.get_learning(), false);
}

BOOST_AUTO_TEST_CASE( BlockLMSFilter_memory_999_test )
{
  std::array<double, PROCESSSIZE> data;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::BlockLMSFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.999);
  filter.set_mu(.0001);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_output_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  output.set_input_port(0, &filter, 0);

  filter.process(100);
  output.process(PROCESSSIZE);

  std::array<double, PROCESSSIZE> ref;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_blocklms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(ref.data()), PROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(outdata[i], ref[i], 0.0001);
  }
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef MINIMIZER_H
#define MINIMIZER_H

#include <iostream>
#include <cassert>
#include <utility>
#include <Eigen/Core>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

#include "error.hpp"
#include "ewsb_solver.hpp"
#include "logger.hpp"
#include "gsl_utils.hpp"
#include "gsl_vector.hpp"

namespace flexiblesusy {

/**
 * @class Minimizer
 * @brief Function minimizer
 *
 * The user has to provide the function to be minimized of the type
 * Function_t.  This function gets as arguments an Eigen vector of
 * lenght `dimension' and returns a double.
 *
 * Example:
 * @code
 * auto parabola = [](const Eigen::Matrix<double,2,1>& x) -> double {
 *    const double y = x(0);
 *    const double z = x(1);
 *    return Sqr(y - 5.0) + Sqr(z - 1.0);
 * };
 *
 * Minimizer<2> minimizer(parabola, 100, 1.0e-5);
 * const double start[2] = { 10, 10 };
 * const int status = minimizer.minimize(start);
 * @endcode
 */
template <std::size_t dimension>
class Minimizer : public EWSB_solver {
public:
   using Vector_t = Eigen::Matrix<double,dimension,1>;
   using Function_t = std::function<double(const Vector_t&)>;
   enum Solver_type { GSLSimplex, GSLSimplex2, GSLSimplex2Rand };

   Minimizer() = default;
   template <typename F>
   Minimizer(F&&, std::size_t, double, Solver_type solver_type_ = GSLSimplex2);
   virtual ~Minimizer() = default;

   double get_minimum_value() const { return minimum_value; }
   template <typename F>
   void set_function(F&& f) { function = std::forward<F>(f); }
   void set_precision(double p) { precision = p; }
   void set_max_iterations(std::size_t n) { max_iterations = n; }
   void set_solver_type(Solver_type t) { solver_type = t; }
   int minimize(const Vector_t&);

   // EWSB_solver interface methods
   virtual std::string name() const override { return "Minimizer"; }
   virtual int solve(const Eigen::VectorXd&) override;
   virtual Eigen::VectorXd get_solution() const override { return minimum_point; }

private:
   std::size_t max_iterations{100};     ///< maximum number of iterations
   double precision{1.e-2};             ///< precision goal
   double minimum_value{0.};            ///< minimum function value found
   Vector_t minimum_point{Vector_t::Zero()}; ///< vector of minimum point
   Function_t function{nullptr};        ///< function to minimize
   Solver_type solver_type{GSLSimplex2};///< solver type

   void print_state(gsl_multimin_fminimizer*, std::size_t) const;
   static double gsl_function(const gsl_vector*, void*);
   const gsl_multimin_fminimizer_type* solver_type_to_gsl_pointer() const;
};

/**
 * Constructor
 *
 * @param function_ pointer to the function to minimize
 * @param max_iterations_ maximum number of iterations
 * @param precision_ precision goal
 * @param solver_type_ GSL multimin minimizer type
 */
template <std::size_t dimension>
template <typename F>
Minimizer<dimension>::Minimizer(
   F&& function_,
   std::size_t max_iterations_,
   double precision_,
   Solver_type solver_type_
)
   : max_iterations(max_iterations_)
   , precision(precision_)
   , function(std::forward<F>(function_))
   , solver_type(solver_type_)
{
}

/**
 * Start the minimization
 *
 * @param start starting point
 *
 * @return GSL error code (GSL_SUCCESS if minimum found)
 */
template <std::size_t dimension>
int Minimizer<dimension>::minimize(const Vector_t& start)
{
   if (!function)
      throw SetupError("Minimizer: function not callable");

   gsl_multimin_fminimizer *minimizer;
   gsl_multimin_function minex_func;

   GSL_vector min_point = to_GSL_vector(start);

   // Set initial step sizes
   GSL_vector step_size(dimension);
   step_size.set_all(1.0);

   // Initialize method and iterate
   minex_func.n = dimension;
   minex_func.f = gsl_function;
   minex_func.params = &function;

   minimizer = gsl_multimin_fminimizer_alloc(solver_type_to_gsl_pointer(), dimension);
   gsl_multimin_fminimizer_set(minimizer, &minex_func, min_point.raw(), step_size.raw());

   size_t iter = 0;
   int status;

   do {
      iter++;
      status = gsl_multimin_fminimizer_iterate(minimizer);

      if (status)
         break;

      const double size = gsl_multimin_fminimizer_size(minimizer);
      status = gsl_multimin_test_size(size, precision);

#ifdef ENABLE_VERBOSE
      print_state(minimizer, iter);
#endif
   } while (status == GSL_CONTINUE && iter < max_iterations);

   VERBOSE_MSG("\t\t\tMinimization status = " << gsl_strerror(status));

   // save minimum point and function value
   minimum_point = to_eigen_vector_fixed<dimension>(minimizer->x);
   minimum_value = minimizer->fval;

   gsl_multimin_fminimizer_free(minimizer);

   return status;
}

/**
 * Print state of the minimizer
 *
 * @param minimizer minimizer
 * @param iteration iteration number
 */
template <std::size_t dimension>
void Minimizer<dimension>::print_state(gsl_multimin_fminimizer* minimizer,
                                               std::size_t iteration) const
{
   VERBOSE_MSG("\t\t\tIteration " << iteration
               << ": x = " << GSL_vector(minimizer->x)
               << ", f(x) = " << minimizer->fval);
}

template <std::size_t dimension>
int Minimizer<dimension>::solve(const Eigen::VectorXd& start)
{
   return (minimize(start) == GSL_SUCCESS ?
           EWSB_solver::SUCCESS : EWSB_solver::FAIL);
}

template <std::size_t dimension>
double Minimizer<dimension>::gsl_function(const gsl_vector* x, void* params)
{
   if (!is_finite(x))
      return std::numeric_limits<double>::max();

   Function_t* fun = static_cast<Function_t*>(params);
   const Vector_t arg(to_eigen_vector_fixed<dimension>(x));
   double result = std::numeric_limits<double>::max();

   try {
      result = (*fun)(arg);
   } catch (const flexiblesusy::Error&) {
   }

   return result;
}

template <std::size_t dimension>
const gsl_multimin_fminimizer_type* Minimizer<dimension>::solver_type_to_gsl_pointer() const
{
   switch (solver_type) {
   case GSLSimplex     : return gsl_multimin_fminimizer_nmsimplex;
   case GSLSimplex2    : return gsl_multimin_fminimizer_nmsimplex2;
   case GSLSimplex2Rand: return gsl_multimin_fminimizer_nmsimplex2rand;
   default:
      throw SetupError("Unknown minimizer solver type: "
                       + std::to_string(solver_type));
   }

   return nullptr;
}

} // namespace flexiblesusy

#endif


#ifndef ASLAM_BACKEND_DV_MATRIX_HPP
#define ASLAM_BACKEND_DV_MATRIX_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include <boost/shared_ptr.hpp>
#include <set>

namespace aslam {
namespace backend {

/**
 * \class MatrixExpressionNode
 * \brief The superclass of all classes representing transformations.
 */
class MatrixExpressionNode
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	MatrixExpressionNode();
	virtual ~MatrixExpressionNode();

	/// \brief Evaluate the transformation matrix.
	Eigen::Matrix3d toMatrix3x3();

	/// \brief Evaluate the Jacobians
	void evaluateJacobians(JacobianContainer & outJacobians) const;

	/// \brief Evaluate the Jacobians and apply the chain rule.
	void evaluateJacobians(JacobianContainer & outJacobians,
			const Eigen::MatrixXd & applyChainRule) const;
	void getDesignVariables(DesignVariable::set_t & designVariables) const;
protected:
	// These functions must be implemented by child classes.
	virtual Eigen::Matrix3d toMatrix3x3Implementation() = 0;
	virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const = 0;
	virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians,
			const Eigen::MatrixXd & applyChainRule) const = 0;
	virtual void getDesignVariablesImplementation(
			DesignVariable::set_t & designVariables) const = 0;

};

} // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_DV_MATRIX_HPP */


#ifndef INCLUDE_SWIFT_VIO_IMU_RIG_HPP_
#define INCLUDE_SWIFT_VIO_IMU_RIG_HPP_

#include <memory>

#include <Eigen/Core>
#include <Eigen/StdVector>

#include <swift_vio/imu/ImuModels.hpp>
#include <swift_vio/ceres/EuclideanParamBlockSized.hpp>

#include <okvis/Parameters.hpp>

namespace swift_vio {
class ImuModel {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /**
   * @brief ImuModel
   * @param modelId
   * @param bg
   * @param ba
   * @param extraParams
   */
  ImuModel(int modelId, const Eigen::Vector3d bg, Eigen::Vector3d ba,
           const Eigen::VectorXd &extraParams)
      : modelId_(modelId), gyroBias_(bg), accelerometerBias_(ba),
        extraParams_(extraParams) {}

  inline Eigen::VectorXd getImuAugmentedParams() const {
    return extraParams_;
  }

  inline int modelId() const {
    return modelId_;
  }

  Eigen::VectorXd computeImuAugmentedParamsError() const {
    return ImuModelComputeAugmentedParamsError(modelId_, extraParams_);
  }

  inline void setImuAugmentedParams(const Eigen::VectorXd& extraParams) {
    extraParams_ = extraParams;
  }

private:
  int modelId_;
  Eigen::Vector3d gyroBias_;
  Eigen::Vector3d accelerometerBias_;
  Eigen::VectorXd extraParams_;
};

class ImuRig {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  inline ImuRig() {}
  inline int getModelId(int index) const {
    if ((int)imus_.size() > index) {
      return imus_[index].modelId();
    } else {
      return -1;
    }
  }

  int addImu(const okvis::ImuParameters& imuParams);

  inline int getImuParamsMinimalDim(int imu_id=0) const {
    return ImuModelGetMinimalDim(imus_[imu_id].modelId());
  }

  inline Eigen::VectorXd getImuAugmentedParams(int imu_id=0) const {
    return imus_.at(imu_id).getImuAugmentedParams();
  }

  inline Eigen::VectorXd computeImuAugmentedParamsError(int imu_id=0) const {
    return imus_.at(imu_id).computeImuAugmentedParamsError();
  }

  inline void setImuAugmentedParams(int imu_id, const Eigen::VectorXd& euclideanParams) {
    imus_.at(imu_id).setImuAugmentedParams(euclideanParams);
  }

  inline int getAugmentedMinimalDim(int imu_id) const {
    return ImuModelGetAugmentedMinimalDim(imus_[imu_id].modelId());
  }

  inline int getAugmentedDim(int imu_id) const {
    return ImuModelGetAugmentedDim(imus_[imu_id].modelId());
  }
private:
  std::vector<ImuModel> imus_;
};

/**
 * @brief getImuAugmentedStatesEstimate get augmented IMU parameters except for
 *     biases from parameter blocks.
 * @param imuAugmentedParameterPtrs[in]
 * @param extraParams[out]
 * @param imuModelId[in]
 */
void getImuAugmentedStatesEstimate(
     std::vector<std::shared_ptr<const okvis::ceres::ParameterBlock>> imuAugmentedParameterPtrs,
    Eigen::Matrix<double, Eigen::Dynamic, 1>* extraParams, int imuModelId);

}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_IMU_RIG_HPP_


// morpho/cdl/bgl_expansions/algorithms/ullmann.hpp header file//
// Copyright (c) 2003-2008 Vladimir J. Sykora
// Copyright (c) 2007-2008 Vladimir J. Sykora and NCU Studies Ltd
// Modifications by Greg Landrum, January 2009
//
//*****************************************************************************
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

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

//-----------------------------------------------------------------------------

// Algorithm due to JR Ullmann. "An Algorithm for Subgraph
// Isomorphism". Journal of the Association for
// Computing Machinery, Vol 23, No.1, January 1976, pp 31-42.

#ifndef MORPHO_CDL_BGL_EXP_ULLMANN_HPP
#define MORPHO_CDL_BGL_EXP_ULLMANN_HPP

// -------- boost
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/graph/graph_traits.hpp>
// -------- std
#include <utility>    // for std::pair<>
#include <algorithm>

namespace boost {
  namespace detail {

    template <class Graph, class UblasMatrix, class BackInsertionSequence, class EdgeLabeling>
    bool forward_checking(const Graph& g1, const Graph& g2,
                          UblasMatrix& M, size_t count, BackInsertionSequence& F, size_t num_vert_g1,
                          size_t num_vert_g2, EdgeLabeling& edge_labeling){
      typedef std::pair<typename graph_traits<Graph>::edge_descriptor,bool> edge_presence;
      typename BackInsertionSequence::iterator   fi, fi_end=F.end();
      for (size_t k=count+1; k<num_vert_g1; ++k) {
        for (size_t l=0; l<num_vert_g2; ++l) {
          if(!M(k,l)) continue;
          // check mapping:
          fi=F.begin();
          while(fi!=fi_end) {
            bool flag1(0), flag1_1(0), flag2(0), flag2_1(0);
            edge_presence ep1=edge(k,fi->first,g1);
            if(ep1.second) {
              flag1_1=true;
              edge_presence ep2=edge(l,fi->second,g2);
              if(ep2.second) flag1=edge_labeling(ep1.first,ep2.first);
            }
            if(flag1_1 && flag1) {
              M(k,l)=1;
              ++fi;
              continue;
            }
            edge_presence ep2=edge(l,fi->second,g2);
            if(ep2.second) {
              flag2_1=true;
              ep1=edge(k,fi->first,g1);
              if(ep1.second){
                flag2=edge_labeling(ep1.first,ep2.first);
              } else {
                // ring closed in main structure, not closed in query. This should pass
                flag2=true;
              }
            }
            if(flag2_1 && flag2) {  //if one edge exists, there must be a mapping
              M(k,l)=1;
            }
            else if ( !flag1_1 && !flag2_1 ) { // or both edges are not present
              M(k,l)=1;
            }
            else M(k,l)=0;         // if not, there's no mapping
            ++fi;
          }
        }
      }
      // TODO: change the data structure of the M matrix to sparse matrix. This wouldn't be necessary
      size_t cero_row(0);
      for (size_t k=0; k<num_vert_g1; ++k) {
        for (size_t l=0; l<num_vert_g2; ++l) {
          if(M(k,l)) break;
          else ++cero_row;
        }
        if(cero_row==num_vert_g2) return false; // if there is a cero row
        cero_row=0;
      }
      return true;
    }

    template <class Graph, class EdgeLabeling, class UblasMatrix, class BackInsertionSequence>
    bool backtrack(const Graph& g1, const Graph& g2, size_t count,
                   const UblasMatrix& M, BackInsertionSequence& F, const size_t num_vert_g1,
                   const size_t num_vert_g2, EdgeLabeling& edge_labeling){
      if(count==num_vert_g1) return true;
      for (size_t i=0; i<num_vert_g2; ++i) {
        if(M(count,i)) {
          F.push_back(std::make_pair(count,i));
          UblasMatrix M_prime(M);
          for (size_t m=count+1; m<num_vert_g1; ++m) {
            M_prime(m,i)=0;
          }

          if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
            if(backtrack(g1,g2,count+1,M_prime,F,num_vert_g1,num_vert_g2,edge_labeling)){
              return true;
            }
          }
          F.erase(std::remove(F.begin(),F.end(),std::make_pair(count,i)), F.end());
        }
      }
      return false;
    }

    template <class Graph, class EdgeLabeling, class UblasMatrix, class DoubleBackInsertionSequence>
    void backtrack_all(const Graph& g1, const Graph& g2, size_t count,
                       const UblasMatrix& M, DoubleBackInsertionSequence& FF, const size_t num_vert_g1,
                       const size_t num_vert_g2, EdgeLabeling& edge_labeling) {
      if(count==num_vert_g1) return;
      DoubleBackInsertionSequence holdFF;
      holdFF.insert(holdFF.begin(),FF.begin(),FF.end());
      FF.clear();
      for (size_t i=0; i<num_vert_g2; ++i) {
        if(M(count,i)) {
          DoubleBackInsertionSequence tFF;

          UblasMatrix M_prime(M);
          for (size_t m=count+1; m<num_vert_g1; ++m){
            M_prime(m,i)=0;
          }

          if(holdFF.size()){
            for(typename DoubleBackInsertionSequence::const_iterator iter=holdFF.begin();
                iter!=holdFF.end();++iter){
              typename DoubleBackInsertionSequence::value_type F=*iter;
              F.push_back(std::make_pair(count,i));
              if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
                tFF.push_back(F);
              }
            }
          } else {
            typename DoubleBackInsertionSequence::value_type F;
            F.push_back(std::make_pair(count,i));
            if (forward_checking(g1,g2,M_prime,count,F,num_vert_g1,num_vert_g2,edge_labeling)){
              tFF.push_back(F);
            }
          }
          backtrack_all(g1,g2,count+1,M_prime,tFF,num_vert_g1,num_vert_g2,edge_labeling);
          if(tFF.size()){
            for(typename DoubleBackInsertionSequence::const_iterator iter=tFF.begin();
                iter!=tFF.end();++iter){
              FF.push_back(*iter);
            }
          }
        }
      }
    }

    template <  class Graph
                , class VertexLabeling    // binary predicate
                ,  class UblasMatrix
                >
    void prepareM(const Graph& g1, const Graph& g2,
                  VertexLabeling& vertex_labeling,UblasMatrix &M){
      size_t rows(num_vertices(g1));
      size_t cols(num_vertices(g2));
      M.resize(rows,cols);
      // initialize the matrix:
      for (size_t i=0; i<rows; ++i) {
        for (size_t j=0; j<cols; ++j) {
          if(out_degree(j,g2)>=out_degree(i,g1) && vertex_labeling(i,j)) {
            M(i,j)=1;
          }
          else M(i,j)=0;
        }
      }
    }
  }  // namespace detail

  // test if g1 is a subgraph of g2. mapped vertices are returned in F
  // Mapping : first: g1 vertices, second : g2 vertices
  // O( num_vertices(g1)! num_vertices(g1) ^ 3 )
  // This function doesnt Work with filtered graphs!
  // The size of F doesn't indicate match!
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class BackInsertionSequence   // contains std::pair<vertex_descriptor,vertex_descriptor>
              >
  bool ullmann(const Graph& g1, const Graph& g2,
               VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling, BackInsertionSequence& F) {
    typedef ::boost::numeric::ublas::matrix<int>   matrix_t;
    size_t rows(num_vertices(g1));
    size_t cols(num_vertices(g2));
    matrix_t M;
    detail::prepareM(g1,g2,vertex_labeling,M);
    size_t    count(0);
    return detail::backtrack(g1,g2,count,M,F,rows,cols,edge_labeling);
  }

  // test if g1 is a subgraph of g2.
  // F returns all mappings of g1 in g2. mapping in separate containers
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class DoubleBackInsertionSequence   // contains a back insertion sequence
              >
  bool ullmann_all(const Graph& g1, const Graph& g2,
                   VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling, DoubleBackInsertionSequence& F) {
    typedef ::boost::numeric::ublas::matrix<int>   matrix_t;
    size_t rows(num_vertices(g1));
    size_t cols(num_vertices(g2));
    matrix_t M;
    detail::prepareM(g1,g2,vertex_labeling,M);
    size_t    count(0);
    detail::backtrack_all(g1,g2,count,M,F,rows,cols,edge_labeling);
    return !F.empty();
  }

}  // namespace boost

#endif // MORPHO_CDL_BGL_EXP_ULLMANN_HPP


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#include <boost/test/unit_test.hpp>
#include "armnnTfLiteParser/ITfLiteParser.hpp"
#include "ParserFlatbuffersFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct AvgPool2DFixture : public ParserFlatbuffersFixture
{
    explicit AvgPool2DFixture(std::string inputdim, std::string outputdim, std::string dataType)
    {
        m_JsonString = R"(
        {
            "version": 3,
            "operator_codes": [ { "builtin_code": "AVERAGE_POOL_2D" } ],
            "subgraphs": [
            {
                "tensors": [
                {
                    "shape": )"
                    + outputdim
                    + R"(,
                    "type": )"
                      + dataType
                      + R"(,
                            "buffer": 0,
                            "name": "OutputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ]
                            }
                },
                {
                    "shape": )"
                    + inputdim
                    + R"(,
                    "type": )"
                      + dataType
                      + R"(,
                            "buffer": 1,
                            "name": "InputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ]
                            }
                }
                ],
                "inputs": [ 1 ],
                "outputs": [ 0 ],
                "operators": [ {
                        "opcode_index": 0,
                        "inputs": [ 1 ],
                        "outputs": [ 0 ],
                        "builtin_options_type": "Pool2DOptions",
                        "builtin_options":
                        {
                            "padding": "VALID",
                            "stride_w": 2,
                            "stride_h": 2,
                            "filter_width": 2,
                            "filter_height": 2,
                            "fused_activation_function": "NONE"
                        },
                        "custom_options_format": "FLEXBUFFERS"
                    } ]
                }
            ],
            "description": "AvgPool2D test.",
            "buffers" : [ {}, {} ]
        })";

        SetupSingleInputSingleOutput("InputTensor", "OutputTensor");
    }
};

struct AvgPoolLiteFixtureUint1DOutput : AvgPool2DFixture
{
    AvgPoolLiteFixtureUint1DOutput() : AvgPool2DFixture("[ 1, 2, 2, 1 ]", "[ 1, 1, 1, 1 ]", "UINT8") {}
};

struct AvgPoolLiteFixtureFloat1DOutput : AvgPool2DFixture
{
    AvgPoolLiteFixtureFloat1DOutput() : AvgPool2DFixture("[ 1, 2, 2, 1 ]", "[ 1, 1, 1, 1 ]", "FLOAT32") {}
};

struct AvgPoolLiteFixture2DOutput : AvgPool2DFixture
{
    AvgPoolLiteFixture2DOutput() : AvgPool2DFixture("[ 1, 4, 4, 1 ]", "[ 1, 2, 2, 1 ]", "UINT8") {}
};

BOOST_FIXTURE_TEST_CASE(AvgPoolLite1DOutput, AvgPoolLiteFixtureUint1DOutput)
{
    RunTest<4, armnn::DataType::QuantisedAsymm8>(0, {2, 3, 5, 2 }, { 3 });
}

BOOST_FIXTURE_TEST_CASE(AvgPoolLiteFloat1DOutput, AvgPoolLiteFixtureFloat1DOutput)
{
    RunTest<4, armnn::DataType::Float32>(0, { 2.0f, 3.0f, 5.0f, 2.0f },  { 3.0f });
}

BOOST_FIXTURE_TEST_CASE(AvgPoolLite2DOutput, AvgPoolLiteFixture2DOutput)
{
    RunTest<4, armnn::DataType::QuantisedAsymm8>(
        0, { 1, 2, 2, 3, 5, 6, 7, 8, 3, 2, 1, 0, 1, 2, 3, 4 }, { 4, 5, 2, 2 });
}

BOOST_FIXTURE_TEST_CASE(IncorrectDataTypeError, AvgPoolLiteFixtureFloat1DOutput)
{
    BOOST_CHECK_THROW((RunTest<4, armnn::DataType::QuantisedAsymm8>(0, {2, 3, 5, 2 }, { 3 })), armnn::Exception);
}

BOOST_AUTO_TEST_SUITE_END()


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <algorithm>

#include <boost/make_shared.hpp>

#include "tudat/simulation/estimation_setup/createAngularPositionPartials.h"
#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h"

namespace tudat
{

namespace observation_partials
{

//! Function to generate angular position partial wrt a position of a body.
std::shared_ptr< AngularPositionPartial > createAngularPositionPartialWrtBodyPosition(
        const observation_models::LinkEnds angularPositionLinkEnds,
        const simulation_setup::SystemOfBodies& bodies,
        const std::string bodyToEstimate,
        const std::shared_ptr< AngularPositionScaling > angularPositionScaler,
        const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >&
        lightTimeCorrectionPartialObjects )
{
    // Create position partials of link ends for current body position
    std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials =
            createCartesianStatePartialsWrtBodyState( angularPositionLinkEnds, bodies, bodyToEstimate );

    // Create angular position partials if any position partials are created (i.e. if any dependency exists).
    std::shared_ptr< AngularPositionPartial > angularPositionPartial;
    if( positionPartials.size( ) > 0 )
    {
        angularPositionPartial = std::make_shared< AngularPositionPartial >(
                    angularPositionScaler, positionPartials, std::make_pair(
                        estimatable_parameters::initial_body_state, std::make_pair( bodyToEstimate, "" ) ),
                    lightTimeCorrectionPartialObjects );
    }

    return angularPositionPartial;
}

}

}


#include <cassert>
#include <cstring>
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>

#define GLEW_STATIC
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#define EIGEN_DONT_ALIGN_STATICALLY
#include <Eigen/Dense>

#include "Camera.h"
#include "GLSL.h"
#include "MatrixStack.h"
#include "OctreeNode.h"
#include "Program.h"
#include "Shape.h"
#include "Texture.h"
#include "WorldObject.h"

using namespace std;
using namespace Eigen;

GLFWwindow *window;         // Main application window
string RESOURCE_DIR = "./"; // Where the resources are loaded from

shared_ptr<Camera> camera;
shared_ptr<Program> prog;
shared_ptr<Program> silProg;
shared_ptr<Program> transProg;
shared_ptr<Shape> bunny; // This saves the bunny shape
shared_ptr<Shape> sphere; // This saves the sphere shape
shared_ptr<Shape> teapot; // This saves the teapot shape
vector<shared_ptr<WorldObject>> objs; // This saves the world objects (this includes shape and octree)
shared_ptr<Texture> gridTex;

bool keyToggles[256] = {false}; // only for English keyboards!
bool collision = false;

// This function is called when a GLFW error occurs.
static void error_callback(int error, const char *description) {
    cerr << description << endl;
}

// This function is called when a key is pressed.
static void key_callback(GLFWwindow *window, int key, int scancode, int action, int mods) {
    if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
        glfwSetWindowShouldClose(window, GL_TRUE);
    }
}

// This function is called when a character key on the keyboard is pressed.
static void char_callback(GLFWwindow *window, unsigned int key) {
    /* Let only one of the number keys (0 - 9) be active */
    if (key >= (unsigned)'0' && key <= (unsigned)'9') {
        for (int i = 0; i < 10; ++i)
            keyToggles[(unsigned)'0' + i] = false;
    }
    keyToggles[key] = !keyToggles[key];
}

// This function is called when the mouse is clicked.
static void mouse_button_callback(GLFWwindow *window, int button, int action, int mods) {
    // Get the current mouse position.
    double xmouse, ymouse;
    glfwGetCursorPos(window, &xmouse, &ymouse);
    // Get current window size.
    int width, height;
    glfwGetWindowSize(window, &width, &height);
    if (action == GLFW_PRESS) {
        bool shift = mods & GLFW_MOD_SHIFT;
        bool ctrl = mods & GLFW_MOD_CONTROL;
        bool alt = mods & GLFW_MOD_ALT;
        camera->mouseClicked(xmouse, ymouse, shift, ctrl, alt);
    }
}

// This function is called when the mouse moves.
static void cursor_position_callback(GLFWwindow *window, double xmouse, double ymouse) {
    int state = glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT);
    if (state == GLFW_PRESS) {
        camera->mouseMoved(xmouse, ymouse);
    }
}

// If the window is resized, capture the new size and reset the viewport.
static void resize_callback(GLFWwindow *window, int width, int height) {
    glViewport(0, 0, width, height);
}

// This function is called once to initialize the scene and OpenGL.
static void init() {
    // Initialize time.
    glfwSetTime(0.0);

    // Set background color.
    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
    // Enable z-buffer test.
    glEnable(GL_DEPTH_TEST);

    // Program for Objects
    prog = make_shared<Program>();
    prog->setShaderNames(RESOURCE_DIR + "vert.glsl", RESOURCE_DIR + "frag.glsl");
    prog->setVerbose(true);
    prog->init();
    prog->addAttribute("aPos");
    prog->addAttribute("aNor");
    prog->addAttribute("aTex");
    prog->addUniform("MV");
    prog->addUniform("P");

    silProg = make_shared<Program>();
    silProg->setShaderNames(RESOURCE_DIR + "vert.glsl",
                            RESOURCE_DIR + "fragGrid.glsl");
    silProg->setVerbose(true);
    silProg->init();
    silProg->addAttribute("aPos");
    silProg->addAttribute("aNor");
    silProg->addAttribute("aTex");
    silProg->addUniform("MV");
    silProg->addUniform("P");
    silProg->addUniform("T");
    silProg->addUniform("texture");

    transProg = make_shared<Program>();
    transProg->setShaderNames(RESOURCE_DIR + "vert.glsl",
                              RESOURCE_DIR + "fragTrans.glsl");
    transProg->setVerbose(true);
    transProg->init();
    transProg->addAttribute("aPos");
    transProg->addAttribute("aNor");
    transProg->addAttribute("aTex");
    transProg->addUniform("MV");
    transProg->addUniform("P");

    // Set camera
    camera = make_shared<Camera>();

    // Load meshes
    bunny = make_shared<Shape>();
    bunny->loadMesh(RESOURCE_DIR + "bunny.obj");
    bunny->fitToUnitBox();
    bunny->init();

    sphere = make_shared<Shape>();
    sphere->loadMesh(RESOURCE_DIR + "sphere.obj");
    sphere->fitToUnitBox();
    sphere->init();

    teapot = make_shared<Shape>();
    teapot->loadMesh(RESOURCE_DIR + "teapot.obj");
    teapot->fitToUnitBox();
    teapot->init();

    // Texture
    gridTex = make_shared<Texture>();
    gridTex->setFilename(RESOURCE_DIR + "grid.jpg");
    gridTex->init();
    gridTex->setUnit(1);
    gridTex->setWrapModes(GL_REPEAT, GL_REPEAT);

    // Create our three world objects (including octrees)
    auto o = make_shared<WorldObject>(bunny, sphere, prog, silProg, transProg,
                                      Vector3f(-2.0f, -1.0f, 0.0f), // Position
                                      Vector3f(0.01f, 0.001f, 0.0f), // Velocity
                                      keyToggles);
    o->init();
    objs.push_back(o);

    o = make_shared<WorldObject>(teapot, sphere, prog, silProg, transProg,
                                 Vector3f(2.0f, -0.8f, 0.0f), // Position
                                 Vector3f(-0.01f, 0.0005f, 0.0f), // Velocity
                                 keyToggles);
    o->init();
    objs.push_back(o);

    o = make_shared<WorldObject>(bunny, sphere, prog, silProg, transProg,
                                 Vector3f(0.0f, 1.0f, 0.0f), // Position
                                 Vector3f(0.0f, -0.005f, 0.0f), // Velocity
                                 keyToggles);
    o->init();
    objs.push_back(o);

    GLSL::checkError(GET_FILE_LINE);
}

// This function is called every frame to draw the scene.
static void render() {
    // Clear framebuffer.
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    if (keyToggles[(unsigned)'c']) {
        glDisable(GL_CULL_FACE);
    } else {
        glEnable(GL_CULL_FACE);
    }
    if (keyToggles[(unsigned)'g']) {
        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    } else {
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    }

    // Get current frame buffer size.
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    camera->setAspect((float)width / (float)height);

    // Draw our objects
    silProg->bind();
    gridTex->bind(silProg->getUniform("texture"));
    silProg->unbind();
    for (auto it = objs.begin(); it != objs.end(); ++it)
        (*it)->draw(camera);

    GLSL::checkError(GET_FILE_LINE);
}

// Execute a movement step for the objects, if the user did not pause and there
// is no collision
void step() {
    // Move every object and check for collisions
    if (keyToggles[(unsigned)' '] && !collision) {
        for (size_t current = 0; current < objs.size(); ++current) {
            objs.at(current)->move();
            for (size_t next = current + 1; next < objs.size(); ++next)
                objs.at(current)->collisionDetection(objs.at(next));
        }

        collision = true;
        for (size_t current = 0; current < objs.size(); ++current)
            collision &= objs.at(current)->getColliding();
        keyToggles[(unsigned)' '] = !collision;
    }

    // If the current scene stopped, because every object was colliding, and the
    // user presses space again, then reset the scene.
    if (keyToggles[(unsigned)' '] && collision) {
        keyToggles[(unsigned)' '] = false;
        for (auto it = objs.begin(); it != objs.end(); ++it)
            (*it)->init();
        collision = false;
    }
}

int main(int argc, char **argv) {
    if (argc < 2) {
        cout << "Please specify the resource directory." << endl;
        return 0;
    }
    RESOURCE_DIR = argv[1] + string("/");

    // Set error callback.
    glfwSetErrorCallback(error_callback);
    // Initialize the library.
    if (!glfwInit()) {
        return -1;
    }
    // Create a windowed mode window and its OpenGL context.
    window = glfwCreateWindow(640, 480, "Sphere Octree", NULL, NULL);
    if (!window) {
        glfwTerminate();
        return -1;
    }
    // Make the window's context current.
    glfwMakeContextCurrent(window);
    // Initialize GLEW.
    glewExperimental = true;
    if (glewInit() != GLEW_OK) {
        cerr << "Failed to initialize GLEW" << endl;
        return -1;
    }
    glGetError(); // A bug in glewInit() causes an error that we can safely
    // ignore.
    cout << "OpenGL version: " << glGetString(GL_VERSION) << endl;
    cout << "GLSL version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << endl;
    GLSL::checkVersion();
    // Set vsync.
    glfwSwapInterval(1);
    // Set keyboard callback.
    glfwSetKeyCallback(window, key_callback);
    // Set char callback.
    glfwSetCharCallback(window, char_callback);
    // Set cursor position callback.
    glfwSetCursorPosCallback(window, cursor_position_callback);
    // Set mouse button callback.
    glfwSetMouseButtonCallback(window, mouse_button_callback);
    // Set the window resize call back.
    glfwSetFramebufferSizeCallback(window, resize_callback);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // Initialize scene.
    cout << "Creating scene and objects with sphere-octrees." << endl;
    init();
    // Loop until the user closes the window.
    while (!glfwWindowShouldClose(window)) {
        step();
        // Render scene.
        render();
        // Swap front and back buffers.
        glfwSwapBuffers(window);
        // Poll for and process events.
        glfwPollEvents();
    }
    // Quit program.
    glfwDestroyWindow(window);
    glfwTerminate();
    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 6> test_int_6_6(const Eigen::Matrix<int, 6, 6>& M) { return M; }
void export_int_6_6() { boost::python::def("test_int_6_6", test_int_6_6); }


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_SHR_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_SHR_HPP_INCLUDED

#include <boost/simd/bitwise/functions/shr.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/assert.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( shr_, tag::cpu_, (A0)(A1)(X)
                                   , ((simd_<arithmetic_<A0>,X>))
                                     (generic_< integer_<A1> >)
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type ntype;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shr: a shift is out of range");
      return bitwise_cast<result_type>( bitwise_cast<ntype>(a0) >> a1 );
    }
  };
} } }

#endif