#include "TetraQualityUtils.hpp"

#include "AMIPS.h"
#include "EnergyHarmonicTet.hpp"
#include "Logger.hpp"
#include "spdlog/sinks/ansicolor_sink.h"

#include <igl/doublearea.h>
#include <spdlog/fmt/ostr.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <array>

#include <igl/point_simplex_squared_distance.h>

std::array<size_t, 4> wmtk::orient_preserve_tet_reorder(
    const std::array<size_t, 4>& conn,
    size_t v0)
{
    auto id_in_array = [](const auto& arr, auto a) {
        for (auto i = 0; i < arr.size(); i++)
            if (arr[i] == a) return i;
        return -1;
    };
    auto vl_id = id_in_array(conn, v0);
    assert(vl_id != -1);
    auto reorder = std::array<std::array<size_t, 4>, 4>{
        {{{0, 1, 2, 3}}, {{1, 0, 3, 2}}, {{2, 0, 1, 3}}, {{3, 1, 0, 2}}}};
    auto newconn = conn;
    for (auto j = 0; j < 4; j++) newconn[j] = conn[reorder[vl_id][j]];
    return newconn;
}

auto newton_direction = [](auto& compute_energy,
                           auto& compute_jacobian,
                           auto& compute_hessian,
                           auto& assembles,
                           const Eigen::Vector3d& pos) -> Eigen::Vector3d {
    auto total_energy = 0.;
    Eigen::Vector3d total_jac = Eigen::Vector3d::Zero();
    Eigen::Matrix3d total_hess = Eigen::Matrix3d::Zero();

    // E = \sum_i E_i(x)
    // J = \sum_i J_i(x)
    // H = \sum_i H_i(x)
    auto local_id = 0;
    for (auto& T : assembles) {
        for (auto j = 0; j < 3; j++) {
            T[j] = pos[j]; // only filling the front point.
        }
        auto jac = decltype(total_jac)();
        auto hess = decltype(total_hess)();
        total_energy += compute_energy(T);
        compute_jacobian(T, jac);
        compute_hessian(T, hess);
        total_jac += jac;
        total_hess += hess;
        assert(!std::isnan(total_energy));
    }
    Eigen::Vector3d x = total_hess.ldlt().solve(total_jac);
    wmtk::logger().trace("energy {}", total_energy);
    if (total_jac.isApprox(total_hess * x)) // a hacky PSD trick. TODO: change this.
        return -x;
    else {
        wmtk::logger().trace("gradient descent instead.");
        return -total_jac;
    }
};

auto gradient_direction = [](auto& compute_energy,
                             auto& compute_jacobian,
                             auto& assembles,
                             const Eigen::Vector3d& pos) -> Eigen::Vector3d {
    Eigen::Vector3d total_jac = Eigen::Vector3d::Zero();

    for (auto& T : assembles) {
        for (auto j = 0; j < 3; j++) {
            T[j] = pos[j]; // only filling the front point.
        }
        auto jac = decltype(total_jac)();
        compute_jacobian(T, jac);
        total_jac += jac;
    }
    return total_jac;
};

auto linesearch = [](auto&& energy_from_point,
                     const Eigen::Vector3d& pos,
                     const Eigen::Vector3d& dir,
                     const int& max_iter) {
    auto lr = 0.5;
    auto old_energy = energy_from_point(pos);
    wmtk::logger().trace("old energy {} dir {}", old_energy, dir.transpose());
    for (auto iter = 1; iter <= max_iter; iter++) {
        Eigen::Vector3d newpos = pos + std::pow(lr, iter) * dir;
        wmtk::logger().trace("pos {}, dir {}, [{}]", pos.transpose(), dir.transpose(), std::pow(lr, iter));
        auto new_energy = energy_from_point(newpos);
        wmtk::logger().trace("iter {}, E= {}, [{}]", iter, new_energy, newpos.transpose());
        if (new_energy < old_energy) return newpos; // TODO: armijo conditions.
    }
    return pos;
};

