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

// Copyright (c) 2009-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)

#define BOOST_GEOMETRY_REPORT_OVERLAY_ERROR
#define BOOST_GEOMETRY_NO_BOOST_TEST
#define BOOST_GEOMETRY_TIME_OVERLAY

#include <test_overlay_p_q.hpp>

#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/timer.hpp>

#include <star_comb.hpp>

template <typename Polygon>
void add(Polygon& polygon, double x, double y, int)
{
    typedef typename boost::geometry::point_type<Polygon>::type p;
    boost::geometry::exterior_ring(polygon).push_back(boost::geometry::make<p>(x, y));
}

template <typename Polygon>
void test_star_comb(int star_point_count, int comb_comb_count, double factor1, double factor2, bool do_union, p_q_settings const& settings)
{
    Polygon star, comb;
    make_star(star, add<Polygon>, star_point_count, factor1, factor2);
    make_comb(comb, add<Polygon>, comb_comb_count);

    std::ostringstream out;
    out << "star_comb";
    test_overlay_p_q
        <
            Polygon,
            typename bg::coordinate_type<Polygon>::type
        >(out.str(), star, comb, settings);
}

template <typename T, bool Clockwise, bool Closed>
void test_all(int count, int star_point_count, int comb_comb_count, double factor1, double factor2, bool do_union, p_q_settings const& settings)
{
    boost::timer t;

    typedef bg::model::polygon
        <
            bg::model::d2::point_xy<T>, Clockwise, Closed
        > polygon;

    int index = 0;
    for(int i = 0; i < count; i++)
    {
        test_star_comb<polygon>(star_point_count, comb_comb_count, factor1, factor2, do_union, settings);
    }
    std::cout
        << " type: " << string_from_type<T>::name()
        << " time: " << t.elapsed()  << std::endl;
}

int main(int argc, char** argv)
{
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== star_comb ===\nAllowed options");

        int count = 1;
        bool do_union = false;
        bool ccw = false;
        bool open = false;
        double factor1 = 1.1;
        double factor2 = 0.2;
        int point_count = 50;
        p_q_settings settings;

        description.add_options()
            ("help", "Help message")
            ("count", po::value<int>(&count)->default_value(1), "Number of tests")
            ("point_count", po::value<int>(&point_count)->default_value(1), "Number of points in the star")
            ("diff", po::value<bool>(&settings.also_difference)->default_value(false), "Include testing on difference")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help"))
        {
            std::cout << description << std::endl;
            return 1;
        }

        int star_point_count = point_count * 2 + 1;
        int comb_comb_count = point_count;

        if (ccw && open)
        {
            test_all<double, false, false>(count, star_point_count, comb_comb_count, factor1, factor2, do_union, settings);
        }
        else if (ccw)
        {
            test_all<double, false, true>(count, star_point_count, comb_comb_count, factor1, factor2, do_union, settings);
        }
        else if (open)
        {
            test_all<double, true, false>(count, star_point_count, comb_comb_count, factor1, factor2, do_union, settings);
        }
        else
        {
            test_all<double, true, true>(count, star_point_count, comb_comb_count, factor1, factor2, do_union, settings);
        }

#if defined(HAVE_TTMATH)
        // test_all<ttmath_big, true, true>(seed, count, max, svg, level);
#endif
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }

    return 0;
}


/*
 * NormalColorMapFilter.cpp
 *
 *  Created on: Aug 22, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_filters/NormalColorMapFilter.hpp>

#include <grid_map_core/grid_map_core.hpp>
#include <pluginlib/class_list_macros.h>

#include <Eigen/Dense>

using namespace filters;

namespace grid_map {

template<typename T>
NormalColorMapFilter<T>::NormalColorMapFilter()
{
}

template<typename T>
NormalColorMapFilter<T>::~NormalColorMapFilter()
{
}

template<typename T>
bool NormalColorMapFilter<T>::configure()
{
  if (!FilterBase < T > ::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) {
    ROS_ERROR("Normal color map filter did not find parameter `input_layers_prefix`.");
    return false;
  }
  ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str());

  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
    ROS_ERROR("Normal color map filter did not find parameter `output_layer`.");
    return false;
  }
  ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str());
  return true;
}

template<typename T>
bool NormalColorMapFilter<T>::update(const T& mapIn, T& mapOut)
{
  const auto& normalX = mapIn[inputLayersPrefix_ + "x"];
  const auto& normalY = mapIn[inputLayersPrefix_ + "y"];
  const auto& normalZ = mapIn[inputLayersPrefix_ + "z"];

  mapOut = mapIn;
  mapOut.add(outputLayer_);
  auto& color = mapOut[outputLayer_];

  // X: -1 to +1 : Red: 0 to 255
  // Y: -1 to +1 : Green: 0 to 255
  // Z:  0 to  1 : Blue: 128 to 255

  // For each cell in map.
  for (size_t i = 0; i < color.size(); ++i) {
    const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0,
                                      (normalY(i) + 1.0) / 2.0,
                                      (normalZ(i) / 2.0) + 0.5);
    colorVectorToValue(colorVector, color(i));
  }

  return true;
}

} /* namespace */