Eigen::Vector3d wmtk::newton_method_from_stack(
    std::vector<std::array<double, 12>>& assembles,
    std::function<double(const std::array<double, 12>&)> compute_energy,
    std::function<void(const std::array<double, 12>&, Eigen::Vector3d&)> compute_jacobian,
    std::function<void(const std::array<double, 12>&, Eigen::Matrix3d&)> compute_hessian)
{
    assert(!assembles.empty());
    auto& T = assembles.front();
    Eigen::Vector3d old_pos(T[0], T[1], T[2]);

    auto energy_from_point = [&assembles, &compute_energy](const Eigen::Vector3d& pos) -> double {
        auto total_energy = 0.;
        for (auto& T : assembles) {
            for (auto j = 0; j < 3; j++) {
                T[j] = pos[j]; // only filling the front point x,y,z.
            }
            total_energy += compute_energy(T);
        }
        return total_energy;
    };

    auto compute_new_valid_pos =
        [&energy_from_point, &assembles, &compute_energy, &compute_jacobian, &compute_hessian](
            const Eigen::Vector3d& old_pos) {
            auto current_pos = old_pos;
            auto line_search_iters = 12;
            auto newton_iters = 10;
            for (auto iter = 0; iter < newton_iters; iter++) {
                auto dir = newton_direction(
                    compute_energy,
                    compute_jacobian,
                    compute_hessian,
                    assembles,
                    current_pos);
                auto newpos = linesearch(energy_from_point, current_pos, dir, line_search_iters);
                if ((newpos - current_pos).norm() < 1e-9) // barely moves
                {
                    break;
                }
                current_pos = newpos;
            }
            return current_pos;
        };
    return compute_new_valid_pos(old_pos);
}

Eigen::Vector3d wmtk::gradient_descent_from_stack(
    std::vector<std::array<double, 12>>& assembles,
    std::function<double(const std::array<double, 12>&)> compute_energy,
    std::function<void(const std::array<double, 12>&, Eigen::Vector3d&)> compute_jacobian)
{
    assert(!assembles.empty());
    auto& T = assembles.front();
    Eigen::Vector3d old_pos(T[0], T[1], T[2]);

    auto energy_from_point = [&assembles, &compute_energy](const Eigen::Vector3d& pos) -> double {
        auto total_energy = 0.;
        for (auto& T : assembles) {
            for (auto j = 0; j < 3; j++) {
                T[j] = pos[j]; // only filling the front point x,y,z.
            }
            total_energy += compute_energy(T);
        }
        return total_energy;
    };

    auto compute_new_valid_pos = [&energy_from_point,
                                  &assembles,
                                  &compute_energy,
                                  &compute_jacobian](const Eigen::Vector3d& old_pos) {
        auto current_pos = old_pos;
        auto line_search_iters = 12;
        auto newton_iters = 10;
        for (auto iter = 0; iter < newton_iters; iter++) {
            Eigen::Vector3d dir =
                -gradient_direction(compute_energy, compute_jacobian, assembles, current_pos);
            dir.normalize(); // HACK: TODO: should use flip_avoid_line_search.
            auto newpos = linesearch(energy_from_point, current_pos, dir, line_search_iters);
            if ((newpos - current_pos).norm() < 1e-9) // barely moves
            {
                break;
            }
            current_pos = newpos;
        }
        return current_pos;
    };
    return compute_new_valid_pos(old_pos);
}

double wmtk::harmonic_energy(const Eigen::MatrixXd& verts)
{
    assert(verts.rows() == 4 && verts.cols() == 3);
    auto T = std::array<double, 12>();
    for (auto i = 0; i < 4; i++)
        for (auto j = 0; j < 3; j++) {
            T[i * 3 + j] = verts(i, j);
        }
    return harmonic_tet_energy(T);
}

std::optional<Eigen::Vector3d> wmtk::try_project(
    const Eigen::Vector3d& point,
    const std::vector<std::array<double, 12>>& assembled_neighbor)
{
    auto min_dist = std::numeric_limits<double>::infinity();
    Eigen::Vector3d closest_point = Eigen::Vector3d::Zero();
    for (const auto& tri : assembled_neighbor) {
        auto V = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(tri.data());
        Eigen::Vector3d project;
        auto dist2 = -1;
        igl::point_simplex_squared_distance<3>(
            point,
            V,
            Eigen::RowVector3i(0, 1, 2),
            0,
            dist2,
            project);
        // TODO: libigl might not be robust how to verify this?
        if (dist2 < min_dist) {
            min_dist = dist2;
            closest_point = project;
        }
    }
    return closest_point;
}


#pragma once

// Armadillo
#include <armadillo>

namespace mant {
  arma::uword polynomialSize(
      const arma::uword numberOfIndeterminates,
      const arma::uword highestDegree);

  arma::vec polynomial(
      const arma::vec& parameter,
      const arma::uword highestDegree);
}