PLUGINLIB_EXPORT_CLASS(grid_map::NormalColorMapFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


// Copyright 2020 Google LLC
//
// 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.

#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <unsupported/Eigen/MatrixFunctions>
#include <assert.h>

#include <vector>
#define DCHECK_GT(a,b) assert((a)>(b))
#define DCHECK_EQ(a,b) assert((a)==(b))

#ifdef _WIN32
typedef __int64 qp_int64;
#else
typedef long long qp_int64;
#endif //_WIN32

using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::VectorXd;
#ifdef TDS_ENABLE_QPOASES
#include "qpOASES.hpp"
#include "qpOASES/Types.hpp"

using qpOASES::QProblem;

typedef Eigen::Matrix<qpOASES::real_t, Eigen::Dynamic, Eigen::Dynamic,
                      Eigen::RowMajor>
    RowMajorMatrixXd;

#endif //TDS_ENABLE_QPOASES

constexpr int k3Dim = 3;
constexpr double kGravity = 9.8;
constexpr double kMaxScale = 10;
constexpr double kMinScale = 0.1;

#include "Eigen/Core"
#include "Eigen/SparseCore"
#include "osqp/include/ctrlc.h"
#include "osqp/include/osqp.h"

enum QPSolverName
{
  OSQP, QPOASES
};

#ifdef TDS_ENABLE_QPOASES
// Auxiliary function for copying data to qpOASES data structure.
void CopyToVec(const Eigen::VectorXd& vec,
               const std::vector<int> foot_contact_states, int num_legs,
               int planning_horizon, int blk_size,
               std::vector<qpOASES::real_t>* out) {
  int buffer_index = 0;
  for (int i = 0; i < num_legs * planning_horizon; ++i) {
    int leg_id = (i % num_legs);
    if (foot_contact_states[leg_id] == 0) {
      // skip the block.
      continue;
    }
    // otherwise copy this block.
    assert(buffer_index < out->size());
    for (int j = 0; j < blk_size; ++j) {
      int index = i * blk_size + j;
      (*out)[buffer_index] = vec[index];
      ++buffer_index;
    }
  }
}

// Auxiliary function for copying data to qpOASES data structure.
void CopyToMatrix(const Eigen::MatrixXd& input,
                  const std::vector<int> foot_contact_states, int num_legs,
                  int planning_horizon, int row_blk_size, int col_blk_size,
                  bool is_block_diagonal, Eigen::Map<RowMajorMatrixXd>* out) {
  // the block index in the destination matrix.
  int row_blk = 0;
  for (int i = 0; i < planning_horizon * num_legs; ++i) {
    int leg_id = (i % num_legs);
    if (foot_contact_states[leg_id] == 0) {
      // skip the row block.
      continue;
    }
    if (is_block_diagonal) {
      // just copy the block
      int col_blk = row_blk;
      out->block(row_blk * row_blk_size, col_blk * col_blk_size, row_blk_size,
                 col_blk_size) = input.block(i * row_blk_size, i * col_blk_size,
                                             row_blk_size, col_blk_size);
    } else {
      int col_blk = 0;
      // Non-diagonal, need to copy all elements.
      for (int j = 0; j < planning_horizon * num_legs; ++j) {
        int leg_id = (j % num_legs);
        if (foot_contact_states[leg_id] == 0) {
          // skip the col block.
          continue;
        }
        out->block(row_blk * row_blk_size, col_blk * col_blk_size, row_blk_size,
                   col_blk_size) =
            input.block(i * row_blk_size, j * col_blk_size, row_blk_size,
                        col_blk_size);
        ++col_blk;
      }
    }
    ++row_blk;
  }
}
#endif //TDS_ENABLE_QPOASES

// Converts the roll pitchh yaw angle vector to the corresponding rotation
// matrix.
Eigen::Matrix3d ConvertRpyToRot(const Eigen::Vector3d& rpy);

// Converts a vector to the skew symmetric matrix form. For an input vector
// [a, b, c], the output matrix would be:
//   [ 0, -c,  b]
//   [ c,  0, -a]
//   [-b,  a,  0]
Eigen::Matrix3d ConvertToSkewSymmetric(const Eigen::Vector3d& vec);

// The CoM dynamics can be written as:
//   X_dot = A X + B u
// where X is the 13-dimensional state vector (r, p, y, x, y, z, r_dot, p_dot,
// y_dot, vx, vy, vz, -g) constructed from the CoM roll/pitch/yaw/position, and
// their first order derivatives. 'g' is the gravity constant. This API
// constructs the A matrix in the formula. Check the MIT paper for details of
// the formulation.
void CalculateAMat(const Eigen::Vector3d& rpy, Eigen::MatrixXd* a_mat_ptr);

// Constructs the B matrix in the linearized CoM dynamics equation. See the
// documentation for 'CalculateAMat' for details of the symbol.
void CalculateBMat(double inv_mass, const Eigen::Matrix3d& inv_inertia,
    const Eigen::MatrixXd& foot_positions,
    Eigen::MatrixXd* b_mat_ptr);

// Calculates the discretized space-time dynamics. Given the dynamics equation:
//   X_dot = A X + B u
// and a timestep dt, we can estimate the snapshot of the state at t + dt by:
//   X[t + dt] = exp([A, B]dt) [X, u] = A_exp X + B_exp u
void CalculateExponentials(const Eigen::MatrixXd& a_mat,
    const Eigen::MatrixXd& b_mat, double timestep,
    Eigen::MatrixXd* ab_mat_ptr,
    Eigen::MatrixXd* a_exp_ptr,
    Eigen::MatrixXd* b_exp_ptr);

// Calculates the dense QP formulation of the discretized space time dynamics.
// Given:
//   X_k+1 = A_exp X_k + B_exp u_k
// We can unroll the dynamics in time using a forward pass:
//   [X_1, X_2,..., X_k+1] = A_qp X_0 + B_qp [u_0, u_1,..., u_k]
void CalculateQpMats(const Eigen::MatrixXd& a_exp, const Eigen::MatrixXd& b_exp,
    int horizon, Eigen::MatrixXd* a_qp_ptr,
    Eigen::MatrixXd* b_qp_ptr);

void UpdateConstraintsMatrix(std::vector<double>& friction_coeff,
    int horizon, int num_legs,
    Eigen::MatrixXd* constraint_ptr);

void CalculateConstraintBounds(const Eigen::MatrixXd& contact_state, double fz_max,
    double fz_min, double friction_coeff, int horizon,
    Eigen::VectorXd* constraint_lb_ptr,
    Eigen::VectorXd* constraint_ub_ptr);

double EstimateCoMHeightSimple(const Eigen::MatrixXd& foot_positions_world,
    const std::vector<bool> foot_contact_states);

// The MIT convex mpc implementation as described in this paper:
//   https://ieeexplore.ieee.org/document/8594448/
// Computes the optimal feet contact forces given a desired center of mass
// trajectory and gait pattern.
class ConvexMpc {
public:
    static constexpr int kStateDim =
        13;  // 6 dof pose + 6 dof velocity + 1 gravity.

    // For each foot contact force we use 4-dim cone approximation + 1 for z.
    static constexpr int kConstraintDim = 5;

    ConvexMpc(double mass, const std::vector<double>& inertia, int num_legs,
        int planning_horizon, double timestep,
        const std::vector<double>& qp_weights, double alpha = 1e-5,
#ifdef TDS_ENABLE_QPOASES
        QPSolverName qp_solver_name=QPOASES
#else
        QPSolverName qp_solver_name=OSQP
#endif
    );

    virtual ~ConvexMpc()
    {
        osqp_cleanup(workspace_);
    }
    // If not explicitly specified, we assume the quantities are measured in a
    // world frame. Usually we choose the yaw-aligned horizontal frame i.e. an
    // instanteneous world frame at the time of planning with its origin at CoM
    // and z axis aligned with gravity. The yaw-alignment means that the CoM
    // rotation measured in this frame has zero yaw component. Caveat: We expect
    // the input euler angle roll_pitch_yaw to be in ZYX format, i.e. the rotation
    // order is X -> Y -> Z, with respect to the extrinsic (fixed) coordinate
    // frame. In the intrinsic (body-attached) frame the rotation order is Z -> Y'
    // -> X".
    std::vector<double> ComputeContactForces(
        std::vector<double> com_position,
        std::vector<double> com_velocity,
        std::vector<double> com_roll_pitch_yaw,
        std::vector<double> com_angular_velocity,
        std::vector<int> foot_contact_states,
        std::vector<double> foot_positions_body_frame,
        std::vector<double> foot_friction_coeffs,
        std::vector<double> desired_com_position,
        std::vector<double> desired_com_velocity,
        std::vector<double> desired_com_roll_pitch_yaw,
        std::vector<double> desired_com_angular_velocity);

    // Reset the solver so that for the next optimization run the solver is
    // re-initialized.
    void ResetSolver();

private:
    const double mass_;
    const double inv_mass_;
    const Eigen::Matrix3d inertia_;
    const Eigen::Matrix3d inv_inertia_;
    const int num_legs_;
    const int planning_horizon_;
    const double timestep_;
    QPSolverName qp_solver_name_;

    // 13 * horizon diagonal matrix.
    const Eigen::MatrixXd qp_weights_;

    // 13 x 13 diagonal matrix.
    const Eigen::MatrixXd qp_weights_single_;

    // num_legs * 3 * horizon diagonal matrix.
    const Eigen::MatrixXd alpha_;
    const Eigen::MatrixXd alpha_single_;
    const int action_dim_;

    // The following matrices will be updated for every call. However, their sizes
    // can be determined at class initialization time.
    Eigen::VectorXd state_;                 // 13
    Eigen::VectorXd desired_states_;        // 13 * horizon
    Eigen::MatrixXd contact_states_;        // horizon x num_legs
    Eigen::MatrixXd foot_positions_base_;   // num_legs x 3
    Eigen::MatrixXd foot_positions_world_;  // num_legs x 3
    Eigen::VectorXd foot_friction_coeff_;   // num_legs
    Eigen::Matrix3d rotation_;
    Eigen::Matrix3d inertia_world_;    // rotation x inertia x rotation_transpose
    Eigen::MatrixXd a_mat_;            // 13 x 13
    Eigen::MatrixXd b_mat_;            // 13 x (num_legs * 3)
    Eigen::MatrixXd ab_concatenated_;  // 13 + num_legs * 3 x 13 + num_legs * 3
    Eigen::MatrixXd a_exp_;            // same dimension as a_mat_
    Eigen::MatrixXd b_exp_;            // same dimension as b_mat_

    // Contains all the power mats of a_exp_. Consider Eigen::SparseMatrix.
    Eigen::MatrixXd a_qp_;  // 13 * horizon x 13
    Eigen::MatrixXd b_qp_;  // 13 * horizon x num_legs * 3 * horizon sparse
    Eigen::MatrixXd b_qp_transpose_;
    Eigen::MatrixXd p_mat_;  // num_legs * 3 * horizon x num_legs * 3 * horizon
    Eigen::VectorXd q_vec_;  // num_legs * 3 * horizon vector

    // Auxiliary containing A^n*B, with n in [0, num_legs * 3)
    Eigen::MatrixXd anb_aux_;  // 13 * horizon x (num_legs * 3)

    // Contains the constraint matrix and bounds.
    Eigen::MatrixXd
        constraint_;  // 5 * num_legs * horizon x 3 * num_legs * horizon
    Eigen::VectorXd constraint_lb_;  // 5 * num_legs * horizon
    Eigen::VectorXd constraint_ub_;  // 5 * num_legs * horizon

    std::vector<double> qp_solution_;

    ::OSQPWorkspace* workspace_;
    // Whether optimizing for the first step
    bool initial_run_;
};

constexpr int ConvexMpc::kStateDim;

Matrix3d ConvertRpyToRot(const Vector3d& rpy) {
    assert(rpy.size() == k3Dim);
    const AngleAxisd roll(rpy[0], Vector3d::UnitX());
    const AngleAxisd pitch(rpy[1], Vector3d::UnitY());
    const AngleAxisd yaw(rpy[2], Vector3d::UnitZ());
    Quaterniond q = yaw * pitch * roll;

    return q.matrix();
}

Matrix3d ConvertToSkewSymmetric(const Vector3d& vec) {
    Matrix3d skew_symm;
    skew_symm << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
    return skew_symm;
}

void CalculateAMat(const Vector3d& rpy, MatrixXd* a_mat_ptr) {
    // The transformation of angular velocity to roll pitch yaw rate. Caveat:
    // rpy rate is not a proper vector and does not follow the common vector
    // transformation dicted by the rotation matrix. Here we assume the input
    // rotation is in X->Y->Z order in the extrinsic/fixed frame, or z->y'->x''
    // order in the intrinsic frame.
    const double cos_yaw = cos(rpy[2]);
    const double sin_yaw = sin(rpy[2]);
    const double cos_pitch = cos(rpy[1]);
    const double tan_pitch = tan(rpy[1]);
    Matrix3d angular_velocity_to_rpy_rate;
    angular_velocity_to_rpy_rate << cos_yaw / cos_pitch, sin_yaw / cos_pitch, 0,
        -sin_yaw, cos_yaw, 0, cos_yaw* tan_pitch, sin_yaw* tan_pitch, 1;

    MatrixXd& a_mat = *a_mat_ptr;
    a_mat.block<3, 3>(0, 6) = angular_velocity_to_rpy_rate;
    a_mat(3, 9) = 1;
    a_mat(4, 10) = 1;
    a_mat(5, 11) = 1;
    a_mat(11, 12) = 1;
}

void CalculateBMat(double inv_mass, const Matrix3d& inv_inertia,
    const MatrixXd& foot_positions, MatrixXd* b_mat_ptr) {
    // b_mat contains non_zero elements only in row 6:12.
    const int num_legs = foot_positions.rows();
    MatrixXd& b_mat = *b_mat_ptr;
    for (int i = 0; i < num_legs; ++i) {
        b_mat.block<k3Dim, k3Dim>(6, i * k3Dim) =
            inv_inertia * ConvertToSkewSymmetric(foot_positions.row(i));
        b_mat(9, i * k3Dim) = inv_mass;
        b_mat(10, i * k3Dim + 1) = inv_mass;
        b_mat(11, i * k3Dim + 2) = inv_mass;
    }
}

void CalculateExponentials(const MatrixXd& a_mat, const MatrixXd& b_mat,
    double timestep, MatrixXd* ab_mat_ptr,
    MatrixXd* a_exp_ptr, MatrixXd* b_exp_ptr) {
    const int state_dim = ConvexMpc::kStateDim;
    MatrixXd& ab_mat = *ab_mat_ptr;
    ab_mat.block<state_dim, state_dim>(0, 0) = a_mat * timestep;
    const int action_dim = b_mat.cols();
    ab_mat.block(0, state_dim, state_dim, action_dim) = b_mat * timestep;

    // This temporary is inevitable.
    MatrixXd ab_exp = ab_mat.exp();
    *a_exp_ptr = ab_exp.block<state_dim, state_dim>(0, 0);
    *b_exp_ptr = ab_exp.block(0, state_dim, state_dim, action_dim);
}

void CalculateQpMats(const MatrixXd& a_exp, const MatrixXd& b_exp,
    const MatrixXd& qp_weights_single,
    const MatrixXd& alpha_single, int horizon,
    MatrixXd* a_qp_ptr, MatrixXd* anb_aux_ptr,
    MatrixXd* b_qp_ptr, MatrixXd* p_mat_ptr) {
    const int state_dim = ConvexMpc::kStateDim;
    MatrixXd& a_qp = *a_qp_ptr;
    a_qp.block(0, 0, state_dim, state_dim) = a_exp;
    for (int i = 1; i < horizon - 1; ++i) {
        a_qp.block<state_dim, state_dim>(i * state_dim, 0) =
            a_exp * a_qp.block<state_dim, state_dim>((i - 1) * state_dim, 0);
    }

    const int action_dim = b_exp.cols();

    MatrixXd& anb_aux = *anb_aux_ptr;
    anb_aux.block(0, 0, state_dim, action_dim) = b_exp;
    for (int i = 1; i < horizon; ++i) {
        anb_aux.block(i * state_dim, 0, state_dim, action_dim) =
            a_exp * anb_aux.block((i - 1) * state_dim, 0, state_dim, action_dim);
    }

    MatrixXd& b_qp = *b_qp_ptr;
    for (int i = 0; i < horizon; ++i) {
        // Diagonal block.
        b_qp.block(i * state_dim, i * action_dim, state_dim, action_dim) = b_exp;
        // Off diagonal Diagonal block = A^(i - j - 1) * B_exp.
        for (int j = 0; j < i; ++j) {
            const int power = i - j;
            b_qp.block(i * state_dim, j * action_dim, state_dim, action_dim) =
                anb_aux.block(power * state_dim, 0, state_dim, action_dim);
        }
    }

    MatrixXd& p_mat = *p_mat_ptr;
    for (int i = horizon - 1; i >= 0; --i) {
        p_mat.block(i * action_dim, (horizon - 1) * action_dim, action_dim,
            action_dim) =
            anb_aux.block((horizon - i - 1) * state_dim, 0, state_dim, action_dim)
            .transpose() *
            qp_weights_single * b_exp;
        if (i != horizon - 1) {
            p_mat.block((horizon - 1) * action_dim, i * action_dim, action_dim,
                action_dim) =
                p_mat
                .block(i * action_dim, (horizon - 1) * action_dim, action_dim,
                    action_dim)
                .transpose();
        }
    }

    for (int i = horizon - 2; i >= 0; --i) {
        // Diagonal block.
        p_mat.block(i * action_dim, i * action_dim, action_dim, action_dim) =
            p_mat.block((i + 1) * action_dim, (i + 1) * action_dim, action_dim,
                action_dim) +
            anb_aux.block((horizon - i - 1) * state_dim, 0, state_dim, action_dim)
            .transpose() *
            qp_weights_single *
            anb_aux.block((horizon - i - 1) * state_dim, 0, state_dim,
                action_dim);
        // Off diagonal block
        for (int j = i + 1; j < horizon - 1; ++j) {
            p_mat.block(i * action_dim, j * action_dim, action_dim, action_dim) =
                p_mat.block((i + 1) * action_dim, (j + 1) * action_dim, action_dim,
                    action_dim) +
                anb_aux.block((horizon - i - 1) * state_dim, 0, state_dim, action_dim)
                .transpose() *
                qp_weights_single *
                anb_aux.block((horizon - j - 1) * state_dim, 0, state_dim,
                    action_dim);
            p_mat.block(j * action_dim, i * action_dim, action_dim, action_dim) =
                p_mat.block(i * action_dim, j * action_dim, action_dim, action_dim)
                .transpose();
        }
    }

    p_mat *= 2.0;
    for (int i = 0; i < horizon; ++i) {
        p_mat.block(i * action_dim, i * action_dim, action_dim, action_dim) +=
            alpha_single;
    }
}

void UpdateConstraintsMatrix(std::vector<double>& friction_coeff,
    int horizon, int num_legs,
    MatrixXd* constraint_ptr) {
    const int constraint_dim = ConvexMpc::kConstraintDim;
    MatrixXd& constraint = *constraint_ptr;
    for (int i = 0; i < horizon * num_legs; ++i) {
        constraint.block<constraint_dim, k3Dim>(i * constraint_dim, i * k3Dim)
            << -1,
            0, friction_coeff[0], 1, 0, friction_coeff[1], 0, -1, friction_coeff[2],
            0, 1, friction_coeff[3], 0, 0, 1;
    }
}

void CalculateConstraintBounds(const MatrixXd& contact_state, double fz_max,
    double fz_min, double friction_coeff,
    int horizon, VectorXd* constraint_lb_ptr,
    VectorXd* constraint_ub_ptr) {
    const int constraint_dim = ConvexMpc::kConstraintDim;

    const int num_legs = contact_state.cols();

    VectorXd& constraint_lb = *constraint_lb_ptr;
    VectorXd& constraint_ub = *constraint_ub_ptr;
    for (int i = 0; i < horizon; ++i) {
        for (int j = 0; j < num_legs; ++j) {
            const int row = (i * num_legs + j) * constraint_dim;
            constraint_lb(row) = 0;
            constraint_lb(row + 1) = 0;
            constraint_lb(row + 2) = 0;
            constraint_lb(row + 3) = 0;
            constraint_lb(row + 4) = fz_min * contact_state(i, j);

            const double friction_ub =
                (friction_coeff + 1) * fz_max * contact_state(i, j);
            constraint_ub(row) = friction_ub;
            constraint_ub(row + 1) = friction_ub;
            constraint_ub(row + 2) = friction_ub;
            constraint_ub(row + 3) = friction_ub;
            constraint_ub(row + 4) = fz_max * contact_state(i, j);
        }
    }
}

double EstimateCoMHeightSimple(
    const MatrixXd& foot_positions_world,
    const std::vector<int> foot_contact_states) {
    int legs_in_contact = 0;
    double com_height = 0;
    const int z_dim = 2;
    for (int i = 0; i < foot_contact_states.size(); ++i) {
        if (foot_contact_states[i]) {
            com_height += foot_positions_world(i, z_dim);
            legs_in_contact += 1;
        }
    }

    // We don't support jumping in air for now.
    DCHECK_GT(legs_in_contact, 0);
    return abs(com_height / legs_in_contact);
}

MatrixXd AsBlockDiagonalMat(const std::vector<double>& qp_weights,
    int planning_horizon) {
    const Eigen::Map<const VectorXd> qp_weights_vec(qp_weights.data(),
        qp_weights.size());
    // Directly return the rhs will cause a TSAN failure, probably due to the
    // asDiagonal not reall copying the memory. Creates the temporary will ensure
    // copy on return.
    const MatrixXd qp_weights_mat =
        qp_weights_vec.replicate(planning_horizon, 1).asDiagonal();
    return qp_weights_mat;
}

ConvexMpc::ConvexMpc(double mass, const std::vector<double>& inertia,
    int num_legs, int planning_horizon, double timestep,
    const std::vector<double>& qp_weights, double alpha,
      QPSolverName qp_solver_name)
    : mass_(mass),
    inv_mass_(1 / mass),
    inertia_(inertia.data()),
    inv_inertia_(inertia_.inverse()),
    num_legs_(num_legs),
    planning_horizon_(planning_horizon),
    timestep_(timestep),
    qp_solver_name_(qp_solver_name),
    qp_weights_(AsBlockDiagonalMat(qp_weights, planning_horizon)),
    qp_weights_single_(AsBlockDiagonalMat(qp_weights, 1)),
    alpha_(alpha* MatrixXd::Identity(num_legs* planning_horizon* k3Dim,
        num_legs* planning_horizon* k3Dim)),
    alpha_single_(alpha*
        MatrixXd::Identity(num_legs* k3Dim, num_legs* k3Dim)),
    action_dim_(num_legs* k3Dim),
    state_(kStateDim),
    desired_states_(kStateDim* planning_horizon),
    contact_states_(planning_horizon, num_legs),
    foot_positions_base_(num_legs, k3Dim),
    foot_positions_world_(num_legs, k3Dim),
    foot_friction_coeff_(num_legs_),
    a_mat_(kStateDim, kStateDim),
    b_mat_(kStateDim, action_dim_),
    ab_concatenated_(kStateDim + action_dim_, kStateDim + action_dim_),
    a_exp_(kStateDim, kStateDim),
    b_exp_(kStateDim, action_dim_),
    a_qp_(kStateDim* planning_horizon, kStateDim),
    b_qp_(kStateDim* planning_horizon, action_dim_* planning_horizon),
    p_mat_(num_legs* planning_horizon* k3Dim,
        num_legs* planning_horizon* k3Dim),
    q_vec_(num_legs* planning_horizon* k3Dim),
    anb_aux_(kStateDim* planning_horizon, action_dim_),
    constraint_(kConstraintDim* num_legs* planning_horizon,
        action_dim_* planning_horizon),
    constraint_lb_(kConstraintDim* num_legs* planning_horizon),
    constraint_ub_(kConstraintDim* num_legs* planning_horizon),
    qp_solution_(k3Dim* num_legs* planning_horizon),
    workspace_(0),
    initial_run_(true)

{
    assert(qp_weights.size() == kStateDim);
    // We assume the input inertia is a 3x3 matrix.
    assert(inertia.size() == k3Dim * k3Dim);
    state_.setZero();
    desired_states_.setZero();
    contact_states_.setZero();
    foot_positions_base_.setZero();
    foot_positions_world_.setZero();
    foot_friction_coeff_.setZero();
    a_mat_.setZero();
    b_mat_.setZero();
    ab_concatenated_.setZero();
    a_exp_.setZero();
    b_exp_.setZero();
    a_qp_.setZero();
    b_qp_.setZero();
    b_qp_transpose_.setZero();
    constraint_.setZero();
    constraint_lb_.setZero();
    constraint_ub_.setZero();
}

void ConvexMpc::ResetSolver() { initial_run_ = true; }

std::vector<double> ConvexMpc::ComputeContactForces(
    std::vector<double> com_position,
    std::vector<double> com_velocity,
    std::vector<double> com_roll_pitch_yaw,
    std::vector<double> com_angular_velocity,
    std::vector<int> foot_contact_states,
    std::vector<double> foot_positions_body_frame,
    std::vector<double> foot_friction_coeffs,
    std::vector<double> desired_com_position,
    std::vector<double> desired_com_velocity,
    std::vector<double> desired_com_roll_pitch_yaw,
    std::vector<double> desired_com_angular_velocity) {

    std::vector<double> error_result;

    // First we compute the foot positions in the world frame.
    DCHECK_EQ(com_roll_pitch_yaw.size(), k3Dim);
    const Quaterniond com_rotation =
        AngleAxisd(com_roll_pitch_yaw[0], Vector3d::UnitX()) *
        AngleAxisd(com_roll_pitch_yaw[1], Vector3d::UnitY()) *
        AngleAxisd(com_roll_pitch_yaw[2], Vector3d::UnitZ());

    DCHECK_EQ(foot_positions_body_frame.size(), k3Dim * num_legs_);
    foot_positions_base_ = Eigen::Map<const MatrixXd>(
        foot_positions_body_frame.data(), k3Dim, num_legs_)
        .transpose();
    for (int i = 0; i < num_legs_; ++i) {
        foot_positions_world_.row(i) = com_rotation * foot_positions_base_.row(i);
    }

    // Now we can estimate the body height using the world frame foot positions
    // and contact states. We use simple averges leg height here.
    DCHECK_EQ(foot_contact_states.size(), num_legs_);
    const double com_z =
        com_position.size() == k3Dim
        ? com_position[2]
        : EstimateCoMHeightSimple(foot_positions_world_, foot_contact_states);

    // In MPC planning we don't care about absolute position in the horizontal
    // plane.
    const double com_x = 0;
    const double com_y = 0;

    // Prepare the current and desired state vectors of length kStateDim *
    // planning_horizon.
    DCHECK_EQ(com_velocity.size(), k3Dim);
    DCHECK_EQ(com_angular_velocity.size(), k3Dim);
    state_ << com_roll_pitch_yaw[0], com_roll_pitch_yaw[1], com_roll_pitch_yaw[2],
        com_x, com_y, com_z, com_angular_velocity[0], com_angular_velocity[1],
        com_angular_velocity[2], com_velocity[0], com_velocity[1],
        com_velocity[2], -kGravity;

    for (int i = 0; i < planning_horizon_; ++i) {
        desired_states_[i * kStateDim + 0] = desired_com_roll_pitch_yaw[0];
        desired_states_[i * kStateDim + 1] = desired_com_roll_pitch_yaw[1];
        desired_states_[i * kStateDim + 2] =
            com_roll_pitch_yaw[2] +
            timestep_ * (i + 1) * desired_com_angular_velocity[2];

        desired_states_[i * kStateDim + 3] =
            timestep_ * (i + 1) * desired_com_velocity[0];
        desired_states_[i * kStateDim + 4] =
            timestep_ * (i + 1) * desired_com_velocity[1];
        desired_states_[i * kStateDim + 5] = desired_com_position[2];

        // Prefer to stablize roll and pitch.
        desired_states_[i * kStateDim + 6] = 0;
        desired_states_[i * kStateDim + 7] = 0;
        desired_states_[i * kStateDim + 8] = desired_com_angular_velocity[2];

        desired_states_[i * kStateDim + 9] = desired_com_velocity[0];
        desired_states_[i * kStateDim + 10] = desired_com_velocity[1];
        // Prefer to stablize the body height.
        desired_states_[i * kStateDim + 11] = 0;

        desired_states_[i * kStateDim + 12] = -kGravity;
    }

    const Vector3d rpy(com_roll_pitch_yaw[0], com_roll_pitch_yaw[1],
        com_roll_pitch_yaw[2]);

    CalculateAMat(rpy, &a_mat_);

    rotation_ = ConvertRpyToRot(rpy);
    const Matrix3d inv_inertia_world =
        rotation_ * inv_inertia_ * rotation_.transpose();

    CalculateBMat(inv_mass_, inv_inertia_world, foot_positions_world_, &b_mat_);

    CalculateExponentials(a_mat_, b_mat_, timestep_, &ab_concatenated_, &a_exp_,
        &b_exp_);

    CalculateQpMats(a_exp_, b_exp_, qp_weights_single_, alpha_single_,
        planning_horizon_, &a_qp_, &anb_aux_, &b_qp_, &p_mat_);

    const MatrixXd state_diff = a_qp_ * state_ - desired_states_;

    q_vec_ = 2 * b_qp_.transpose() * (qp_weights_ * state_diff);

    const VectorXd one_vec = VectorXd::Constant(planning_horizon_, 1.0);
    const VectorXd zero_vec = VectorXd::Zero(planning_horizon_);
    for (int j = 0; j < foot_contact_states.size(); ++j) {
        if (foot_contact_states[j]) {
            contact_states_.col(j) = one_vec;
        }
        else {
            contact_states_.col(j) = zero_vec;
        }
    }

    CalculateConstraintBounds(contact_states_, mass_ * kGravity * kMaxScale,
        mass_ * kGravity * kMinScale,
        foot_friction_coeffs[0], planning_horizon_,
        &constraint_lb_, &constraint_ub_);

    if (qp_solver_name_ == OSQP)
    {
      UpdateConstraintsMatrix(foot_friction_coeffs, planning_horizon_, num_legs_,
          &constraint_);
      foot_friction_coeff_ << foot_friction_coeffs[0], foot_friction_coeffs[1],
          foot_friction_coeffs[2], foot_friction_coeffs[3];

      Eigen::SparseMatrix<double, Eigen::ColMajor, qp_int64> objective_matrix = p_mat_.sparseView();
      Eigen::VectorXd objective_vector = q_vec_;
      Eigen::SparseMatrix<double, Eigen::ColMajor, qp_int64> constraint_matrix = constraint_.sparseView();

      int num_variables = constraint_.cols();
      int num_constraints = constraint_.rows();

      ::OSQPSettings settings;
      osqp_set_default_settings(&settings);
      settings.verbose = false;
      settings.warm_start = true;
      settings.polish = true;
      settings.adaptive_rho_interval = 25;
      settings.eps_abs = 1e-3;
      settings.eps_rel = 1e-3;

      assert(p_mat_.cols()== num_variables);
      assert(p_mat_.rows()== num_variables);
      assert(q_vec_.size()== num_variables);
      assert(constraint_lb_.size() == num_constraints);
      assert(constraint_ub_.size() == num_constraints);

      VectorXd clipped_lower_bounds = constraint_lb_.cwiseMax(-OSQP_INFTY);
      VectorXd clipped_upper_bounds = constraint_ub_.cwiseMin(OSQP_INFTY);

      ::OSQPData data;
      data.n = num_variables;
      data.m = num_constraints;

      Eigen::SparseMatrix<double, Eigen::ColMajor, qp_int64>
          objective_matrix_upper_triangle =
          objective_matrix.triangularView<Eigen::Upper>();

      ::csc osqp_objective_matrix = {
          objective_matrix_upper_triangle.outerIndexPtr()[num_variables],
          num_variables,
          num_variables,
          const_cast<qp_int64*>(objective_matrix_upper_triangle.outerIndexPtr()),
          const_cast<qp_int64*>(objective_matrix_upper_triangle.innerIndexPtr()),
          const_cast<double*>(objective_matrix_upper_triangle.valuePtr()),
          -1 };
      data.P = &osqp_objective_matrix;

      ::csc osqp_constraint_matrix = {
          constraint_matrix.outerIndexPtr()[num_variables],
          num_constraints,
          num_variables,
          const_cast<qp_int64*>(constraint_matrix.outerIndexPtr()),
          const_cast<qp_int64*>(constraint_matrix.innerIndexPtr()),
          const_cast<double*>(constraint_matrix.valuePtr()),
          -1 };
      data.A = &osqp_constraint_matrix;

      data.q = const_cast<double*>(objective_vector.data());
      data.l = clipped_lower_bounds.data();
      data.u = clipped_upper_bounds.data();

      const int return_code = 0;

      if (workspace_==0) {
          osqp_setup(&workspace_, &data, &settings);
          initial_run_ = false;
      }
      else {

          UpdateConstraintsMatrix(foot_friction_coeffs, planning_horizon_,
              num_legs_, &constraint_);
          foot_friction_coeff_ << foot_friction_coeffs[0], foot_friction_coeffs[1],
              foot_friction_coeffs[2], foot_friction_coeffs[3];

          c_int nnzP = objective_matrix_upper_triangle.nonZeros();

          c_int nnzA = constraint_matrix.nonZeros();

          int return_code = osqp_update_P_A(
              workspace_, objective_matrix_upper_triangle.valuePtr(), OSQP_NULL, nnzP,
              constraint_matrix.valuePtr(), OSQP_NULL, nnzA);

          return_code =
              osqp_update_lin_cost(workspace_, objective_vector.data());

          return_code = osqp_update_bounds(
              workspace_, clipped_lower_bounds.data(), clipped_upper_bounds.data());
      }

      if (osqp_solve(workspace_) != 0) {
          if (osqp_is_interrupted()) {
              return error_result;
          }
      }

      Map<VectorXd> solution(qp_solution_.data(), qp_solution_.size());

      if (workspace_->info->status_val== OSQP_SOLVED) {
          solution = -Map<const VectorXd>(workspace_->solution->x, workspace_->data->n);
      }
      else {
          //LOG(WARNING) << "QP does not converge";
          return error_result;
      }

      return qp_solution_;
    }
    else
    {
#ifdef TDS_ENABLE_QPOASES
      // Solve the QP Problem using qpOASES
    UpdateConstraintsMatrix(foot_friction_coeffs, planning_horizon_, num_legs_,
                            &constraint_);

    // To use qpOASES, we need to eleminate the zero rows/cols from the
    // matrices when copy to qpOASES buffer
    int num_legs_in_contact = 0;
    for (int i = 0; i < foot_contact_states.size(); ++i) {
      if (foot_contact_states[i]) {
        num_legs_in_contact += 1;
      }
    }

    const int qp_dim = num_legs_in_contact * k3Dim * planning_horizon_;
    const int constraint_dim = num_legs_in_contact * 5 * planning_horizon_;
    std::vector<qpOASES::real_t> hessian(qp_dim * qp_dim, 0);
    Map<RowMajorMatrixXd> hessian_mat_view(hessian.data(), qp_dim, qp_dim);
    // Copy to the hessian
    CopyToMatrix(p_mat_, foot_contact_states, num_legs_, planning_horizon_,
                 k3Dim, k3Dim, false, &hessian_mat_view);

    std::vector<qpOASES::real_t> g_vec(qp_dim, 0);
    // Copy the g_vec
    CopyToVec(q_vec_, foot_contact_states, num_legs_, planning_horizon_, k3Dim,
              &g_vec);

    std::vector<qpOASES::real_t> a_mat(qp_dim * constraint_dim, 0);
    Map<RowMajorMatrixXd> a_mat_view(a_mat.data(), constraint_dim, qp_dim);
    CopyToMatrix(constraint_, foot_contact_states, num_legs_, planning_horizon_,
                 5, k3Dim, true, &a_mat_view);

    std::vector<qpOASES::real_t> a_lb(constraint_dim, 0);
    CopyToVec(constraint_lb_, foot_contact_states, num_legs_, planning_horizon_,
              5, &a_lb);

    std::vector<qpOASES::real_t> a_ub(constraint_dim, 0);
    CopyToVec(constraint_ub_, foot_contact_states, num_legs_, planning_horizon_,
              5, &a_ub);

    auto qp_problem = QProblem(qp_dim, constraint_dim, qpOASES::HST_UNKNOWN,
                               qpOASES::BT_TRUE);

    qpOASES::Options options;
    options.setToMPC();
    options.printLevel = qpOASES::PL_NONE;
    qp_problem.setOptions(options);

    int max_solver_iter = 100;

    qp_problem.init(hessian.data(), g_vec.data(), a_mat.data(), nullptr,
                    nullptr, a_lb.data(), a_ub.data(), max_solver_iter,
                    nullptr);

    std::vector<qpOASES::real_t> qp_sol(qp_dim, 0);
    qp_problem.getPrimalSolution(qp_sol.data());
    for (auto& force : qp_sol) {
      force = -force;
    }
    Map<VectorXd> qp_sol_vec(qp_sol.data(), qp_sol.size());

    int buffer_index = 0;
    for (int i = 0; i < num_legs_ * planning_horizon_; ++i) {
      int leg_id = i % num_legs_;
      if (foot_contact_states[leg_id] == 0) {
        qp_solution_[i * k3Dim] = 0;
        qp_solution_[i * k3Dim + 1] = 0;
        qp_solution_[i * k3Dim + 2] = 0;
      } else {
        qp_solution_[i * k3Dim] = qp_sol[buffer_index * k3Dim];
        qp_solution_[i * k3Dim + 1] = qp_sol[buffer_index * k3Dim + 1];
        qp_solution_[i * k3Dim + 2] = qp_sol[buffer_index * k3Dim + 2];
        ++buffer_index;
      }
    }
#endif //TDS_ENABLE_QPOASES
    return qp_solution_;
    }
}


/*
 * Sprite.cpp
 *
 *  Created on: 2012-03-05
 *      Author: leo
 */

#include <fruitpunch/Graphics/Sprite.h>
#include <boost/shared_ptr.hpp>

using namespace fp_core::common;

namespace fp_core {

Sprite::Sprite() : Primitive() {
	init();

}

Sprite::~Sprite() {
	// TODO Auto-generated destructor stub
}

Sprite::Sprite(std::string image) : Primitive() {
  boost::shared_ptr<Image> i(new Image(image));

  init();
  setImage(i);
}

Sprite::Sprite(boost::shared_ptr<Image> image) : Primitive() {

  init();
  setImage(image);
}

void Sprite::init() {

    // The max here is 2 because we need a number that is a power of 2
    // so when we transform the primitive to local space, it won't have
    // a weird floating point center. For example: if this was just 1, then
    // the center of the primitive would be 0.5, and therefore when it scales
    // it will be off by those 0.5
    static float MAX = 2.0f;

	// Makes a MAXxMAX (2x2) square primitive
	point p1;
	p1.x = 0;
	p1.y = MAX;

	point uv1;
	uv1.x = 0;
	uv1.y = 0;

	point p2;
	p2.x = 0;
	p2.y = 0;

	point uv2;
	uv2.x = 0;
	uv2.y = 1;

	point p3;
	p3.x = MAX;
	p3.y = MAX;

    point uv3;
    uv3.x = 1;
    uv3.y = 0;

	point p4;
	p4.x = MAX;
	p4.y = 0;

    point uv4;
    uv4.x = 1;
    uv4.y = 1;

	Vertex v1(p1);
	v1.setUvmap(uv1);

	Vertex v2(p2);
	v2.setUvmap(uv2);

	Vertex v3(p3);
	v3.setUvmap(uv3);

	Vertex v4(p4);
	v4.setUvmap(uv4);

	addVertex(v1);
	addVertex(v2);
	addVertex(v3);
	addVertex(v4);

	// positions at 0x0 by default
//	setTop(0);
//	setLeft(0);
}

//void Sprite::renderFrame() {
//
//}
//
//void Sprite::onAdd() {
//}
//
//void Sprite::onRemove() {
//}

}/* namespace fp_core */

void fp_core::Sprite::setImage(boost::shared_ptr<Image> image) {
	Primitive::setImage(image);

	// defaults to the width and height of the image
	setWidth(getImage()->getWidth());
	setHeight(getImage()->getHeight());
}

void fp_core::Sprite::crop(float x, float y, float width, float height) {

	point uv1;
	uv1.x = x / getImage()->getWidth();
	uv1.y = (getImage()->getHeight() - y - height) / getImage()->getHeight();

	point uv2;
	uv2.x = x / getImage()->getWidth();
	uv2.y = (getImage()->getHeight() - y) / getImage()->getHeight();

	point uv3;
	uv3.x = (x + width) / getImage()->getWidth();
	uv3.y = (getImage()->getHeight() - y - height) / getImage()->getHeight();

	point uv4;
	uv4.x = (x + width) / getImage()->getWidth();
	uv4.y = (getImage()->getHeight() - y) / getImage()->getHeight();

	getVertex(0).setUvmap(uv1);
	getVertex(1).setUvmap(uv2);
	getVertex(2).setUvmap(uv3);
	getVertex(3).setUvmap(uv4);
}


#ifndef TIMING_ANALYSIS_HPP
#define TIMING_ANALYSIS_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <iostream>

#include "rtcf/macros.hpp"
OROCOS_HEADERS_BEGIN
#include <rtt_rosclock/rtt_rosclock.h>
OROCOS_HEADERS_END

namespace acc = boost::accumulators;

class TimingAnalysis {
  public:
    using time_t = uint64_t;
    using DurationAccumulator =
        acc::accumulator_set<time_t, acc::features<acc::tag::count, acc::tag::min, acc::tag::max, acc::tag::mean>>;

    TimingAnalysis();
    void configure(double period, double outlier_threshold = 0.1, size_t iterations_to_ignore = 1000);
    void reset();
    const ros::Time& start();
    void stop();

    const DurationAccumulator& getAccuPeriodDuration() const { return acc_iter_period_; };
    const DurationAccumulator& getAccuCalculationDuration() const { return acc_iter_calculation_; };

    const ros::Time& getIterationStamp() const { return time_iteration_; }
    const RTT::os::TimeService::nsecs& getCalculationDuration() const { return delta_calculation_; }

    std::string generateStatisticsString() const;

  private:
    // statistics
    DurationAccumulator acc_iter_period_;
    DurationAccumulator acc_iter_calculation_;

    // time source
    RTT::os::TimeService* time_service_;

    // period duration calculation
    RTT::os::TimeService::nsecs start_calculation_;
    RTT::os::TimeService::nsecs start_calculation_last_;
    RTT::os::TimeService::nsecs delta_calculation_;
    ros::Time time_iteration_;
    size_t remaining_iterations_to_ignore_;

    // overly delayed count
    size_t count_delayed_iterations_;
    RTT::os::TimeService::nsecs period_threshold_;
    double period_margin_;

    // number of ignored iterations at start
    size_t iterations_to_ignore_;
};

std::ostream& operator<<(std::ostream& os, const TimingAnalysis& t);

#endif  // TIMING_ANALYSIS_HPP


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
//  Sample output
//
// DFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// back: 1 --> 1
// tree: 1 --> 3
// back: 3 --> 1
// tree: 3 --> 4
// back: 4 --> 0
// back: 4 --> 1
// forward or cross: 2 --> 3

// BFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// tree: 2 --> 3
// cycle: 1 --> 1
// cycle: 1 --> 3
// cycle: 3 --> 1
// tree: 3 --> 4
// cycle: 4 --> 0
// cycle: 4 --> 1

#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>
#include <string>

#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/depth_first_search.hpp>

using namespace boost;
using namespace std;

template <class Tag>
struct edge_printer : public base_visitor<edge_printer<Tag> > {
  typedef Tag event_filter;
  edge_printer(std::string edge_t) : m_edge_type(edge_t) { }
  template <class Edge, class Graph>
  void operator()(Edge e, Graph& G) {
    std::cout << m_edge_type << ": " << source(e, G)
              << " --> " <<  target(e, G) << std::endl;
  }
  std::string m_edge_type;
};
template <class Tag>
edge_printer<Tag> print_edge(std::string type, Tag) {
  return edge_printer<Tag>(type);
}

int
main(int, char*[])
{

  using namespace boost;

  typedef adjacency_list<> Graph;
  typedef std::pair<int,int> E;
  E edges[] = { E(0, 2),
                E(1, 1), E(1, 3),
                E(2, 1), E(2, 3),
                E(3, 1), E(3, 4),
                E(4, 0), E(4, 1) };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph G(5);
  for (std::size_t j = 0; j < sizeof(edges)/sizeof(E); ++j)
    add_edge(edges[j].first, edges[j].second, G);
#else
  Graph G(edges, edges + sizeof(edges)/sizeof(E), 5);
#endif

  typedef boost::graph_traits<Graph>::vertices_size_type size_type;

  std::vector<size_type> d(num_vertices(G));
  std::vector<size_type> f(num_vertices(G));

  cout << "DFS categorized directed graph" << endl;
  depth_first_search(G, visitor(make_dfs_visitor(
      make_list(print_edge("tree", on_tree_edge()),
                print_edge("back", on_back_edge()),
                print_edge("forward or cross", on_forward_or_cross_edge())
                ))));

  cout << endl << "BFS categorized directed graph" << endl;
  boost::breadth_first_search
    (G, vertex(0, G), visitor(make_bfs_visitor(
     std::make_pair(print_edge("tree", on_tree_edge()),
                    print_edge("cycle", on_non_tree_edge())))));

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// filter.hpp                                                                //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_FACTOR_FILTER_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_FACTOR_FILTER_HPP_ER_2010
#include <boost/mpl/bool.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{

    template<typename Keys>
    struct filter{

        template<typename Key>
        struct contains : boost::mpl::contains<Keys, Key>{};

        template<typename T>
        struct apply : boost::mpl::bool_<false>{};

        template<typename Key>
        struct apply< contingency_table::tag::levels<Key> > : contains<Key>{};
    };

}// contingency_table
}// detail
}// statistics
}// boost