///////////////////////////////////////////////////////////////////////////////
// cells_count.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_CELLS_CELLS_COUNT_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_CELLS_COUNT_HPP_ER_2010
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/traits.hpp>

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

    // The total number of cells is the cross product of the number of levels
    // for each variable. For example, if there are 3 rows and 4 columns,
    // there are 3 x 4 = 12 cells.
    template<typename A,typename N>
    struct count{

        typedef N size_;
        typedef void result_type;

        count(const A& acc,size_& n)
        :acc_( acc ),n_( n ){}

        template<typename Key>
        void operator()(const boost::mpl::detail::wrapper<Key>& u){
            this->n_ *= contingency_table::levels<Key>( this->acc_ ).size();
        }

        const A& acc_;
        N& n_;
    };

}// cells_aux
namespace result_of{

    template<typename Keys,typename AccSet>
    struct cells_count : contingency_table::cells_traits::size_of<
            Keys,AccSet>{};

}// result_of

  	template<typename Keys,typename AccSet>
    typename contingency_table::result_of::cells_count<Keys,AccSet>::type
    cells_count(AccSet const& acc)
    {
        namespace ns = statistics::detail;
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        typedef typename contingency_table::result_of::cells_count<
            Keys,AccSet>::type size_;
        typedef contingency_table::cells_aux::count<AccSet,size_> fun_;
        size_ n = 1;
        boost::mpl::for_each<Keys,op_>( fun_( acc, n ) );
        return n;
    }

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

#endif


/*
 * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include "QuadProgSolver.h"
#include "doctest.h"
#include "systems.h"
#ifdef EIGEN_QLD_FOUND
#include "QLDSolver.h"
#endif
#ifdef EIGEN_LSSOL_FOUND
#include "LSSOLSolver.h"
#endif
#ifdef EIGEN_GUROBI_FOUND
#include "GUROBISolver.h"
#endif
#ifdef EIGEN_OSQP_FOUND
#include "OSQPSolver.h"
#endif

#include <Eigen/Core>
#include <iostream>
#include <numeric>

TEST_CASE_FIXTURE(Problem, "QuadProgTest")
{
    copra::QuadProgDenseSolver qpQuadProg;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    REQUIRE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    REQUIRE_EQ(qpQuadProg.SI_fail(), 0);
}

#ifdef EIGEN_QLD_FOUND
TEST_CASE_FIXTURE(Problem, "QLDOnQuadProgTest")
{
    copra::QLDSolver qpQLD;
    copra::QuadProgDenseSolver qpQuadProg;

    qpQLD.SI_problem(nrvars, nreqs, nrineqs);
    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    REQUIRE(qpQLD.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    REQUIRE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQLD = qpQLD.SI_result();
    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    CHECK(resQuadProg.isApprox(resQLD));
    REQUIRE_EQ(qpQLD.SI_fail(), 0);
    REQUIRE_EQ(qpQuadProg.SI_fail(), 0);
}
#endif

#ifdef EIGEN_LSSOL_FOUND
TEST_CASE_FIXTURE(Problem, "LSSOLOnQuadProgTest")
{
    copra::QuadProgDenseSolver qpQuadProg;
    copra::LSSOLSolver qpLSSOL;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    qpLSSOL.SI_problem(nrvars, nreqs, nrineqs);
    REQUIRE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    REQUIRE(qpLSSOL.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resLSSOL = qpLSSOL.SI_result();
    CHECK(resLSSOL.isApprox(resQuadProg));
    REQUIRE_EQ(qpLSSOL.SI_fail(), 0);
}
#endif

#ifdef EIGEN_GUROBI_FOUND
TEST_CASE_FIXTURE(Problem, "GUROBIOnQuadProgTest")
{
    copra::QuadProgDenseSolver qpQuadProg;
    copra::GUROBISolver qpGUROBI;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    qpGUROBI.SI_problem(nrvars, nreqs, nrineqs);
    REQUIRE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    REQUIRE(qpGUROBI.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resGUROBI = qpGUROBI.SI_result();
    CHECK(resGUROBI.isApprox(resQuadProg, 1e-6));
    REQUIRE_EQ(qpGUROBI.SI_fail(), GRB_OPTIMAL);
}
#endif

#ifdef EIGEN_OSQP_FOUND
TEST_CASE_FIXTURE(Problem, "OSQPOnQuadProgTest")
{
    copra::QuadProgDenseSolver qpQuadProg;
    copra::OSQPSolver osqp;

    qpQuadProg.SI_problem(nrvars, nreqs, nrineqs);
    osqp.SI_problem(nrvars, nreqs, nrineqs);
    REQUIRE(qpQuadProg.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));
    REQUIRE(osqp.SI_solve(Q, c, Aeq, beq, Aineq, bineq, XL, XU));

    Eigen::VectorXd resQuadProg = qpQuadProg.SI_result();
    Eigen::VectorXd resOSQP = osqp.SI_result();
    CHECK(resOSQP.isApprox(resQuadProg, 1e-6));
    REQUIRE_EQ(osqp.SI_fail(), 1);
}
#endif


#ifndef CUAUV_SIM_VISION_H
#define CUAUV_SIM_VISION_H

#include <unordered_set>
#include <utility>

#include <Eigen/Core>

#include "world.hpp"

namespace cuauv {
namespace fishbowl {

class camera  {
public:
    /**
     * Constructs a new selective attached pinhole camera.
     * @param w The world.
     * @param pq A pointer to the orientation quaternion of the parent object.
     * @param px A pointer to the position of the parent object.
     * @param q The orientation of the camera in the praent object frame.
     * @param x The position of the camera in the parent object frame.
     * @param f The focal length of the camera.
     */
    camera(world* w, Eigen::Quaterniond* pq, Eigen::Vector3d* px, const Eigen::Quaterniond& q, const Eigen::Vector3d& x, double f);

    Eigen::Quaterniond* pq;
    Eigen::Vector3d* px;
    Eigen::Quaterniond q;
    Eigen::Vector3d x;
    double f;

    void step();

    /**
     * @returns A tuple of the form (x, y, r, d).  (x, y) corresponds to the
     * apparent position of the target entity, r is the apparent radius of the
     * target entity, and d is the distance from the camera to the target
     * entity.  The origin is along the camera's x axis. The x values increase
     * to the right, and y values increase downwards.
     */
    std::tuple<double, double, double, double> query(entity_id id);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
    world* w;

    Eigen::Matrix<double, 3, 4> C;
};

} // namespace fishbowl
} // namespace cuauv