#endif


#include "rlib/crc16.h"

#include <gmock/gmock.h>
#include <algorithm>
#include <boost/crc.hpp>
#include <climits>
#include <functional>
#include <random>
#include <vector>

using crc_modbus_type = boost::crc_optimal<16, 0x8005, 0xFFFF, 0, true, true>;

#define CRC_TEST_TYPE(name, function_name, boost_specialization_name)                                    \
    struct name : public ::testing::Test {                                                               \
        using result_type = uint16_t;                                                                    \
        using boost_specialization = boost_specialization_name;                                          \
        static result_type Calc(const void* data, size_t len) { return rlib::function_name(data, len); } \
        static result_type Calc(const void* data, size_t len, result_type initial) {                     \
            return rlib::function_name(data, len, initial);                                              \
        }                                                                                \
    }

CRC_TEST_TYPE(Crc16Modbus, crc16_modbus, crc_modbus_type);
CRC_TEST_TYPE(Crc16CcittFfff, crc16_ccitt_ffff, boost::crc_ccitt_type);

template <typename CrcXxx>
struct CrcGenericTest : public ::testing::Test {
    void CalcCompleteAndCompare(std::vector<uint8_t> array) {
        auto result = CrcXxx::Calc(array.data(), array.size());
        typename CrcXxx::boost_specialization bcrc;
        bcrc.process_bytes(array.data(), array.size());
        EXPECT_THAT(result, ::testing::Eq(bcrc.checksum()));
    }
};

using CrcTypes = ::testing::Types<Crc16Modbus, Crc16CcittFfff>;
TYPED_TEST_SUITE(CrcGenericTest, CrcTypes);

TYPED_TEST(CrcGenericTest, Compiles) {}

TYPED_TEST(CrcGenericTest, CalculatesOkForZeroOrOneBytes) {
    this->CalcCompleteAndCompare({});
    this->CalcCompleteAndCompare({0x00});
}

TYPED_TEST(CrcGenericTest, CalculatesOkForRandom512) {
    using random_bytes_engine = std::independent_bits_engine<std::default_random_engine, CHAR_BIT, uint8_t>;
    random_bytes_engine rbe;
    std::vector<uint8_t> data(512);
    std::generate(begin(data), end(data), std::ref(rbe));

    this->CalcCompleteAndCompare(data);
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "aig_from_cirkit_bdd.hpp"

#include <functional>
#include <map>
#include <vector>

#include <boost/format.hpp>
#include <boost/range/algorithm.hpp>

#include <core/utils/timer.hpp>
#include <classical/utils/aig_utils.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

aig_function aig_from_bdd_rec( aig_graph& aig, const bdd& node,
                               std::map<unsigned, aig_function>& visited,
                               bool complement_optimization )
{
  auto it = visited.find( node.index );
  if ( it != visited.end() )
  {
    return it->second;
  }

  if ( complement_optimization )
  {
    auto comp = !node;
    it = visited.find( comp.index );
    if ( it != visited.end() )
    {
      return {it->second.node, !it->second.complemented};
    }
  }

  const auto& info = aig_info( aig );

  auto index = node.var();

  auto f_true  = aig_from_bdd_rec( aig, node.high(), visited, complement_optimization );
  auto f_false = aig_from_bdd_rec( aig, node.low(), visited, complement_optimization );

  auto func = aig_create_ite( aig, {info.inputs[index], false}, f_true, f_false );
  visited.insert( {node.index, func} );

  return func;
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

std::vector<aig_function> aig_from_bdd( aig_graph& aig, const std::vector<bdd>& fs,
                                        const properties::ptr& settings, const properties::ptr& statistics )
{
  using namespace std::placeholders;

  assert( !fs.empty() );

  /* settings */
  auto input_labels            = get( settings, "input_labels", std::vector<std::string>() );
  auto complement_optimization = get( settings, "complement_optimization", false );

  /* run-time */
  properties_timer t( statistics );

  const auto& info = aig_info( aig );

  auto n = fs.front().manager->num_vars();
  auto num_pis = info.inputs.size();

  for ( auto i = num_pis; i < n; ++i )
  {
    aig_create_pi( aig, i < input_labels.size() ? input_labels.at( i ) : boost::str( boost::format( "x%d" ) % i ) );
  }

  std::map<unsigned, aig_function> visited = { {0u, aig_get_constant( aig, false )}, {1u, aig_get_constant( aig, true )} };

  std::vector<aig_function> result( fs.size() );
  boost::transform( fs, result.begin(), std::bind( aig_from_bdd_rec, std::ref( aig ), _1, std::ref( visited ), complement_optimization ) );
  return result;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestNextPermutation
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/next_permutation.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(next_permutation_int)
{
    int dataset[] = {1, 3, 4, 2, 5};
    bc::vector<bc::int_> vector(dataset, dataset + 5, queue);

    bool result =
    	bc::next_permutation(vector.begin(), vector.begin() + 5, queue);

    CHECK_RANGE_EQUAL(int, 5, vector, (1, 3, 4, 5, 2));
    BOOST_VERIFY(result == true);

    vector[0] = 10; vector[1] = 9; vector[2] = 6;

    result = bc::next_permutation(vector.begin(), vector.begin() + 5, queue);

    CHECK_RANGE_EQUAL(int, 5, vector, (2, 5, 6, 9, 10));
    BOOST_VERIFY(result == false);
}

BOOST_AUTO_TEST_CASE(next_permutation_string)
{
    char dataset[] = "aaab";
    bc::vector<bc::char_> vector(dataset, dataset + 4, queue);

    bool result =
    	bc::next_permutation(vector.begin(), vector.begin() + 4, queue);

    CHECK_RANGE_EQUAL(char, 4, vector, ('a', 'a', 'b', 'a'));
    BOOST_VERIFY(result == true);

    result = bc::next_permutation(vector.begin(), vector.begin() + 4, queue);

    CHECK_RANGE_EQUAL(char, 4, vector, ('a', 'b', 'a', 'a'));
    BOOST_VERIFY(result == true);

    result = bc::next_permutation(vector.begin(), vector.begin() + 4, queue);

    CHECK_RANGE_EQUAL(char, 4, vector, ('b', 'a', 'a', 'a'));
    BOOST_VERIFY(result == true);

    result = bc::next_permutation(vector.begin(), vector.begin() + 4, queue);

    CHECK_RANGE_EQUAL(char, 4, vector, ('a', 'a', 'a', 'b'));
    BOOST_VERIFY(result == false);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE graph

#include "generic_label.hpp"
#include "graph.hpp"

#include <boost/graph/connected_components.hpp>
#include <boost/test/unit_test.hpp>
#include <set>

#include <memory>

using namespace std;

// The tests for the default-constructed edges.
BOOST_AUTO_TEST_CASE(default_edge_test)
{
  edge ne1, ne2;
  BOOST_CHECK(ne1 == ne2);
  unique_ptr<edge> nep = make_unique<edge>();
  BOOST_CHECK(ne1 == *nep);
  BOOST_CHECK(ne2 == *nep);
}

BOOST_AUTO_TEST_CASE(undirected_edge_test)
{
  graph g(2);
  vertex a = *(vertices(g).first);
  vertex b = *(vertices(g).first + 1);

  // Add a single edge using the boost add_edge function.
  boost::add_edge(a, b, g);

  // Get an edge descriptor of the normal edge.
  auto p1 = boost::edge(a, b, g);
  // Get an edge descriptor of the reverse edge.
  auto p2 = boost::edge(b, a, g);
  // Make sure we got the descriptros.
  BOOST_CHECK(p1.second && p2.second);
  // Make sure the edges are the same.
  BOOST_CHECK(p1 == p2);
  // Yet, BGL can tell what the source and target nodes are, and they
  // are different for e1 and e2.
  BOOST_CHECK(boost::source(p1.first, g) == a);
  BOOST_CHECK(boost::target(p1.first, g) == b);
  BOOST_CHECK(boost::source(p2.first, g) == b);
  BOOST_CHECK(boost::target(p2.first, g) == a);
}

BOOST_AUTO_TEST_CASE(end_edge_iterator_test)
{
  graph g(2);
  vertex a = *(vertices(g).first);
  vertex b = *(vertices(g).first + 1);

  // Does the end edge iterator change after adding an edge?
  auto p1 = edges(g);
  boost::add_edge(a, b, g);
  auto p2 = edges(g);

  BOOST_CHECK(p1.first != p2.first);
  BOOST_CHECK(p1.second == p2.second);

  // The edge descriptors should be the same.
  BOOST_CHECK(*(p1.second) == *(p2.second));
}

BOOST_AUTO_TEST_CASE(parallel_edge_test)
{
  graph g(2);
  vertex a = *(vertices(g).first);
  vertex b = *(vertices(g).first + 1);
  edge e1, e2;
  bool r1, r2;
  tie(e1, r1) = add_edge(a, b, g);
  tie(e2, r2) = add_edge(a, b, g);

  // Make sure we can represent multi-graphs.  That we successfully
  // added the edges, and that the edges are different.
  BOOST_CHECK(r1);
  BOOST_CHECK(r2);
  BOOST_CHECK(e1 != e2);
  BOOST_CHECK(!(e1 == e2));

  // Make sure that parallel edges are differently identified by a
  // map.
  std::map<edge, int> e2i;
  e2i[e1] = 1;
  e2i[e2] = 2;
  BOOST_CHECK(e2i[e1] == 1);
  BOOST_CHECK(e2i[e2] == 2);
  BOOST_CHECK(e2i.size() == 2);
}

BOOST_AUTO_TEST_CASE(connected_components_test)
{
  graph g(2);
  vertex a = *(vertices(g).first);
  vertex b = *(vertices(g).first + 1);
  edge e;
  bool r;
  tie(e, r) = add_edge(a, b, g);

  std::vector<int> c(num_vertices(g));
  // "num" is the number of connected components.
  int num = boost::connected_components(g, &c[0]);
  BOOST_CHECK(num == 1);

  tie(e, r) = add_edge(b, a, g);

  // "num" is the number of connected components.
  num = boost::connected_components(g, &c[0]);
  BOOST_CHECK(num == 1);
}

BOOST_AUTO_TEST_CASE(connected_components_test2)
{
  graph g(4);
  vertex a = *(vertices(g).first);
  vertex b = *(vertices(g).first + 1);
  vertex c = *(vertices(g).first + 2);
  vertex d = *(vertices(g).first + 3);

  add_edge(b, d, g);
  add_edge(c, d, g);

  std::vector<int> comp(num_vertices(g));
  // "num" is the number of connected components.
  int num = boost::connected_components(g, &comp[0]);
  BOOST_CHECK(num == 2);
}


#ifndef TCM_HERMITIAN_HPP
#define TCM_HERMITIAN_HPP

#include <cassert>
#include <limits>
#include <memory>

#include <boost/core/demangle.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <benchmark.hpp>
#include <detail/lapack_wrapper.hpp>
#include <detail/utils.hpp>

// ============================================================================
// ||                                                                        ||
// ||                             ? H E E V R                                ||
// ||                                                                        ||
// ============================================================================

namespace tcm {

namespace lapack {

namespace {

template< class _Alloc
        , class _T
        , class = std::enable_if_t
                  <    std::is_same<_T, float>()
                    or std::is_same<_T, double>()
		          >
        >
auto heevr_impl( int const N
               , _T* A, int const LDA
               , _T* W
               , _T* Z, int const LDZ
               , utils::Type2Type<_T> ) -> void
{
	if (N == 0) return;

	assert(N > 0);
	assert(A != nullptr and LDA >= N);
	assert(W != nullptr);
	assert(LDZ >= (Z == nullptr) ? 1 : N);

	/*
	using _Alloc_traits = std::allocator_traits<_Alloc>;
	typename _Alloc_traits::template rebind_alloc<_T>
		_T_alloc;
	typename _Alloc_traits::template rebind_alloc<int>
		_int_alloc;
	*/
	using size_type = std::make_unsigned_t<int>;

	char const JOBZ   = (Z == nullptr) ? 'N' : 'V';
	char const RANGE  = 'A';
	char const UPLO   = 'U';
	_T   const VL     { 0.0 };
	_T   const VU     { 0.0 };
	int  const IL     = 0;
	int  const IU     = 0;
	_T   const ABSTOL { 0.0 }; //std::numeric_limits<_T>::min();
	int        M      = 0;
	auto       ISUPPZ =
		tcm::utils::_Storage<int, _Alloc>{2 * static_cast<size_type>(N)};
	// tcm::utils::allocate_workspace(_int_alloc, 2 * N);
	int        LWORK  = -1;
	int        LIWORK = -1;
	int        INFO   = 0;

	{
		_T  _work_dummy;
		int _iwork_dummy;

		tcm::import::heevr<_T>
			( &JOBZ, &RANGE, &UPLO, &N
			, A, &LDA
			, &VL, &VU
			, &IL, &IU
			, &ABSTOL, &M
			, W
			, Z, &LDZ
			, ISUPPZ.data()
			, &_work_dummy, &LWORK
			, &_iwork_dummy, &LIWORK
			, &INFO
		    );

		LWORK = static_cast<int>(_work_dummy);
		LIWORK = _iwork_dummy;
	}

	auto       WORK   =
		tcm::utils::_Storage<_T, _Alloc>{static_cast<size_type>(LWORK)};
	// tcm::utils::allocate_workspace(_T_alloc, LWORK);
	auto       IWORK  =
		tcm::utils::_Storage<int, _Alloc>{static_cast<size_type>(LIWORK)};
	// allocate_workspace(_int_alloc, LIWORK);

	tcm::import::heevr<_T>
		( &JOBZ, &RANGE, &UPLO, &N
		, A, &LDA
		, &VL, &VU
		, &IL, &IU
		, &ABSTOL, &M
		, W, Z, &LDZ
		, ISUPPZ.data()
		, WORK.data(), &LWORK
		, IWORK.data(), &LIWORK
		, &INFO
		);

	if (INFO < 0) {
		throw std::invalid_argument{ "Argument #" + std::to_string(-INFO)
		                           + " had an illegal value." };
	}
	else if (INFO > 0) {
		throw std::runtime_error{"Call to ?SYEVR failed."};
	}
}

template< class _Alloc
        , class _T
        , class = std::enable_if_t
                  <    std::is_same<_T, float>()
                    or std::is_same<_T, double>()
                  >
        >
auto heevr_impl( int const N
               , std::complex<_T>* A, int const LDA
               , _T* W
               , std::complex<_T>* Z, int const LDZ
               , utils::Type2Type<std::complex<_T>> ) -> void
{
	if (N == 0) return;

	assert(N > 0);
	assert(A != nullptr and LDA >= N);
	assert(W != nullptr);
	assert(LDZ >= (Z == nullptr) ? 1 : N);

	/*
	using _Alloc_traits = std::allocator_traits<_Alloc>;
	typename _Alloc_traits::template rebind_alloc<std::complex<_T>>
		_Cmpl_T_alloc;
	typename _Alloc_traits::template rebind_alloc<_T>
		_T_alloc;
	typename _Alloc_traits::template rebind_alloc<int>
		_int_alloc;
	*/
	using size_type = std::make_unsigned_t<int>;

	char const JOBZ   = (Z == nullptr) ? 'N' : 'V';
	char const RANGE  = 'A';
	char const UPLO   = 'U';
	_T   const VL     { 0.0 };
	_T   const VU     { 0.0 };
	int  const IL     = 0;
	int  const IU     = 0;
	_T   const ABSTOL { 0.0 }; //std::numeric_limits<T>::min();
	int        M      = 0;
	auto       ISUPPZ =
		tcm::utils::_Storage<int, _Alloc>{2 * static_cast<size_type>(N)};
	// tcm::utils::allocate_workspace(_int_alloc, 2 * N);
	                 // std::make_unique<int[]>(2 * N);
	int        LWORK  = -1;
	int        LRWORK = -1;
	int        LIWORK = -1;
	int        INFO   = 0;

	{
		std::complex<_T>   _work_dummy;
		_T                 _rwork_dummy;
		int                _iwork_dummy;

		tcm::import::heevr<std::complex<_T>>
			( &JOBZ, &RANGE, &UPLO, &N
			, A, &LDA
			, &VL, &VU
			, &IL, &IU
			, &ABSTOL, &M
			, W
			, Z, &LDZ
			, ISUPPZ.data()
			, &_work_dummy, &LWORK
			, &_rwork_dummy, &LRWORK
			, &_iwork_dummy, &LIWORK
			, &INFO
		    );

		LWORK  = static_cast<int>(std::real(_work_dummy));
		LRWORK = static_cast<int>(_rwork_dummy);
		LIWORK = _iwork_dummy;
	}

	auto       WORK   =
		tcm::utils::_Storage<std::complex<_T>, _Alloc>{
			static_cast<size_type>(LWORK) };
	// tcm::utils::allocate_workspace(_Cmpl_T_alloc, LWORK);
	                 // std::make_unique<std::complex<_T>[]>(LWORK);
	auto       RWORK  =
		tcm::utils::_Storage<_T, _Alloc>{static_cast<size_type>(LRWORK)};
	// tcm::utils::allocate_workspace(_T_alloc, LRWORK);
	                 // std::make_unique<_T[]>(LRWORK);
	auto       IWORK  =
		tcm::utils::_Storage<int, _Alloc>{static_cast<size_type>(LIWORK)};
	// tcm::utils::allocate_workspace(_int_alloc, LIWORK);
	                 // std::make_unique<int[]>(LIWORK);

	tcm::import::heevr<std::complex<_T>>
		( &JOBZ, &RANGE, &UPLO, &N
		, A, &LDA
		, &VL, &VU
		, &IL, &IU
		, &ABSTOL, &M
		, W, Z, &LDZ
		, ISUPPZ.data()
		, WORK.data(), &LWORK
		, RWORK.data(), &LRWORK
		, IWORK.data(), &LIWORK
		, &INFO
		);

	if(INFO < 0) {
		throw std::invalid_argument{ "Argument #" + std::to_string(-INFO)
		                           + " had an illegal value." };
	}
	else if(INFO > 0) {
		throw std::runtime_error{"Call to ?HEEVR failed."};
	}
}

} // unnamed namespace

template< class _T
        , class _Alloc = std::allocator<_T>
        >
auto heevr( std::size_t const n
          , _T* A, std::size_t const lda
          , utils::Base<_T>* W
          , _T* Z, std::size_t const ldz ) -> void
{
	TCM_MEASURE("heevr<" + boost::core::demangle(typeid(_T).name()) + ">()");

	heevr_impl<_Alloc>
		( boost::numeric_cast<int>(n)
	    , A, boost::numeric_cast<int>(lda)
	    , W
	    , Z, boost::numeric_cast<int>(ldz)
	    , utils::Type2Type<_T>{} );
}

} // namespace lapack

} // namespace tcm

// ============================================================================
// ||                                                                        ||
// ||                              ? H E E V                                 ||
// ||                                                                        ||
// ============================================================================

/*
namespace tcm {

namespace lapack {

namespace {

template< class _T
        , class = std::enable_if_t
                  <   std::is_same<_T, float>()
                   or std::is_same<_T, double>()
                  >
        >
auto heev_impl( int const N
              , _T* A, int const LDA
              , _T* W
              , bool compute_eigenvectors
              , utils::Type2Type<_T> ) -> void
{
	if (N == 0) return;

	assert(N > 0);
	assert(A != nullptr and LDA >= N);
	assert(W != nullptr);

	char const JOBZ  = compute_eigenvectors ? 'V' : 'N';
	char const UPLO  = 'U';
	int        LWORK = -1;
	int        INFO;

	{
		_T _work_dummy;

		tcm::import::heev<_T>
			( &JOBZ, &UPLO, &N
			, A, &LDA
			, W
			, &_work_dummy,  &LWORK
			, &INFO
			);

		LWORK = static_cast<int>(_work_dummy);
	}

	auto       WORK  = std::make_unique<_T[]>(LWORK);

	tcm::import::heev<_T>
		( &JOBZ, &UPLO, &N
		, A, &LDA
		, W
		, WORK.get(), &LWORK
		, &INFO
		);

	if (INFO < 0) {
		throw std::invalid_argument{ "Argument #" + std::to_string(-INFO)
		                           + " had an illegal value." };
	}
	else if (INFO > 0) {
		throw std::runtime_error{"Call to ?SYEV failed."};
	}
}

template< class _T
        , class = std::enable_if_t
                  <   std::is_same<_T, float>()
                   or std::is_same<_T, double>()
                  >
        >
auto heev_impl( int const N
              , std::complex<_T>* A, int const LDA
              , _T* W
              , bool compute_eigenvectors
              , utils::Type2Type<std::complex<_T>> ) -> void
{
	if(N == 0) return;

	assert(N > 0);
	assert(A != nullptr and LDA >= N);
	assert(W != nullptr);

	char const JOBZ  = compute_eigenvectors ? 'V' : 'N';
	char const UPLO  = 'U';
	auto       RWORK = std::make_unique<_T[]>(3 * N - 2);
	int        LWORK = -1;
	int        INFO;

	{
		std::complex<_T> _work_dummy;

		tcm::import::heev<std::complex<_T>>
			( &JOBZ, &UPLO, &N
			, A, &LDA
			, W
			, &_work_dummy, &LWORK
			, RWORK.get()
			, &INFO
			);

		LWORK = static_cast<int>(std::real(_work_dummy));
	}

	auto       WORK  = std::make_unique<std::complex<_T>[]>(LWORK);

	tcm::import::heev<std::complex<_T>>
		( &JOBZ, &UPLO, &N
		, A, &LDA
		, W
		, WORK.get(),  &LWORK
		, RWORK.get()
		, &INFO
		);

	if (INFO < 0) {
		throw std::invalid_argument{ "Argument #" + std::to_string(-INFO)
		                           + " had an illegal value." };
	}
	else if (INFO > 0) {
		throw std::runtime_error{"Call to ?HEEV failed."};
	}
}

} // unnamed namespace

template<class _T>
inline
auto heev( std::size_t const n
         , _T* A, std::ptrdiff_t const lda
         , utils::Base<_T>* W
         , bool compute_eigenvectors ) -> void
{
	MEASURE;

	heev_impl( boost::numeric_cast<int>(n)
	         , A, boost::numeric_cast<int>(lda)
	         , W
	         , compute_eigenvectors
	         , utils::Type2Type<_T>{} );
}

} // namespace lapack

} // namespace tcm
*/

#endif // TCM_HERMITIAN_HPP


/***************************************************************************
 * NASA Glenn Research Center, Cleveland, OH
 * Released under the NASA Open Source Agreement (NOSA)
 * May  2021
 *
 ****************************************************************************
 */
#include "MemoryManagerTreeArray.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/detail/bitscan.hpp>
#include <iostream>
#include <string>
#include <inttypes.h>
#ifdef USE_BITTEST
# include <immintrin.h>
# ifdef HAVE_INTRIN_H
#  include <intrin.h>
# endif
# ifdef HAVE_X86INTRIN_H
#  include <x86intrin.h>
# endif
#elif defined(USE_ANDN)
# include <immintrin.h>
# if defined(__GNUC__) && defined(HAVE_X86INTRIN_H)
#  include <x86intrin.h>
#  define _andn_u64(a, b)   (__andn_u64((a), (b)))
# elif defined(_MSC_VER)
#  include <ammintrin.h>
# endif
#endif //USE_BITTEST or USE_ANDN

 //static uint64_t g_numLeaves = 0;

MemoryManagerTreeArray::MemoryManagerTreeArray(const uint64_t maxSegments) : M_MAX_SEGMENTS(maxSegments) {
    SetupTree();
}
MemoryManagerTreeArray::~MemoryManagerTreeArray() {
    FreeTree();
}

void MemoryManagerTreeArray::SetupTree() {
    for (unsigned int i = 0; i < MAX_TREE_ARRAY_DEPTH; ++i) {
        const uint64_t arraySize64s = (((uint64_t)1) << (i * 6));
        //std::cout << i << " " << arraySize64s << "\n";
        m_bitMasks[i] = (uint64_t*)malloc(arraySize64s * sizeof(uint64_t));
        uint64_t * const currentArrayPtr = m_bitMasks[i];
        for (uint64_t j = 0; j < arraySize64s; ++j) {
            currentArrayPtr[j] = UINT64_MAX;
        }
    }

}

void MemoryManagerTreeArray::FreeTree() {
    for (unsigned int i = 0; i < MAX_TREE_ARRAY_DEPTH; ++i) {
        free(m_bitMasks[i]);
    }
}

void MemoryManagerTreeArray::BackupDataToVector(backup_memmanager_t & backup) const {
    backup.resize(MAX_TREE_ARRAY_DEPTH);
    for (unsigned int i = 0; i < MAX_TREE_ARRAY_DEPTH; ++i) {
        const uint64_t arraySize64s = (((uint64_t)1) << (i * 6));
        std::vector<uint64_t> & row = backup[i];
        row.resize(arraySize64s);
        const uint64_t * const currentArrayPtr = m_bitMasks[i];
        for (uint64_t j = 0; j < arraySize64s; ++j) {
            row[j] = currentArrayPtr[j];
        }
    }
}

bool MemoryManagerTreeArray::IsBackupEqual(const backup_memmanager_t & backup) const {
    for (unsigned int i = 0; i < MAX_TREE_ARRAY_DEPTH; ++i) {
        const uint64_t arraySize64s = (((uint64_t)1) << (i * 6));
        const std::vector<uint64_t> & row = backup[i];
        const uint64_t * const currentArrayPtr = m_bitMasks[i];
        for (uint64_t j = 0; j < arraySize64s; ++j) {
            if (row[j] != currentArrayPtr[j]) return false;
        }
    }
    return true;
}

bool MemoryManagerTreeArray::GetAndSetFirstFreeSegmentId(const uint32_t depthIndex, const uint32_t rowIndex, uint32_t * segmentId) {

    uint64_t * const currentArrayPtr = m_bitMasks[depthIndex];
    uint64_t * const currentBit64Ptr = &currentArrayPtr[rowIndex];
    const unsigned int firstFreeIndex = boost::multiprecision::detail::find_lsb<uint64_t>(*currentBit64Ptr);
    *segmentId += firstFreeIndex * (1 << (((MAX_TREE_ARRAY_DEPTH - 1) - depthIndex) * 6)); // 64^depth

    if ((depthIndex == MAX_TREE_ARRAY_DEPTH - 1) || GetAndSetFirstFreeSegmentId(depthIndex + 1, rowIndex + firstFreeIndex * (1 << (depthIndex * 6)), segmentId)) {
        if (*segmentId < M_MAX_SEGMENTS) {
#ifdef USE_BITTEST
            _bittestandreset64((int64_t*)currentBit64Ptr, firstFreeIndex);
#elif defined(USE_ANDN)
            const uint64_t mask64 = (((uint64_t)1) << firstFreeIndex);
            *currentBit64Ptr = _andn_u64(mask64, *currentBit64Ptr);
#else
            const uint64_t mask64 = (((uint64_t)1) << firstFreeIndex);
            *currentBit64Ptr &= ~mask64;
#endif
        }
    }
    /*

    if (depthIndex == MAX_TREE_ARRAY_DEPTH - 1) { //at the child leaf
        *currentBit64Ptr &= ~mask64;

    }
    else { //at an inner node
        const unsigned int nextRowIndex = rowIndex + firstFreeIndex * (1 << (depthIndex * 6));
        if (GetAndSetFirstFreeSegmentId(depthIndex + 1, nextRowIndex, segmentId)) {
            *currentBit64Ptr &= ~mask64;
        }
    }
    */

    return (*currentBit64Ptr == 0); //return isFull

}

segment_id_t MemoryManagerTreeArray::GetAndSetFirstFreeSegmentId_NotThreadSafe() {
    if (m_bitMasks[0][0] == 0) return UINT32_MAX; //bitmask of zero means full
    uint32_t segmentId = 0;
    GetAndSetFirstFreeSegmentId(0, 0, &segmentId);
    if (segmentId >= M_MAX_SEGMENTS) return UINT32_MAX;
    return segmentId;
}

bool MemoryManagerTreeArray::IsSegmentFree(const uint32_t depthIndex, const uint32_t rowIndex, uint32_t segmentId) {

    const unsigned int index = (segmentId >> (((MAX_TREE_ARRAY_DEPTH - 1) - depthIndex) * 6)) & 63;

    if (depthIndex == MAX_TREE_ARRAY_DEPTH - 1) { //leaf
        const uint64_t * const currentArrayPtr = m_bitMasks[depthIndex];
        const uint64_t * const currentBit64Ptr = &currentArrayPtr[rowIndex];
#ifdef USE_BITTEST
        return _bittest64((const int64_t*)currentBit64Ptr, index);
#else
        const uint64_t mask64 = (((uint64_t)1) << index);
        return (*currentBit64Ptr & mask64); //leaf bit is 1 (empty)
#endif
    }
    else { //inner node
        return IsSegmentFree(depthIndex + 1, rowIndex + index * (1 << (depthIndex * 6)), segmentId);
    }
}

bool MemoryManagerTreeArray::IsSegmentFree(segment_id_t segmentId) {
    return IsSegmentFree(0, 0, segmentId);
}

void MemoryManagerTreeArray::FreeSegmentId(const uint32_t depthIndex, const uint32_t rowIndex, uint32_t segmentId, bool *success) {

    uint64_t * const currentArrayPtr = m_bitMasks[depthIndex];
    uint64_t * const currentBit64Ptr = &currentArrayPtr[rowIndex];
    const unsigned int index = (segmentId >> (((MAX_TREE_ARRAY_DEPTH - 1) - depthIndex) * 6)) & 63;
#ifdef USE_BITTEST
    const bool bitWasAlreadyOne = _bittestandset64((int64_t*)currentBit64Ptr, index);
    if (depthIndex == MAX_TREE_ARRAY_DEPTH - 1) { //leaf
        *success = !bitWasAlreadyOne; //error if leaf bit is already 1 (empty)
    }
    else { //inner node
        FreeSegmentId(depthIndex + 1, rowIndex + index * (1 << (depthIndex * 6)), segmentId, success);
    }
#else
    const uint64_t mask64 = (((uint64_t)1) << index);

    if (depthIndex == MAX_TREE_ARRAY_DEPTH - 1) { //leaf
        //if (*currentBit64Ptr & mask64) { //leaf bit is already 1 (empty)
        //    *success = false; //error
        //}
        *success = ((*currentBit64Ptr & mask64) == 0);
    }
    else { //inner node
        FreeSegmentId(depthIndex + 1, rowIndex + index * (1 << (depthIndex * 6)), segmentId, success);
    }
    *currentBit64Ptr |= mask64;
#endif
}

bool MemoryManagerTreeArray::AllocateSegmentId_NoCheck(const uint32_t depthIndex, const uint32_t rowIndex, uint32_t segmentId) {

    uint64_t * const currentArrayPtr = m_bitMasks[depthIndex];
    uint64_t * const currentBit64Ptr = &currentArrayPtr[rowIndex];
    const unsigned int index = (segmentId >> (((MAX_TREE_ARRAY_DEPTH - 1) - depthIndex) * 6)) & 63;

    if ((depthIndex == MAX_TREE_ARRAY_DEPTH - 1) || AllocateSegmentId_NoCheck(depthIndex + 1, rowIndex + index * (1 << (depthIndex * 6)), segmentId)) {
        if (segmentId < M_MAX_SEGMENTS) {
#ifdef USE_BITTEST
            _bittestandreset64((int64_t*)currentBit64Ptr, index);
#elif defined(USE_ANDN)
            const uint64_t mask64 = (((uint64_t)1) << index);
            *currentBit64Ptr = _andn_u64(mask64, *currentBit64Ptr);
#else
            const uint64_t mask64 = (((uint64_t)1) << index);
            *currentBit64Ptr &= ~mask64;
#endif
        }
    }

    return (*currentBit64Ptr == 0); //return isFull

}

void MemoryManagerTreeArray::AllocateSegmentId_NoCheck_NotThreadSafe(segment_id_t segmentId) {
    AllocateSegmentId_NoCheck(0, 0, segmentId);
}

bool MemoryManagerTreeArray::FreeSegmentId_NotThreadSafe(segment_id_t segmentId) {
    bool success = true;
    FreeSegmentId(0, 0, segmentId, &success);
    return success;
}

bool MemoryManagerTreeArray::AllocateSegments_ThreadSafe(segment_id_chain_vec_t & segmentVec) { //number of segments should be the vector size
    boost::mutex::scoped_lock lock(m_mutex);
    const std::size_t size = segmentVec.size();
    for (std::size_t i = 0; i < size; ++i) {
        const segment_id_t segmentId = GetAndSetFirstFreeSegmentId_NotThreadSafe();
        if (segmentId != UINT32_MAX) { //success
            segmentVec[i] = segmentId;
        }
        else { //fail
            for (std::size_t j = 0; j < i; ++j) {
                FreeSegmentId_NotThreadSafe(segmentVec[j]);
            }
            segmentVec.resize(0);
            return false;
        }
    }
    return true;
}

bool MemoryManagerTreeArray::FreeSegments_ThreadSafe(const segment_id_chain_vec_t & segmentVec) {
    boost::mutex::scoped_lock lock(m_mutex);
    const std::size_t size = segmentVec.size();
    bool success = true;
    for (std::size_t i = 0; i < size; ++i) {
        if (!FreeSegmentId_NotThreadSafe(segmentVec[i])) {
            success = false;
        }
    }
    //segmentVec.resize(0);
    return success;
}


//---------------------------------------------------------------------------//
//!
//! \file   tstAtomicMassUnit.cpp
//! \author Alex Robinson
//! \brief  The atomic mass unit unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/mass.hpp>
#include <boost/units/systems/si/mass.hpp>

// FRENSIE Includes
#include "Utility_AtomicMassUnit.hpp"

using namespace Utility::Units;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the atomic mass unit can be initialized
BOOST_AUTO_TEST_CASE( initialize )
{
  boost::units::quantity<Utility::Units::AtomicMass> mass( 1.0*amu );

  BOOST_CHECK_EQUAL( mass.value(), 1.0 );

  mass = 2.0*amu;

  BOOST_CHECK_EQUAL( mass.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the atomic mass unit can be initialized from cgs and si units
BOOST_AUTO_TEST_CASE( initialize_from_cgs_si )
{
  boost::units::quantity<Utility::Units::AtomicMass> mass_cgs( 1.0*cgs::gram );

  boost::units::quantity<Utility::Units::AtomicMass> mass_si( 1.0*si::kilogram );

  BOOST_CHECK_CLOSE_FRACTION( mass_cgs.value(),
                              6.0221408585491615e+23,
                              1e-15 );
  BOOST_CHECK_CLOSE_FRACTION( mass_si.value(),
                              6.0221408585491615e+26,
                              1e-15 );
}

//---------------------------------------------------------------------------//
// Check that the cgs and si units can be initialized from the atomic mass u.
BOOST_AUTO_TEST_CASE( initialize_cgs_si )
{
  boost::units::quantity<cgs::mass> cgs_mass( 1.0*amu );

  boost::units::quantity<si::mass> si_mass( 1.0*amu );

  BOOST_CHECK_CLOSE_FRACTION( cgs_mass.value(),
                              1.660539040e-24,
                              1e-15 );
  BOOST_CHECK_CLOSE_FRACTION( si_mass.value(),
                              1.660539040e-27,
                              1e-15 );
}

//---------------------------------------------------------------------------//
// end tstAtomicMassUnit.cpp
//---------------------------------------------------------------------------//


#include "Transform.h"
#include "../Entity/Entity.h"
#include <boost/range/adaptor/indexed.hpp>

gsl::Transform::Transform(const boost::uuids::uuid & Obj) :self_(Obj) {

}

gsl::Transform::Transform(Transform && Obj) {
  this->self_ = std::move(Obj.self_);
  this->center_ = std::move(Obj.center_);
  this->child_ = std::move(Obj.child_);
  this->globalmatrix_ = std::move(Obj.globalmatrix_);
  this->localmatrix_ = std::move(Obj.localmatrix_);
  this->parent_ = std::move(Obj.parent_);
  this->rot_ = std::move(Obj.rot_);
  this->scale_ = std::move(Obj.scale_);
}

gsl::Transform::~Transform() {

}

gsl::Transform & gsl::Transform::SetCenter(const s3d::Vec2 & Center){
  this->center_ = Center;
  this->LocalMatrixUpdate();
  this->GlobalMatrixUpdate();
  return *this;
}

const s3d::Vec2 & gsl::Transform::GetCenter() const {
  return this->center_;
}

gsl::Transform & gsl::Transform::SetScale(const s3d::Vec2 & Scale){
  this->scale_ = Scale;
  this->LocalMatrixUpdate();
  this->GlobalMatrixUpdate();
  return *this;
}

const s3d::Vec2 & gsl::Transform::GetScale() const {
  return this->scale_;
}

gsl::Transform & gsl::Transform::SetRotation(double Angle) {
  this->rot_ = Angle;
  this->LocalMatrixUpdate();
  this->GlobalMatrixUpdate();
  return *this;
}

double gsl::Transform::GetRotation() const {
  return this->rot_;
}

gsl::Transform & gsl::Transform::AddChild(const boost::uuids::uuid & Obj) {
  this->child_.push_back(Obj);
  auto& last = this->child_.back();
  auto& trans = Entity(last).GetTransform();
  trans->parent_ = this->self_;
  trans->LocalMatrixUpdate();
  trans->GlobalMatrixUpdate();
  return *this;
}

gsl::Transform & gsl::Transform::RemoveChild(const boost::uuids::uuid & Obj) {
  return this->RemoveChildIf([&](const boost::uuids::uuid& Id) {return Obj == Id; });
}

gsl::Transform & gsl::Transform::RemoveChildIf(const std::function<bool(const boost::uuids::uuid&)>& Pred) {
  this->child_.erase(std::remove_if(this->child_.begin(), this->child_.end(), Pred), this->child_.end());
  return *this;
}

gsl::Entity gsl::Transform::GetOwner() {
  return gsl::Entity(this->self_);
}

std::vector<gsl::Entity> gsl::Transform::GetChild() {
  return this->GetChildIf([](auto) {return true; });
}

std::vector<gsl::Entity> gsl::Transform::GetChildIf(const std::function<bool(const Entity&)>& Pred) {
  std::vector<Entity> result;
  for (auto&& id : this->child_) {
    Entity entity(id);
    if (entity.IsValid() && Pred(entity)) {
      result.emplace_back(std::move(entity));
    }
  }
  return result;
}

const s3d::Mat3x2 & gsl::Transform::GetLocalMatrix() const{
  return this->localmatrix_;
}

const s3d::Mat3x2 & gsl::Transform::GetGlobalMatrix() const{
  return this->globalmatrix_;
}

void gsl::Transform::GlobalMatrixUpdate() {
  auto trans = Entity(*this->parent_).GetTransform();
  if (this->parent_ && trans) {
    this->globalmatrix_ = trans->globalmatrix_ * this->localmatrix_;
  }
  else {
    this->globalmatrix_ = this->localmatrix_;
  }
}

void gsl::Transform::LocalMatrixUpdate() {
  this->localmatrix_ = s3d::Mat3x2::Rotate(this->rot_, this->center_).translate(this->center_).scale(this->scale_);
}


#include "Lattice.hpp"

#include <Eigen/Dense>  // for `colPivHouseholderQr()`

#include <support/format.hpp>
using namespace fmt::literals;

namespace cpb {

namespace {
    /// Visit all hopping terms in all families
    template<class F>
    void for_each_term(Lattice::Hoppings const& hoppings, F lambda) {
        for (auto const& pair : hoppings) {
            auto const& family = pair.second;
            for (auto const& term : family.terms) {
                lambda(family, term);
            }
        }
    }
} // anonymous namespace

Lattice::Lattice(Cartesian a1, Cartesian a2, Cartesian a3) {
    vectors.push_back(a1);
    if (!a2.isZero()) { vectors.push_back(a2); }
    if (!a3.isZero()) { vectors.push_back(a3); }
    vectors.shrink_to_fit();
}

void Lattice::add_sublattice(string_view name, Cartesian position, double onsite_energy) {
    add_sublattice(name, position, detail::canonical_onsite_energy(onsite_energy));
}

void Lattice::add_sublattice(string_view name, Cartesian position, VectorXd const& onsite_energy) {
    add_sublattice(name, position, detail::canonical_onsite_energy(onsite_energy));
}

void Lattice::add_sublattice(string_view name, Cartesian position,
                             MatrixXcd const& onsite_energy) {
    detail::check_onsite_energy(onsite_energy);

    auto const hermitian_view = onsite_energy.selfadjointView<Eigen::Upper>();
    auto const unique_id = make_unique_sublattice_id(name);
    auto const alias_id = SubAliasID(unique_id);
    sublattices[name] = {position, hermitian_view, unique_id, alias_id};
}

void Lattice::add_alias(string_view alias_name, string_view original_name, Cartesian position) {
    auto const& original = sublattice(original_name);
    auto const alias_id = SubAliasID(original.unique_id);
    auto const unique_id = make_unique_sublattice_id(alias_name);
    sublattices[alias_name] = {position, original.energy, unique_id, alias_id};
}

void Lattice::register_hopping_energy(std::string const& name, std::complex<double> energy) {
    register_hopping_energy(name, detail::canonical_hopping_energy(energy));
}

void Lattice::register_hopping_energy(std::string const& name, MatrixXcd const& energy) {
    if (name.empty()) { throw std::logic_error("Hopping name can't be blank"); }

    detail::check_hopping_energy(energy);

    auto const unique_id = HopID(hoppings.size());
    auto const is_unique_name = hoppings.insert({name, {energy, unique_id, {}}}).second;
    if (!is_unique_name) { throw std::logic_error("Hopping '" + name + "' already exists"); }
}

void Lattice::add_hopping(Index3D relative_index, string_view from_sub, string_view to_sub,
                          string_view hopping_family_name) {
    if (from_sub == to_sub && relative_index == Index3D::Zero()) {
        throw std::logic_error(
            "Hoppings from/to the same sublattice must have a non-zero relative "
            "index in at least one direction. Don't define onsite energy here."
        );
    }

    auto const& from = sublattice(from_sub);
    auto const& to = sublattice(to_sub);
    auto const& hop_matrix = hopping_family(hopping_family_name).energy;

    if (from.energy.rows() != hop_matrix.rows() || to.energy.cols() != hop_matrix.cols()) {
        throw std::logic_error(
            "Hopping size mismatch: from '{}' ({}) to '{}' ({}) with matrix '{}' ({}, {})"_format(
                from_sub, from.energy.rows(), to_sub, to.energy.cols(),
                hopping_family_name, hop_matrix.rows(), hop_matrix.cols()
            )
        );
    }

    auto const candidate = HoppingTerm{relative_index, from.unique_id, to.unique_id};
    for_each_term(hoppings, [&](HoppingFamily const&, HoppingTerm const& existing) {
        if (candidate == existing) {
            throw std::logic_error("The specified hopping already exists.");
        }
    });

    hoppings[hopping_family_name].terms.push_back(candidate);
}

void Lattice::add_hopping(Index3D relative_index, string_view from_sub, string_view to_sub,
                          std::complex<double> energy) {
    add_hopping(relative_index, from_sub, to_sub, detail::canonical_hopping_energy(energy));
}

void Lattice::add_hopping(Index3D relative_index, string_view from_sub, string_view to_sub,
                          MatrixXcd const& energy) {
    auto const hopping_name = [&] {
        // Look for an existing hopping ID with the same energy
        auto const it = std::find_if(hoppings.begin(), hoppings.end(), [&](Hoppings::reference r) {
            auto const& e = r.second.energy;
            return e.rows() == energy.rows() && e.cols() == energy.cols() && e == energy;
        });

        if (it != hoppings.end()) {
            return it->first;
        } else {
            auto const name = "__anonymous__{}"_format(hoppings.size());
            register_hopping_energy(name, energy);
            return name;
        }
    }();

    add_hopping(relative_index, from_sub, to_sub, hopping_name);
}

void Lattice::set_offset(Cartesian position) {
    if (any_of(translate_coordinates(position).array().abs() > 0.55f)) {
        throw std::logic_error("Lattice origin must not be moved by more than "
                               "half the length of a primitive lattice vector.");
    }
    offset = position;
}

Lattice::Sublattice const& Lattice::sublattice(std::string const& name) const {
    auto const it = sublattices.find(name);
    if (it == sublattices.end()) {
        throw std::out_of_range("There is no sublattice named '{}'"_format(name));
    }
    return it->second;
}

Lattice::Sublattice const& Lattice::sublattice(SubID id) const {
    using Pair = Sublattices::value_type;
    auto const it = std::find_if(sublattices.begin(), sublattices.end(),
                                 [&](Pair const& p) { return p.second.unique_id == id; });
    if (it == sublattices.end()) {
        throw std::out_of_range("There is no sublattice with ID = {}"_format(id.value()));
    }
    return it->second;
}

Lattice::HoppingFamily const& Lattice::hopping_family(std::string const& name) const {
    auto const it = hoppings.find(name);
    if (it == hoppings.end()) {
        throw std::out_of_range("There is no hopping named '{}'"_format(name));
    }
    return it->second;
}

Lattice::HoppingFamily const& Lattice::hopping_family(HopID id) const {
    using Pair = Hoppings::value_type;
    auto const it = std::find_if(hoppings.begin(), hoppings.end(),
                                 [&](Pair const& p) { return p.second.family_id == id; });
    if (it == hoppings.end()) {
        throw std::out_of_range("There is no hopping with ID = {}"_format(id.value()));
    }
    return it->second;
}

int Lattice::max_hoppings() const {
    auto result = idx_t{0};
    for (auto const& pair : sublattices) {
        auto const& sub = pair.second;

        // Include hoppings in onsite matrix (-1 for diagonal value which is not a hopping)
        auto num_scalar_hoppings = sub.energy.cols() - 1;

        // Conjugate term counts rows instead of columns
        for_each_term(hoppings, [&](HoppingFamily const& family, HoppingTerm const& term) {
            if (term.from == sub.unique_id) { num_scalar_hoppings += family.energy.cols(); }
            if (term.to == sub.unique_id)   { num_scalar_hoppings += family.energy.rows(); }
        });

        result = std::max(result, num_scalar_hoppings);
    }
    return static_cast<int>(result);
}

Cartesian Lattice::calc_position(Index3D index, string_view sublattice_name) const {
    auto position = offset;
    // Bravais lattice position
    for (auto i = 0, size = ndim(); i < size; ++i) {
        position += static_cast<float>(index[i]) * vectors[i];
    }
    if (!sublattice_name.empty()) {
        position += sublattice(sublattice_name).position;
    }
    return position;
}

Vector3f Lattice::translate_coordinates(Cartesian position) const {
    auto const size = ndim();
    auto const lattice_matrix = [&]{
        auto m = ColMajorMatrixX<float>(size, size);
        for (auto i = 0; i < size; ++i) {
            m.col(i) = vectors[i].head(size);
        }
        return m;
    }();

    // Solve `lattice_matrix * v = p`
    auto const& p = position.head(size);
    auto v = Vector3f(0, 0, 0);
    v.head(size) = lattice_matrix.colPivHouseholderQr().solve(p);
    return v;
}

Lattice Lattice::with_offset(Cartesian position) const {
    auto new_lattice = *this;
    new_lattice.set_offset(position);
    return new_lattice;
}

Lattice Lattice::with_min_neighbors(int number) const {
    auto new_lattice = *this;
    new_lattice.min_neighbors = number;
    return new_lattice;
}

bool Lattice::has_diagonal_terms() const {
    return std::any_of(sublattices.begin(), sublattices.end(), [](Sublattices::const_reference r) {
        return !r.second.energy.diagonal().isZero();
    });
}

bool Lattice::has_onsite_energy() const {
    return std::any_of(sublattices.begin(), sublattices.end(), [](Sublattices::const_reference r) {
        return !r.second.energy.isZero();
    });
}

OptimizedUnitCell Lattice::optimized_unit_cell() const {
    return OptimizedUnitCell(*this);
}

SiteRegistry Lattice::site_registry() const {
    auto const num_unique_subs = std::count_if(
        sublattices.begin(), sublattices.end(),
        [](Sublattices::const_reference r) {
            return SubID(r.second.alias_id) == r.second.unique_id;
        }
    );

    auto energies = std::vector<MatrixXcd>(num_unique_subs);
    auto names = std::vector<std::string>(num_unique_subs);
    for (auto const& pair : sublattices) {
        auto const& sub = pair.second;
        if (SubID(sub.alias_id) != sub.unique_id) { continue; }

        energies.at(sub.unique_id.value()) = sub.energy;
        names.at(sub.unique_id.value()) = pair.first;
    }
    return {energies, names};
}

HoppingRegistry Lattice::hopping_registry() const {
    auto energies = std::vector<MatrixXcd>(hoppings.size());
    auto names = std::vector<std::string>(hoppings.size());
    for (auto const& pair : hoppings) {
        auto const& hop = pair.second;
        energies.at(hop.family_id.value()) = hop.energy;
        names.at(hop.family_id.value()) = pair.first;
    }
    return {energies, names};
}

SubID Lattice::make_unique_sublattice_id(string_view name) {
    if (name.empty()) { throw std::logic_error("Sublattice name can't be blank"); }

    if (sublattices.find(name) != sublattices.end()) {
        throw std::logic_error("Sublattice '" + name + "' already exists");
    }

    return SubID(sublattices.size());
}

OptimizedUnitCell::OptimizedUnitCell(Lattice const& lattice) : sites(lattice.nsub()) {
    // Populate sites in ascending unique_id order
    for (auto const& pair : lattice.get_sublattices()) {
        auto const& sub = pair.second;
        auto const idx = sub.unique_id.as<size_t>();
        sites[idx] = {sub.position, /*norb*/static_cast<storage_idx_t>(sub.energy.cols()),
                      sub.unique_id, sub.alias_id, /*hoppings*/{}};
    }

    // Sites with equal `alias_id` will be merged in the final system. Stable sort
    // ensures that the ascending unique_id order is preserved within alias groups.
    std::stable_sort(sites.begin(), sites.end(), [](Site const& a, Site const& b) {
        return a.alias_id < b.alias_id;
    });

    // Sort by number of orbitals, but make sure alias ordering from the previous step
    // is preserved (stable sort). Aliases have the same `norb` so these sites will
    // remain as consecutive elements in the final sorted vector.
    std::stable_sort(sites.begin(), sites.end(), [](Site const& a, Site const& b) {
        return a.norb < b.norb;
    });

    // Find the index in `sites` of a site with the given unique ID
    auto find_index = [&](SubID unique_id) {
        auto const it = std::find_if(sites.begin(), sites.end(), [&](Site const& s) {
            return s.unique_id == unique_id;
        });
        assert(it != sites.end());
        return static_cast<storage_idx_t>(it - sites.begin());
    };

    for_each_term(lattice.get_hoppings(), [&](Lattice::HoppingFamily const& hopping_family,
                                              Lattice::HoppingTerm const& term) {
        auto const idx1 = find_index(term.from);
        auto const idx2 = find_index(term.to);
        auto const id = hopping_family.family_id;

        // The other sublattice has an opposite relative index (conjugate)
        sites[idx1].hoppings.push_back({ term.relative_index, idx2, id, /*is_conjugate*/false});
        sites[idx2].hoppings.push_back({-term.relative_index, idx1, id, /*is_conjugate*/true });
    });
}

} // namespace cpb