#endif // CUAUV_SIM_VISION_H


// Copyright (c) 2021 OUXT Polaris
//
// 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 <quaternion_operation/quaternion_operation.h>

#include <algorithm>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <iostream>
#include <navi_sim/primitives/primitive.hpp>
#include <string>
#include <vector>

namespace navi_sim
{
Primitive::Primitive(
  std::string primitive_type, std::string object_type, geometry_msgs::msg::Pose pose)
: primitive_type(primitive_type), object_type(object_type), pose(pose)
{
}

std::vector<geometry_msgs::msg::Point> Primitive::get2DPolygon() const
{
  namespace bg = boost::geometry;

  typedef bg::model::d2::point_xy<double> point;
  typedef bg::model::polygon<point> polygon;

  std::vector<geometry_msgs::msg::Point> ret;
  polygon poly;
  for (auto & v : vertices_) {
    const auto v_transformed = transform(v);
    bg::append(poly.outer(), point(v_transformed.x, v_transformed.y));
  }
  polygon hull;
  bg::convex_hull(poly, hull);
  for (const auto & p : hull.outer()) {
    geometry_msgs::msg::Point point;
    point.x = p.x();
    point.y = p.y();
    point.z = 0;
    ret.emplace_back(point);
  }
  return ret;
}

Vertex Primitive::transform(Vertex v) const
{
  auto mat = quaternion_operation::getRotationMatrix(pose.orientation);
  Eigen::VectorXd point(3);
  point(0) = v.x;
  point(1) = v.y;
  point(2) = v.z;
  point = mat * point;
  point(0) = point(0) + pose.position.x;
  point(1) = point(1) + pose.position.y;
  point(2) = point(2) + pose.position.z;
  Vertex ret;
  ret.x = point(0);
  ret.y = point(1);
  ret.z = point(2);
  return ret;
}

Vertex Primitive::transform(Vertex v, const geometry_msgs::msg::Pose & sensor_pose) const
{
  v = transform(v);
  const auto inv_quat = quaternion_operation::conjugate(sensor_pose.orientation);
  const auto mat = quaternion_operation::getRotationMatrix(inv_quat);
  Eigen::VectorXd point(3);
  point(0) = v.x;
  point(1) = v.y;
  point(2) = v.z;
  point = mat * point;
  point(0) = point(0) - sensor_pose.position.x;
  point(1) = point(1) - sensor_pose.position.y;
  point(2) = point(2) - sensor_pose.position.z;
  Vertex ret;
  ret.x = point(0);
  ret.y = point(1);
  ret.z = point(2);
  return ret;
}

std::vector<Vertex> Primitive::transform(const geometry_msgs::msg::Pose & sensor_pose) const
{
  std::vector<Vertex> ret;
  for (auto & v : vertices_) {
    ret.emplace_back(transform(v, sensor_pose));
  }
  return ret;
}

std::vector<Vertex> Primitive::transform() const
{
  std::vector<Vertex> ret;
  for (auto & v : vertices_) {
    ret.emplace_back(transform(v));
  }
  return ret;
}

std::vector<Vertex> Primitive::getVertex() const { return transform(); }

std::vector<Vertex> Primitive::getVertex(const geometry_msgs::msg::Pose & sensor_pose) const
{
  return transform(sensor_pose);
}

std::vector<Triangle> Primitive::getTriangles() const { return triangles_; }

unsigned int Primitive::addToScene(RTCDevice device, RTCScene scene)
{
  RTCGeometry mesh = rtcNewGeometry(device, RTC_GEOMETRY_TYPE_TRIANGLE);
  const auto transformed_vertices = transform();
  Vertex * vertices = static_cast<Vertex *>(rtcSetNewGeometryBuffer(
    mesh, RTC_BUFFER_TYPE_VERTEX, 0, RTC_FORMAT_FLOAT3, sizeof(Vertex),
    transformed_vertices.size()));
  for (size_t i = 0; i < transformed_vertices.size(); i++) {
    vertices[i] = transformed_vertices[i];
  }
  Triangle * triangles = static_cast<Triangle *>(rtcSetNewGeometryBuffer(
    mesh, RTC_BUFFER_TYPE_INDEX, 0, RTC_FORMAT_UINT3, sizeof(Triangle), triangles_.size()));
  for (size_t i = 0; i < triangles_.size(); i++) {
    triangles[i] = triangles_[i];
  }
  rtcCommitGeometry(mesh);
  unsigned int geometry_id = rtcAttachGeometry(scene, mesh);
  rtcReleaseGeometry(mesh);
  return geometry_id;
}

nlohmann::json Primitive::toBaseJson() const
{
  auto j = nlohmann::json{};
  j = *this;
  return j;
}

void to_json(nlohmann::json & j, const Primitive & p)
{
  auto j_pose = nlohmann::json{};
  to_json(j_pose, p.pose);
  j = nlohmann::json{{"primitive_type", p.primitive_type}, {"object_type", p.object_type}};
  j["pose"] = j_pose;
}

void to_json(nlohmann::json & j, const geometry_msgs::msg::Point & p)
{
  j = nlohmann::json{{"x", p.x}, {"y", p.y}, {"z", p.z}};
}

void from_json(const nlohmann::json & j, geometry_msgs::msg::Point & p)
{
  j.at("x").get_to(p.x);
  j.at("y").get_to(p.y);
  j.at("z").get_to(p.z);
}

void to_json(nlohmann::json & j, const geometry_msgs::msg::Quaternion & q)
{
  j = nlohmann::json{{"x", q.x}, {"y", q.y}, {"z", q.z}, {"w", q.w}};
}

void from_json(const nlohmann::json & j, geometry_msgs::msg::Quaternion & q)
{
  j.at("x").get_to(q.x);
  j.at("y").get_to(q.y);
  j.at("z").get_to(q.z);
  j.at("w").get_to(q.w);
}

void to_json(nlohmann::json & j, const geometry_msgs::msg::Pose & p)
{
  j = nlohmann::json{};
  auto j_position = nlohmann::json{};
  to_json(j_position, p.position);
  j["position"] = j_position;
  auto j_orientation = nlohmann::json{};
  to_json(j_orientation, p.orientation);
  j["orientation"] = j_orientation;
}

void from_json(const nlohmann::json & j, geometry_msgs::msg::Pose & p)
{
  from_json(j["position"], p.position);
  from_json(j["orientation"], p.orientation);
}
}  // namespace navi_sim


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#ifndef __EIGEN_GEOMETRY__
#define __EIGEN_GEOMETRY__

#include <Eigen/Dense>

namespace VideoStitch {
namespace Calibration {

/**
@brief Create a rotation matrix from a yaw pitch roll set of parameters
@param R the output rotation matrix
@param yaw the input yaw parameter
@param pitch the input yaw parameter
@param roll the input yaw parameter
*/
void rotationFromEulerZXY(Eigen::Matrix3d& R, double yaw, double pitch, double roll);

/**
@brief Compute yaw pitch roll vector from a rotation matrix
@param vr the output 3d vector (yaw; pitch; roll)
@param R the input rotation matrix
*/
void EulerZXYFromRotation(Eigen::Vector3d& vr, const Eigen::Matrix3d& R);

}  // namespace Calibration
}  // namespace VideoStitch
#endif