// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2021 Heal Research

#ifndef OPERON_EVAL_DETAIL
#define OPERON_EVAL_DETAIL

#include "core/node.hpp"
#include "core/types.hpp"
#include "interpreter/functions.hpp"

#include "robin_hood.h"
#include <Eigen/Dense>
#include <fmt/core.h>

#include <tuple>

namespace Operon {

namespace detail {
    // this should be good enough - tests show 512 is about optimal
    template<typename T>
    struct batch_size {
        static const size_t value = 512 / sizeof(T);
    };

    template<typename T>
    using eigen_t = typename Eigen::Array<T, batch_size<T>::value, Eigen::Dynamic, Eigen::ColMajor>;

    template<typename T>
    using eigen_ref = Eigen::Ref<eigen_t<T>, Eigen::Unaligned, Eigen::Stride<batch_size<T>::value, 1>>;

    // dispatching mechanism
    // compared to the simple/naive way of evaluating n-ary symbols, this method has the following advantages:
    // 1) improved performance: the naive method accumulates into the result for each argument, leading to unnecessary assignments
    // 2) minimizing the number of intermediate steps which might improve floating point accuracy of some operations
    //    if arity > 4, one accumulation is performed every 4 args
    template<NodeType Type, typename T>
    inline void dispatch_op_nary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Aq);
        auto result = m.col(parentIndex);
        const auto f = [](bool cont, decltype(result) res, auto&&... args) {
            if (cont) {
                ContinuedFunction<Type>{}(res, std::forward<decltype(args)>(args)...);
            } else {
                Function<Type>{}(res, std::forward<decltype(args)>(args)...);
            }
        };
        const auto nextArg = [&](size_t i) { return i - (nodes[i].Length + 1); };

        auto arg1 = parentIndex - 1;

        bool continued = false;

        int arity = nodes[parentIndex].Arity;
        while (arity > 0) {
            switch (arity) {
            case 1: {
                f(continued, result, m.col(arg1));
                arity = 0;
                break;
            }
            case 2: {
                auto arg2 = nextArg(arg1);
                f(continued, result, m.col(arg1), m.col(arg2));
                arity = 0;
                break;
            }
            case 3: {
                auto arg2 = nextArg(arg1), arg3 = nextArg(arg2);
                f(continued, result, m.col(arg1), m.col(arg2), m.col(arg3));
                arity = 0;
                break;
            }
            default: {
                auto arg2 = nextArg(arg1), arg3 = nextArg(arg2), arg4 = nextArg(arg3);
                f(continued, result, m.col(arg1), m.col(arg2), m.col(arg3), m.col(arg4));
                arity -= 4;
                arg1 = nextArg(arg4);
                break;
            }
            }
            continued = true;
        }
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_unary(eigen_t<T>& m, Operon::Vector<Node> const&, size_t i, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Constant && Type > NodeType::Pow);
        Function<Type>{}(m.col(i), m.col(i - 1));
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_binary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t i, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Log && Type > NodeType::Div);
        auto j = i - 1;
        auto k = j - nodes[j].Length - 1;
        Function<Type>{}(m.col(i), m.col(j), m.col(k));
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_simple_unary_or_binary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        auto r = m.col(parentIndex);
        size_t i = parentIndex - 1;
        size_t arity = nodes[parentIndex].Arity;

        Function<Type> f{};

        if (arity == 1) {
            f(r, m.col(i));
        } else {
            auto j = i - (nodes[i].Length + 1);
            f(r, m.col(i), m.col(j));
        }
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_simple_nary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        auto r = m.col(parentIndex);
        size_t arity = nodes[parentIndex].Arity;

        auto i = parentIndex - 1;

        Function<Type> f{};

        if (arity == 1) {
            f(r, m.col(i));
        } else {
            r = m.col(i);

            for (size_t k = 1; k < arity; ++k) {
                i -= nodes[i].Length + 1;
                f(r, m.col(i));
            }
        }
    }

    struct noop {
        template<typename... Args>
        void operator()(Args&&...) {}
    };

    template<typename X, typename Tuple>
    class tuple_index;

    template<typename X, typename... T>
    class tuple_index<X, std::tuple<T...>> {
        template<std::size_t... idx>
        static constexpr ssize_t find_idx(std::index_sequence<idx...>)
        {
            return -1 + ((std::is_same<X, T>::value ? idx + 1 : 0) + ...);
        }

    public:
        static constexpr ssize_t value = find_idx(std::index_sequence_for<T...>{});
    };

    template<typename T>
    using Callable = typename std::function<void(detail::eigen_t<T>&, Operon::Vector<Node> const&, size_t, size_t)>;

    template<NodeType Type, typename T>
    static constexpr Callable<T> MakeCall()
    {
        if constexpr (Type < NodeType::Aq) { // nary: add, sub, mul, div
            return Callable<T>(detail::dispatch_op_nary<Type, T>);
        } else if constexpr (Type < NodeType::Log) { // binary: aq, pow
            return Callable<T>(detail::dispatch_op_binary<Type, T>);
        } else if constexpr (Type < NodeType::Constant) { // unary: exp, log, sin, cos, tan, tanh, sqrt, cbrt, square, dynamic
            return Callable<T>(detail::dispatch_op_unary<Type, T>);
        }
    }

    template<NodeType Type, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0, bool> = true>
    static constexpr auto MakeTuple()
    {
        return std::tuple(MakeCall<Type, Ts>()...);
    };

    template<typename F, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0 && (std::is_invocable_r_v<void, F, detail::eigen_t<Ts>&, Vector<Node> const&, size_t, size_t> && ...), bool> = true>
    static constexpr auto MakeTuple(F&& f)
    {
        return std::tuple(Callable<Ts>(std::forward<F&&>(f))...);
    }

    template<typename F, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0 && (std::is_invocable_r_v<void, F, detail::eigen_t<Ts>&, Vector<Node> const&, size_t, size_t> && ...), bool> = true>
    static constexpr auto MakeTuple(F const& f)
    {
        return std::tuple(Callable<Ts>(f)...);
    }

    template<NodeType Type>
    static constexpr auto MakeDefaultTuple()
    {
        return MakeTuple<Type, Operon::Scalar, Operon::Dual>();
    }

    template<typename F, std::enable_if_t<
        std::is_invocable_r_v<
            void, F, detail::eigen_t<Operon::Scalar>&, Operon::Vector<Node> const&, size_t, size_t
        > &&
        std::is_invocable_r_v<
            void, F, detail::eigen_t<Operon::Dual>&, Operon::Vector<Node> const&, size_t, size_t
        >, bool> = true>
    static constexpr auto MakeDefaultTuple(F&& f)
    {
        return MakeTuple<F, Operon::Scalar, Operon::Dual>(std::forward<F&&>(f));
    }

} // namespace detail

struct DispatchTable {
    template<typename T>
    using Callable = detail::Callable<T>;

    using Tuple    = std::tuple<Callable<Operon::Scalar>, Callable<Operon::Dual>>;
    using Map      = robin_hood::unordered_flat_map<Operon::Hash, Tuple>;
    using Pair     = robin_hood::pair<Operon::Hash, Tuple>;

    DispatchTable()
    {
        InitializeMap();
    }

    DispatchTable(DispatchTable const& other) : map(other.map) { }
    DispatchTable(DispatchTable &&other) : map(std::move(other.map)) { }

    void InitializeMap()
    {
        const auto hash = [](auto t) { return Node(t).HashValue; };

        map = Map{
            { hash(NodeType::Add), detail::MakeDefaultTuple<NodeType::Add>() },
            { hash(NodeType::Sub), detail::MakeDefaultTuple<NodeType::Sub>() },
            { hash(NodeType::Mul), detail::MakeDefaultTuple<NodeType::Mul>() },
            { hash(NodeType::Sub), detail::MakeDefaultTuple<NodeType::Sub>() },
            { hash(NodeType::Div), detail::MakeDefaultTuple<NodeType::Div>() },
            { hash(NodeType::Aq),  detail::MakeDefaultTuple<NodeType::Aq>() },
            { hash(NodeType::Pow), detail::MakeDefaultTuple<NodeType::Pow>() },
            { hash(NodeType::Log), detail::MakeDefaultTuple<NodeType::Log>() },
            { hash(NodeType::Exp), detail::MakeDefaultTuple<NodeType::Exp>() },
            { hash(NodeType::Sin), detail::MakeDefaultTuple<NodeType::Sin>() },
            { hash(NodeType::Cos), detail::MakeDefaultTuple<NodeType::Cos>() },
            { hash(NodeType::Tan), detail::MakeDefaultTuple<NodeType::Tan>() },
            { hash(NodeType::Tanh), detail::MakeDefaultTuple<NodeType::Tanh>() },
            { hash(NodeType::Sqrt), detail::MakeDefaultTuple<NodeType::Sqrt>() },
            { hash(NodeType::Cbrt), detail::MakeDefaultTuple<NodeType::Cbrt>() },
            { hash(NodeType::Square), detail::MakeDefaultTuple<NodeType::Square>() },
            /* constants and variables not needed here */
        };
    };

    template<typename T>
    inline Callable<T>& Get(Operon::Hash const h)
    {
        constexpr ssize_t idx = detail::tuple_index<Callable<T>, Tuple>::value;
        static_assert(idx >= 0, "Tuple does not contain type T");
        if (auto it = map.find(h); it != map.end()) {
            return std::get<static_cast<size_t>(idx)>(it->second);
        }
        throw std::runtime_error(fmt::format("Hash value {} is not in the map\n", h));
    }

    template<typename T>
    inline Callable<T> const& Get(Operon::Hash const h) const
    {
        constexpr ssize_t idx = detail::tuple_index<Callable<T>, Tuple>::value;
        static_assert(idx >= 0, "Tuple does not contain type T");
        if (auto it = map.find(h); it != map.end()) {
            return std::get<static_cast<size_t>(idx)>(it->second);
        }
        throw std::runtime_error(fmt::format("Hash value {} is not in the map\n", h));
    }

    template<typename F, std::enable_if_t<std::is_invocable_r_v<void, F, detail::eigen_t<Operon::Dual>&, Vector<Node> const&, size_t, size_t>, bool> = true>
    void RegisterCallable(Operon::Hash hash, F const& f) {
        map[hash] = detail::MakeTuple<F, Operon::Scalar, Operon::Dual>(f);
    }

private:
    Map map;
};

} // namespace Operon

#endif


#include "problems/mga_1dsm_transx.h"

#include <keplerian_toolbox/core_functions/array3D_operations.h>
#include <keplerian_toolbox/core_functions/fb_vel.h>
#include <keplerian_toolbox/core_functions/propagate_lagrangian.h>
#include <keplerian_toolbox/core_functions/fb_prop.h>
#include <keplerian_toolbox/lambert_problem.h>

#include <string>
#include <cmath>
#include <numeric>
#include <vector>

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

namespace pagmo { namespace problem {

int dimension_from_params(const std::vector<kep_toolbox::planet::planet_ptr> seq, const std::vector<bool> m_dsm) {
    int dim = 2;
    int n = seq.size();
    bool dsm = m_dsm.size() == 0 || (m_dsm.size() > 0 && m_dsm[0]);
    dim += (dsm ? 4 : 1);
    for (int i = 1; i < seq.size() - 1; ++i) {
        dsm = m_dsm.size() == 0 || (m_dsm.size() > i && m_dsm[i]);
        dim += (dsm ? 4 : 1);
    }
    return dim;
}

bool mga_1dsm_transx::dsm_in_leg_i(int i) const {
    return (m_dsm.size() == 0) || (m_dsm.size() > i && m_dsm[i]);
}

int mga_1dsm_transx::t0_index() const { return 0; }
int mga_1dsm_transx::tof_index() const { return 1; }
int mga_1dsm_transx::vinf_index() const { return 3; } // 3, 4

int mga_1dsm_transx::base_idx(int i) const{
    bool dsm = dsm_in_leg_i(0);
    int base = (tof_index() + 1);
    if (i == 0) return base;

    int idx = (base + (dsm ? 4 : 1));
    for (int j = 1; j < i; ++j) {
        dsm = dsm_in_leg_i(j);
        idx += (dsm ? 4 : 1);
    }
    return idx;
}
int mga_1dsm_transx::T_idx(int i) const {
    int base = base_idx(i);
    if (i == 0) return base;

    return base;
}

int mga_1dsm_transx::Beta_idx(int i) const {
    int base = base_idx(i);
    return base + 1;
}

int mga_1dsm_transx::R_idx(int i) const {
    int base = base_idx(i);
    return base + 2;
}

int mga_1dsm_transx::DSM_idx(int i) const {
    int base = base_idx(i);
    if (i == 0) return base + 4;

    return base + 3;
}

mga_1dsm_transx::mga_1dsm_transx(const std::vector<kep_toolbox::planet::planet_ptr> seq,
                    const std::vector<bool> dsm,
                    const double dep_altitude, const double arr_altitude, const bool circularize,
                    const kep_toolbox::epoch t0_l, const kep_toolbox::epoch t0_u,
                    const double tof_l, const double tof_u,
                    const double vinf_l, const double vinf_u,
                    const bool add_vinf_dep, const bool add_vinf_arr,
                    const bool multi_objective) : transx_problem(seq, dep_altitude, arr_altitude, circularize, dimension_from_params(seq, dsm), 1 + (int)multi_objective), m_add_vinf_dep(add_vinf_dep), m_add_vinf_arr(add_vinf_arr), m_dsm(dsm) {
    size_t dim(get_dimension());
    decision_vector lb(dim, 0.0), ub(dim, 0.0);

      lb[0] = t0_l.mjd2000(); ub[0] = t0_u.mjd2000();
      lb[1] = tof_l * 365.25; ub[1] = tof_u * 365.25;

      lb[2] = 1e-5; ub[2] = 1 - 1e-5;

      if (dsm_in_leg_i(0)) {
        lb[vinf_index()] = lb[vinf_index() + 1] = 0.0; ub[vinf_index()] = ub[vinf_index() + 1] = 1.0;
        lb[vinf_index() + 2] = vinf_l * 1000; ub[vinf_index() + 2] = vinf_u * 1000;
        lb[DSM_idx(0)] = 1e-5; ub[DSM_idx(0)] = 1 - 1e-5;
      }

      for (int i = 0; i < get_n_legs() - 1; ++i) {
        auto planet = seq[i + 1];

        int j = base_idx(i + 1);
        bool dsm = dsm_in_leg_i(i + 1);
        lb[j] = 1e-5;  ub[j] = 1 - 1e-5; // T[i]
        if (dsm) {

            double a = planet->compute_elements()[0];
            double soi = a * pow((planet->get_mu_self() / planet->get_mu_central_body()), 2/5);
            double soiRad = soi / planet->get_radius();

            double safeDistanceRatio = planet->get_safe_radius() / planet->get_radius();

            lb[j + 1] = -2 * boost::math::constants::pi<double>(); ub[j + 1] = 2 * boost::math::constants::pi<double>(); // Beta
            lb[j + 2] = safeDistanceRatio;   ub[j + 2] = soiRad; // Rad
            lb[j + 3] = 1e-5;  ub[j + 3] = 1 - 1e-5;
        }
      }

        for (int i = 1; i < get_n_legs() - 1; ++i) {
            bool dsm = dsm_in_leg_i(i);
            if (dsm) {
                kep_toolbox::planet::planet_ptr pl = get_seq()[i];
                lb[R_idx(i)] = pl->get_safe_radius() / pl->get_radius();
            }
        }

    set_bounds(lb, ub);
}

mga_1dsm_transx::mga_1dsm_transx(const mga_1dsm_transx &p) : transx_problem(p.get_seq(), p.get_dep_altitude(), p.get_arr_altitude(), p.get_circularize(), p.get_dimension(), p.get_f_dimension()), m_dsm(p.m_dsm), m_add_vinf_arr(p.get_add_vinf_arr()), m_add_vinf_dep(p.get_add_vinf_dep()), m_multi_obj(p.get_multi_obj()) {
  set_bounds(p.get_lb(), p.get_ub());
}

base_ptr mga_1dsm_transx::clone() const {
  return base_ptr(new mga_1dsm_transx(*this));
}

void mga_1dsm_transx::calc_objective(fitness_vector &f, const decision_vector &x, bool should_print, TransXSolution * solution) const {

  std::vector<double> T(get_n_legs());
  double alpha_sum = 0;
  for (int i = 0; i < T.size(); ++i) {
    T[i] = - log(x[T_idx(i)]);
    alpha_sum += T[i];
  }

  bool dsm = dsm_in_leg_i(0);

  kep_toolbox::array3D Vinf;
  if (dsm) {
    double theta = 2 * M_PI * x[vinf_index()];
    double phi = acos(2 * x[vinf_index() + 1] - 1) - M_PI / 2;

    double vinf = x[vinf_index() + 2];
    Vinf[0] = vinf * cos(phi) * cos(theta);
    Vinf[1] = vinf * cos(phi) * sin(theta);
    Vinf[2] = vinf * sin(phi);

    }

  for (int i = 0; i < T.size(); ++i) {
    T[i] = x[tof_index()] * T[i] / alpha_sum;
  }

  int n = get_seq().size();
  std::vector<kep_toolbox::epoch> t_P(n);
  std::vector<kep_toolbox::array3D> r_P(n);
  std::vector<kep_toolbox::array3D> v_P(n);
  std::vector<double> DV(n + 1, 0.0);

  for (int i = 0; i < n; ++i) {
    kep_toolbox::planet::planet_ptr planet = get_seq()[i];
    t_P[i] = kep_toolbox::epoch(x[t0_index()] + std::accumulate(T.begin(), T.begin() + i, 0.0));
    planet->eph(t_P[i], r_P[i], v_P[i]);
  }

  if (should_print) {
    transx_time_info(solution->mutable_times(), get_seq(), t_P);
  }

  kep_toolbox::array3D r, v;
  r = r_P[0];
  if (dsm) {
    if (m_add_vinf_dep) {
        DV[0] += burn_cost(get_seq()[0], Vinf, false, true);
    }
    if (should_print) {
        transx_escape(solution->mutable_escape(), get_seq()[0], v_P[0], r_P[0], Vinf, t_P[0].mjd());
    }

    kep_toolbox::array3D v0;
    kep_toolbox::sum(v0, v_P[0], Vinf);
    v = v0;
    kep_toolbox::propagate_lagrangian(r, v, (dsm ? x[DSM_idx(0)] : 0) * T[0] * ASTRO_DAY2SEC, get_common_mu());
  }

  double dt = (1 - (dsm ? x[DSM_idx(0)] : 0)) * T[0] * ASTRO_DAY2SEC;
  kep_toolbox::lambert_problem l(r, r_P[1], dt, get_common_mu());
  kep_toolbox::array3D v_end_l(l.get_v2()[0]);
  kep_toolbox::array3D v_beg_l(l.get_v1()[0]);

  if (!dsm) {
    kep_toolbox::diff(Vinf, v_beg_l, v_P[0]);
    v = Vinf;
    kep_toolbox::sum(v, v_P[0], Vinf);

    if (m_add_vinf_dep) {
        DV[0] += burn_cost(get_seq()[0], Vinf, false, true);
    }
    if (should_print) {
        transx_escape(solution->mutable_escape(), get_seq()[0], v_P[0], r_P[0], Vinf, t_P[0].mjd());
    }
  }

  kep_toolbox::array3D deltaV;
  kep_toolbox::diff(deltaV, v_beg_l, v);
  DV[0] += kep_toolbox::norm(deltaV);

  if (dsm && should_print) {
    transx_dsm(solution->add_dsms(), v, r, deltaV, v_beg_l, t_P[0].mjd() + T[0] - dt / ASTRO_DAY2SEC, 0);
  }

  for (int i = 1; i < n - 1; ++i) {
    dsm = dsm_in_leg_i(i);

    kep_toolbox::array3D v_rel_in, v_rel_out;
    if (dsm) {
        double radius = x[R_idx(i)] * get_seq()[i]->get_radius();
        double beta = x[Beta_idx(i)];
        kep_toolbox::array3D v_out;
        kep_toolbox::fb_prop(v_out, v_end_l, v_P[i], radius, beta, get_seq()[i]->get_mu_self());

        kep_toolbox::diff(v_rel_in, v_end_l, v_P[i]);
        kep_toolbox::diff(v_rel_out, v_out, v_P[i]);

        if (should_print) {
            transx_flyby(solution->add_flybyes(), get_seq()[i], v_P[i], r_P[i], v_rel_in, v_rel_out, t_P[i].mjd());
        }

        r = r_P[i]; v = v_out;

        kep_toolbox::propagate_lagrangian(r, v, (dsm ? x[DSM_idx(i)] : 0) * T[i] * ASTRO_DAY2SEC, get_common_mu());
    } else {
        r = r_P[i]; v = v_end_l;
    }

    dt = (1 - (dsm ? x[DSM_idx(i)] : 0)) * T[i] * ASTRO_DAY2SEC;
    kep_toolbox::lambert_problem l2(r, r_P[i + 1], dt, get_common_mu());
    v_beg_l = l2.get_v1()[0];
    v_end_l = l2.get_v2()[0];

    if (dsm) {
        kep_toolbox::diff(deltaV, v_beg_l, v);
        DV[i] = kep_toolbox::norm(deltaV);

        if (should_print) {
            transx_dsm(solution->add_dsms(), v, r, deltaV, v_beg_l, t_P[i].mjd() + T[i] - dt / ASTRO_DAY2SEC, 1);
        }
    } else {
        kep_toolbox::diff(v_rel_in, v, v_P[i]);
        kep_toolbox::diff(v_rel_out, v_beg_l, v_P[i]);

        kep_toolbox::planet::planet_ptr planet = get_seq()[i];
        kep_toolbox::fb_vel(DV[i], v_rel_in, v_rel_out, *planet);

        double ta  = acos(kep_toolbox::dot(v_rel_in, v_rel_out)/sqrt(kep_toolbox::dot(v_rel_in,v_rel_in))/sqrt(kep_toolbox::dot(v_rel_out,v_rel_out)));
        double alt = (planet->get_mu_self() / kep_toolbox::dot(v_rel_in,v_rel_in)*(1/sin(ta/2)-1) - planet->get_radius())/1000;
        if (alt > planet->get_safe_radius()) {
            f[0] = DBL_MAX;
            return;
        }

        if (should_print) {
            transx_flyby(solution->add_flybyes(), planet, v_P[i], r_P[i], v_rel_in, v_rel_out, t_P[i].mjd());
        }
    }

  }

  kep_toolbox::array3D Vexc_arr(v_end_l);
  kep_toolbox::diff(Vexc_arr, v_end_l, v_P[v_P.size() - 1]);
  if (m_add_vinf_arr) {
    DV[DV.size() - 1] += burn_cost(get_seq()[get_seq().size() - 1], Vexc_arr, true, get_circularize());
  }
  if (should_print) {
    transx_arrival(solution->mutable_arrival(), get_seq()[get_seq().size() - 1], Vexc_arr, t_P[t_P.size() - 1].mjd());
  }

  double sumDeltaV = std::accumulate(DV.begin(), DV.end(), 0.0);
  double sumT = std::accumulate(T.begin(), T.end(), 0.0);

  if (should_print) {
    solution->set_fuel_cost(sumDeltaV);
  }

  f[0] = sumDeltaV;
  if (get_f_dimension() == 2) {
    f[1] = sumT;
  }

}

std::string mga_1dsm_transx::get_name() const {
  return "MGA-1DSM";
}

}} // namespaces

BOOST_CLASS_EXPORT_IMPLEMENT(pagmo::problem::mga_1dsm_transx)


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <string>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include "genfile/Error.hpp"
#include "genfile/ToGP.hpp"
#include "genfile/VariantDataReader.hpp"
#include "metro/likelihood/Multinomial.hpp"
#include "components/SNPSummaryComponent/SNPHWE.hpp"
#include "components/SNPSummaryComponent/SNPSummaryComputation.hpp"
#include "components/SNPSummaryComponent/HWEComputation.hpp"
#include "components/SNPSummaryComponent/IntensitySummaryComputation.hpp"
#include "components/SNPSummaryComponent/ClusterFitComputation.hpp"
#include "components/SNPSummaryComponent/InfoComputation.hpp"

// #define DEBUG_SNP_SUMMARY_COMPUTATION 1

namespace stats {
	namespace {
		struct AlleleCountClient: public genfile::VariantDataReader::PerSampleSetter {
			AlleleCountClient( std::vector< double >* counts ):
				m_counts( counts ),
				m_number_of_alleles( 0 )
			{
				assert( counts != 0 ) ;
			}

			~AlleleCountClient() throw() {}

			void set_counts( std::vector< double >* counts ) {
				assert( counts != 0 ) ;
				m_counts = counts ;
			}

			void initialise( std::size_t number_of_samples, std::size_t number_of_alleles ) {
				m_number_of_alleles = number_of_alleles ;
				assert( m_counts->size() == number_of_alleles ) ;
			}

			bool set_sample( std::size_t i ) {
				m_ploidy = 0 ;
				m_table = 0 ;
				return true ;
			}

			void set_number_of_entries( uint32_t ploidy, std::size_t n, OrderType const order_type, ValueType const value_type ) {
				assert( order_type == genfile::ePerUnorderedGenotype ) ;
				assert( value_type == genfile::eProbability ) ;

				std::map< uint32_t, genfile::impl::Enumeration >::iterator where = m_tables.find( ploidy ) ;
				if( where == m_tables.end() ) {
					std::pair< std::map< uint32_t, genfile::impl::Enumeration >::iterator, bool >
						result = m_tables.insert( std::make_pair( ploidy, genfile::impl::enumerate_unphased_genotypes( ploidy ) )) ;
					assert( result.second ) ;
					where = result.first ;
				}
				m_ploidy = ploidy ;
				m_table = &(where->second) ;
			}

			void set_value( std::size_t value_i, genfile::MissingValue const value ) {
				set_value( value_i, 0.0 ) ;
			}

			void set_value( std::size_t value_i, double const value ) {
				assert( value_i <= std::size_t( std::numeric_limits< uint16_t >::max() )) ;
				genfile::impl::Enumeration const& enumeration = *m_table ;
				std::size_t const& maxAlleles = enumeration.first.second ;
				assert( m_number_of_alleles <= maxAlleles ) ;
				uint16_t const encodedGenotype = enumeration.second.second[ uint16_t( value_i ) ] ;
				uint32_t const bitsPerAllele = enumeration.first.first ;
				uint16_t mask = uint16_t( 0xFFFF ) >> ( 16 - bitsPerAllele ) ;
				// Dosage of the 1st allele in the genotype is not encoded directly.
				// We compute it as the ploidy minus the dosage of other alleles.
				uint16_t dosage_of_nonref_alleles = 0 ;
				for( std::size_t allele = 1; allele < m_number_of_alleles; ++allele ) {
					uint16_t const dosage = ( encodedGenotype >> ( (allele-1) * bitsPerAllele )) & mask ;
					(*m_counts)[allele] += value * dosage ;
					dosage_of_nonref_alleles += dosage ;
				}
				(*m_counts)[0] += value * (m_ploidy - dosage_of_nonref_alleles) ;
			}

			void finalise() {}
		private:
			std::vector< double >* m_counts ;
			std::size_t m_number_of_alleles ;
			uint32_t m_ploidy ;
			std::map< uint32_t, genfile::impl::Enumeration > m_tables ;
			genfile::impl::Enumeration* m_table ;
		} ;
	}

	struct AlleleCountComputation: public SNPSummaryComputation {
	public:

		AlleleCountComputation():
			m_counter( &m_counts )
		{}

		void list_variables( NameCallback callback ) const {
			using genfile::string_utils::to_string ;
			// By default we list 10 alleles
			callback( "number_of_alleles" ) ;
			for( std::size_t i = 0; i < 10; ++i ) {
				callback( "allele" + to_string(i+1) + "_count" ) ;
			}
		}

		void operator()(
			VariantIdentifyingData const& snp,
			Genotypes const& genotypes,
			Ploidy const& ploidy,
			genfile::VariantDataReader& data_reader,
			ResultCallback callback
		) {
			using genfile::string_utils::to_string ;
			m_counts = std::vector< double >( snp.number_of_alleles(), 0.0 ) ;
			m_counter.set_counts( &m_counts ) ;
			callback( "number_of_alleles", int64_t( snp.number_of_alleles() )) ;
			data_reader.get( ":genotypes:", genfile::to_GP_unphased( m_counter ) ) ;
			std::size_t i = 0 ;
			for( ; i < snp.number_of_alleles(); ++i ) {
				callback( "allele" + to_string(i+1) + "_count", m_counts[i] ) ;
			}
			for( ; i < 10; ++i ) {
				callback( "allele" + to_string(i+1) + "_count", genfile::MissingValue() ) ;
			}
		}

		std::string get_summary( std::string const& prefix, std::size_t column_width ) const {
			return prefix + "AlleleCountComputation" ;
		}
	private:
		std::vector< double > m_counts ;
		AlleleCountClient m_counter ;
	} ;

	struct AlleleFrequencyComputation: public SNPSummaryComputation
	{
		AlleleFrequencyComputation( std::string const& what ):
			m_compute_counts( what == "everything" || what == "counts" ),
			m_compute_frequencies( what == "everything" )
		{
			assert( what == "counts" || what == "everything" ) ;
		}
		void operator()(
			VariantIdentifyingData const& snp,
			Genotypes const& genotypes,
			Ploidy const& ploidy,
			genfile::VariantDataReader&,
			ResultCallback callback
		) {
			if( snp.number_of_alleles() == 2 ) {
				bool const allDiploid = ( ploidy.array() == 2 ).cast< int >().sum() == ploidy.size() ;
				if( allDiploid ) {
					compute_autosomal_frequency( snp, genotypes, callback ) ;
				} else {
					compute_sex_chromosome_frequency( snp, genotypes, ploidy, callback ) ;
				}
			}
		}

		void compute_sex_chromosome_frequency(
			VariantIdentifyingData const& snp,
			Genotypes const& genotypes,
			Ploidy const& ploidy,
			ResultCallback callback
		) {
			Genotypes haploid_genotypes = genotypes ;
			Genotypes diploid_genotypes = genotypes ;
			assert( std::size_t( genotypes.rows() ) == ploidy.size() ) ;

			for( int i = 0; i < genotypes.rows(); ++i ) {
				if( ploidy(i) != 1 ) {
					haploid_genotypes.row(i).setZero() ;
				}
				if( ploidy(i) != 2 ) {
					diploid_genotypes.row(i).setZero() ;
				}
			}

			double const a_allele_count = haploid_genotypes.col(0).sum()
				+ ( ( 2.0 * diploid_genotypes.col(0).sum() ) + diploid_genotypes.col(1).sum() ) ;
			double const b_allele_count = haploid_genotypes.col(1).sum()
				+ ( ( 2.0 * diploid_genotypes.col(2).sum() ) + diploid_genotypes.col(1).sum() ) ;

			if( m_compute_counts ) {
				callback( "alleleA_count", a_allele_count ) ;
				callback( "alleleB_count", b_allele_count ) ;
			}

			if( m_compute_frequencies ) {
				double const total_allele_count = ( haploid_genotypes.sum() + 2.0 * diploid_genotypes.sum() ) ;
				double const a_allele_freq = a_allele_count / total_allele_count ;
				double const b_allele_freq = b_allele_count / total_allele_count ;

				callback( "alleleA_frequency", a_allele_freq ) ;
				callback( "alleleB_frequency", b_allele_freq ) ;

				if( a_allele_freq < b_allele_freq ) {
					callback( "minor_allele_frequency", a_allele_freq ) ;
					callback( "minor_allele", snp.get_allele(0) ) ;
					callback( "major_allele", snp.get_allele(1) ) ;
				}
				else if( a_allele_freq > b_allele_freq ) {
					callback( "minor_allele_frequency", b_allele_freq ) ;
					callback( "minor_allele", snp.get_allele(1) ) ;
					callback( "major_allele", snp.get_allele(0) ) ;
				} else {
					callback( "minor_allele_frequency", a_allele_freq ) ;
				}
			}
		}

		void compute_autosomal_frequency( VariantIdentifyingData const& snp, Genotypes const& genotypes, ResultCallback callback ) {
			double const a_allele_count = ( 2.0 * genotypes.col(0).sum() ) + genotypes.col(1).sum() ;
			double const b_allele_count = ( 2.0 * genotypes.col(2).sum() ) + genotypes.col(1).sum() ;

			if( m_compute_counts ) {
				callback( "alleleA_count", a_allele_count ) ;
				callback( "alleleB_count", b_allele_count ) ;
			}

			if( m_compute_frequencies ) {
				double const total_allele_count = ( 2.0 * genotypes.sum() ) ;
				double const a_allele_freq = a_allele_count / total_allele_count ;
				double const b_allele_freq = b_allele_count / total_allele_count ;

				callback( "alleleA_frequency", a_allele_freq ) ;
				callback( "alleleB_frequency", b_allele_freq ) ;

				if( a_allele_freq < b_allele_freq ) {
					callback( "minor_allele_frequency", a_allele_freq ) ;
					callback( "minor_allele", snp.get_allele(0) ) ;
					callback( "major_allele", snp.get_allele(1) ) ;
				}
				else if( a_allele_freq > b_allele_freq ) {
					callback( "minor_allele_frequency", b_allele_freq ) ;
					callback( "minor_allele", snp.get_allele(1) ) ;
					callback( "major_allele", snp.get_allele(0) ) ;
				} else {
					callback( "minor_allele_frequency", a_allele_freq ) ;
				}
			}
		}

		std::string get_summary( std::string const& prefix, std::size_t column_width ) const {
			return prefix + "AlleleFrequencyComputation" ;
		}
	private:
		bool const m_compute_counts ;
		bool const m_compute_frequencies ;
	} ;

	// What proportion of the mass on a genotype is due to high-confidence calls?
	struct CallMassComputation: public SNPSummaryComputation
	{

		CallMassComputation( double const threshhold = 0.9 ):
			m_threshhold(threshhold)
		{}

		void operator()( VariantIdentifyingData const& snp, Genotypes const& genotypes, Ploidy const& ploidy, genfile::VariantDataReader&, ResultCallback callback ) {
			bool const allDiploid = ( ploidy.array() == 2 ).cast< int >().sum() == ploidy.size() ;
			if( allDiploid ) {
				compute_autosomal_call_mass( snp, genotypes, callback ) ;
			} else {
				return ;
			}
		}

		void compute_autosomal_call_mass( VariantIdentifyingData const& snp, Genotypes const& genotypes, ResultCallback callback ) {
			Eigen::VectorXd const masses = genotypes.colwise().sum() ;
			m_hcGenotypes = ( genotypes.array() > m_threshhold ).cast< double >()  * genotypes.array() ;
			Eigen::VectorXd const hcMasses = m_hcGenotypes.colwise().sum() ;

			callback( "AA_mass_propn", hcMasses(0)/masses(0) ) ;
			callback( "AB_mass_propn", hcMasses(1)/masses(1) ) ;
			callback( "BB_mass_propn", hcMasses(2)/masses(2) ) ;

			callback( "non-AA-mass_propn", (hcMasses(1)+hcMasses(2))/(masses(1)+masses(2))) ;
			callback( "non-BB-mass_propn", (hcMasses(1)+hcMasses(0))/(masses(1)+masses(0))) ;
		}

		std::string get_summary( std::string const& prefix, std::size_t column_width ) const {
			return prefix + "CallMassComputation" ;
		}
	private:
		double const m_threshhold ;
		Genotypes m_hcGenotypes ;
	} ;

	struct MissingnessComputation: public SNPSummaryComputation {
		MissingnessComputation( double call_threshhold = 0.9 ): m_call_threshhold( call_threshhold ) {}
		void operator()(
			VariantIdentifyingData const& snp,
			Genotypes const& genotypes,
			Ploidy const& ploidy,
			genfile::VariantDataReader&,
			ResultCallback callback
		) {
			assert( std::size_t( genotypes.rows() ) == ploidy.size() ) ;

			double missingness = double( genotypes.rows() ) - genotypes.array().sum() ;
			callback( "missing_proportion", missingness / double( genotypes.rows() ) ) ;

			bool const allDiploid = ( ploidy.array() == 2 ).cast< int >().sum() == ploidy.size() ;
			if( allDiploid ) {
				callback( "A", 0 ) ;
				callback( "B", 0 ) ;
				if( snp.number_of_alleles() == 2 ) {
					callback( "AA", genotypes.col(0).sum() ) ;
					callback( "AB", genotypes.col(1).sum() ) ;
					callback( "BB", genotypes.col(2).sum() ) ;
				}
				callback( "NULL", genotypes.rows() - genotypes.sum() ) ;
			} else {
				compute_haploid_diploid_counts( snp, genotypes, ploidy, callback ) ;
			}
			callback( "total", genfile::VariantEntry::Integer( genotypes.rows() )) ;
		}

		void compute_haploid_diploid_counts(
			VariantIdentifyingData const& snp,
			Genotypes const& genotypes,
			Ploidy const& ploidy,
			ResultCallback callback
		) {
			std::map< int, Eigen::VectorXd > counts ;
			std::map< int, double > null_counts ;
			counts[ -1 ] = Eigen::VectorXd::Zero( 3 ) ;
			counts[ 0 ] = Eigen::VectorXd::Zero( 3 ) ;
			counts[ 1 ] = Eigen::VectorXd::Zero( 3 ) ;
			counts[ 2 ] = Eigen::VectorXd::Zero( 3 ) ;
			std::map< int, std::size_t > sample_counts ;

			for( std::size_t i = 0; i < ploidy.size(); ++i ) {
				counts[ ploidy(i) ] += genotypes.row( i ) ;
				null_counts[ ploidy(i) ] += ( 1 - genotypes.row(i).sum() ) ;
				++sample_counts[ ploidy(i) ] ;
#if DEBUG_SNP_SUMMARY_COMPUTATION
				if( ploidy(i) == 1 && genotypes(i,2) != 0 ) {
					std::cerr << "! ( MissingnessComputation::compute_sex_chromosome_counts() ): individual " << (i+1) << "is male but has genotype " << genotypes.row(i) << "!!\n" ;
				}
#endif
			}

			callback( "A", counts[ 1 ]( 0 ) ) ;
			callback( "B", counts[ 1 ]( 1 ) ) ;
			callback( "AA", counts[ 2 ]( 0 ) ) ;
			callback( "AB", counts[ 2 ]( 1 ) ) ;
			callback( "BB", counts[ 2 ]( 2 ) ) ;
			callback( "NULL", null_counts[ 'm' ] + null_counts[ 'f' ] ) ;
			callback( "unknown_ploidy", counts[ -1 ].sum() + null_counts[ -1 ] ) ;
			assert( counts[ 1 ]( 2 ) == 0 ) ;
		}

		std::string get_summary( std::string const& prefix = "", std::size_t column_width = 20 ) const { return prefix + "MissingnessComputation" ; }
	private:
		double const m_call_threshhold ;
	} ;

	SNPSummaryComputation::UniquePtr SNPSummaryComputation::create(
		std::string const& name
	) {
		UniquePtr result ;
		if( name == "allele-frequencies" ) { result.reset( new stats::AlleleFrequencyComputation( "everything" )) ; }
		else if( name == "allele-counts" ) { result.reset( new stats::AlleleFrequencyComputation( "counts" )) ; }
		else if( name == "HWE" ) { result.reset( new stats::HWEComputation()) ; }
		else if( name == "missingness" ) { result.reset( new stats::MissingnessComputation()) ; }
		else if( name == "info" ) { result.reset( new stats::InfoComputation()) ; }
		else if( name == "call-mass-proportion" ) { result.reset( new stats::CallMassComputation()) ; }
		else if( name == "intensity-stats" ) { result.reset( new stats::IntensitySummaryComputation() ) ; }
		else if( name == "multi-allele-counts" ) { result.reset( new stats::AlleleCountComputation() ) ; }
		else {
			throw genfile::BadArgumentError( "SNPSummaryComputation::create()", "name=\"" + name + "\"" ) ;
		}
		return result ;
	}
}


#include <cmath>

#include <boost/scoped_ptr.hpp>
/*#include <boost/accumulators/accumulators.hpp>
 #include <boost/accumulators/statistics/stats.hpp>
 #include <boost/accumulators/statistics/min.hpp>
 #include <boost/accumulators/statistics/mean.hpp>
 #include <boost/accumulators/statistics/variance.hpp>
 #include <boost/accumulators/statistics/max.hpp>*/

#include <BnSimulator/util/utility.hpp>
#include <BnSimulator/util/state_util.hpp>
#include <BnSimulator/core/BooleanNetwork.hpp>
#include <BnSimulator/experiment/DamianiPlotter.hpp>

namespace bn {

using namespace std;

using namespace boost;

DamianiPlotter::PlotData DamianiPlotter::computePlot(BooleanNetwork& net,
		const size_t order) const {
	assert(order > 0);
	PlotData data(maxSteps);
	for (size_t p = 0; p < probes; ++p) {
		State s0 = util::random_state(net.size());
		State sx = util::perturb_state_randomly(s0, order);
		assert(util::hamming_distance(s0, sx) == order);
		scoped_ptr<vector<State> > t0(traceNetwork(net, s0));
		scoped_ptr<vector<State> > tx(traceNetwork(net, sx));
		for (size_t i = 0; i < maxSteps; ++i) {
			data[i] += (static_cast<double> (util::hamming_distance((*t0)[i],
					(*tx)[i])) / net.size()) / probes;
		}
	}
	return data;
}

DamianiPlotter::PlotData DamianiPlotter::computePlot(BooleanNetwork& net,
		const double d) const {
	assert(d >= 0 && d <= 1);
	return computePlot(net,
			static_cast<size_t> (std::floor(net.size() * d)));
}

vector<State>* DamianiPlotter::traceNetwork(BooleanNetwork& net,
		const State& s0) const {
	net.setState(s0);
	vector<State>* trajectory = new vector<State> (maxSteps);
	for (size_t t = 0; t < maxSteps; ++t, net.update()) {
		(*trajectory)[t] = net.getState();
	}
	return trajectory;
}

} // namespace bn


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

/**
 * @file aig_from_bdd.hpp
 *
 * @brief Create AIGs from BDDs
 *
 * @author Mathias Soeken
 * @since  2.0
 */

#include "aig_from_bdd.hpp"

#include <boost/format.hpp>

#include <cuddInt.h>

#include <classical/utils/aig_utils.hpp>

namespace cirkit
{

aig_function aig_from_bdd_rec( aig_graph& aig, DdManager* dd, DdNode* node )
{
  const auto& info = aig_info( aig );

  auto   is_complement = Cudd_IsComplement( node );
  auto * r = Cudd_Regular( node );

  aig_function f;

  if ( Cudd_IsConstant( r ) )
  {
    f = aig_get_constant( aig, r == DD_ONE( dd ) );
  }
  else
  {
    auto index = Cudd_NodeReadIndex( r );

    auto f_true  = aig_from_bdd_rec( aig, dd, cuddT( r ) );
    auto f_false = aig_from_bdd_rec( aig, dd, cuddE( r ) );

    f = aig_create_ite( aig, {info.inputs[index], false}, f_true, f_false );
  }

  return is_complement ? !f : f;
}

aig_function aig_from_bdd( aig_graph& aig, DdManager* dd, DdNode* node )
{
  const auto& info = aig_info( aig );

  auto n = Cudd_ReadSize( dd );
  auto num_pis = info.inputs.size();

  for ( auto i = num_pis; static_cast<int>( i ) < n; ++i )
  {
    aig_create_pi( aig, boost::str( boost::format( "x%d" ) % i ) );
  }

  return aig_from_bdd_rec( aig, dd, node );
}

aig_function aig_from_bdd( aig_graph& aig, const BDD& node )
{
  return aig_from_bdd( aig, node.manager(), node.getNode() );
}

}

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


/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 */

     #include <iostream>
     #include <Eigen/Eigenvalues>
     #include <Eigen/Dense>
     #include <vector>
     #include <math.h>
     #include <cmath>
     #include <fstream>
     #include <string>
     #include <vector>
     #include <pcl/io/io.h>
     #include <pcl/io/pcd_io.h>
     #include <pcl/point_types.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/common/common_headers.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/visualization/cloud_viewer.h>
     #include <pcl/filters/passthrough.h>
     #include <pcl/ModelCoefficients.h>
     #include <pcl/filters/project_inliers.h>
     #include <pcl/features/shot_omp.h>
     #include "pcl/features/fpfh.h"
     #include <pcl/io/ply_io.h>

using namespace std;
using namespace Eigen;

int
main (int argc, char*argv[])
{
	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr RelevantElements(new pcl::PointCloud<pcl::PointXYZRGBA>);
		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr SelectedElements(new pcl::PointCloud<pcl::PointXYZRGBA>);

	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TwoPlane45.pcd", *cloud);
	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeSharpEdge.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/OnePlane.pcd", *cloud);
	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/bunny.pcd",*cloud);
	     //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Statue.pcd", *cloud);
	  //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/dragon.pcd", *cloud);

	  //   pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/trim-starC.pcd", *cloud);
	      //	pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/VaseC.pcd", *cloud);
	       //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/twirlC.pcd", *cloud);
	      	//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/fandiskC.pcd", *cloud);
	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/sharp_sphereC.pcd", *cloud);
		// pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeFractal2.pcd", *cloud);
		//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/Frac2Guass12Noise.pcd", *cloud);
		  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/IntersectionThreePlanes.pcd", *cloud);
		// pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/SpherMultiple.pcd", *cloud);
		 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Tetrahedron.pcd", *cloud);
		 //pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/TetrahedronNoise35.pcd", *cloud);
		   pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TetrahedronMultiple.pcd", *cloud);

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr edge (new pcl::PointCloud<pcl::PointXYZRGBA>);
	 // pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTwoPlaneX.pcd", *edge);
	  	  // pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTwoPlaneY.pcd", *edge);
	  	// pcl::io::loadPCDFile ("/Path/TO/GroundTruth/ExactEdge.pcd", *edge);
	  	// pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundtruthFractal2.pcd", *edge);
	 // pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundTruthThreeoPlanes.pcd", *edge);
	 // pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundTruthTetrahedron.pcd", *edge);
	  pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTetrahedronMultiple.pcd", *edge);

	   std::cout << "Number of points in the Cube Input cloud is:"<< cloud->points.size() << std::endl;
	   std::cout << "Number of points in the Edge Ground Truth cloud is:"<< edge->points.size() << std::endl;

	   // Compute distance between all the points of the flat cloud and points of the Edge cloud
	   	std::vector<std::vector <double> > Distances ; // 2D vector(Matrix) for each point that has the Angle between lines of normals around barycenter
	   		 int heightD;
	   		 int widthD;
	    // defining an empty 2D vector (matrix)
	   	 heightD= cloud ->points.size ();
	   		 widthD = edge ->points.size ();
	   			 Distances.resize(heightD); // to defining numbers of row in 2D vector
	   				   for(int jj=0; jj< heightD; ++jj)
	   				   	{Distances[jj].resize(widthD);}   // to defining numbers of column for each row in 2d vector

	   for (size_t ii = 0; ii < cloud ->points.size (); ++ii) {
	    for (size_t jj = 0; jj< edge ->points.size (); ++jj) {
	   double distance = sqrt (   	( ( (cloud->points[ii].x)	-  (edge->points[jj].x) ) *  ( (cloud->points[ii].x)	-  (edge->points[jj].x) ) ) +   ( ( (cloud->points[ii].y)	-  (edge->points[jj].y) ) *  ( (cloud->points[ii].y)	-  (edge->points[jj].y) ) ) + ( ( (cloud->points[ii].z)	-  (edge->points[jj].z) ) *  ( (cloud->points[ii].z)	-  (edge->points[jj].z) ) )   );
	   		Distances [ii][jj] = distance;
	   			 }
	   		 }
	   	 // Define a cloud for the relevant elements
	   	 double relevantsize = 0.00 ;
	   		 // find minimum distance for each line
	   		std::vector<double> MinDistances;
	   		 for (size_t ii = 0; ii < cloud ->points.size (); ++ii) {
	   		 double min = 100.00;
	   		for (size_t jj = 0; jj< edge ->points.size (); ++jj) {
	   		 if (Distances [ii][jj] < min ){
	   			 min = Distances [ii][jj] ;}
	   				   }
	   	MinDistances .push_back (min );
	   	 if (min< 0.0075){
	   // Copy the 	coordinates on to  the relevant cloud
	   				pcl::PointXYZRGBA basic_point;
	   				basic_point.x = cloud->points[ii].x;
	   				basic_point.y = cloud->points[ii].y;
	   				basic_point.z = cloud->points[ii].z;
	   				basic_point.r = 255;
	   				basic_point.g = 255;
	   				basic_point.b= 255;
	   				RelevantElements->points.push_back(basic_point);
	   				relevantsize += 1.00 ;
	   				         }// change the color in main cloud
	   		 	 }// if min <0.03
	   // Size of the RelevantElements Cloud
	   		 RelevantElements->width = (int) RelevantElements->points.size ();
	   		 RelevantElements->height = 1;
	   		 std::cout << "Number of points in the relevant location is:"<< relevantsize << std::endl;
	   		 std::cout << "size of the relevant cloud is:"<< RelevantElements->points.size () << std::endl;

// to visualize the relevant points
	   	//  pcl::visualization::CloudViewer viewer1("Relevant points");
	   	//  viewer1.showCloud(RelevantElements);
	   	 //   while (!viewer1.wasStopped ())
	   	  // {}

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZRGBA>);
	  Normals->resize(cloud->size());

	  // K nearest neighbor search
	  int KNumbersNeighbor = 10; // numbers of neighbors 7
	  std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
	  std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

	  int* NumbersNeighbor = new  int [cloud ->points.size ()];
	  pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
	  kdtree.setInputCloud (cloud);
	  pcl::PointXYZRGBA searchPoint;

	  double* SmallestEigen = new  double [cloud->points.size() ];
	  double* MiddleEigen = new  double [cloud->points.size() ];
	  double* LargestEigen = new  double [cloud->points.size() ];

	  double* DLS = new  double [cloud->points.size() ];
	  double* DLM = new  double [cloud->points.size() ];
	  double* DMS = new  double [cloud->points.size() ];
	  double* Sigma = new  double [cloud->points.size() ];

//		std::vector<double> SmallestEigen;
//		std::vector<double> MiddleEigen;
//		std::vector<double> LargestEigen;
//
//		std::vector<double> DLS;
//		std::vector<double> DML;
//		std::vector<double> DMS;

	    //  ************ All the Points of the cloud *******************
	for (size_t i = 0; i < cloud ->points.size (); ++i) {

	searchPoint.x =   cloud->points[i].x;
	searchPoint.y =   cloud->points[i].y;
	searchPoint.z =   cloud->points[i].z;

	if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
		 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
	    else { NumbersNeighbor[i] = 0; }

	float Xmean; float Ymean; float Zmean;
	float sum= 0.00;
// Computing Covariance Matrix
	for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
      sum += cloud->points[ NeighborsKNSearch[ii] ].x; }
	Xmean = sum / NumbersNeighbor[i] ;
	sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].y;}
			Ymean = sum / NumbersNeighbor[i] ;
		 sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].z;}
			Zmean = sum / NumbersNeighbor[i] ;

			float	CovXX;  float CovXY; float CovXZ; float CovYX; float CovYY; float CovYZ; float CovZX; float CovZY; float CovZZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );}
			CovXX = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovXY = sum / ( NumbersNeighbor[i]-1) ;

			CovYX = CovXY ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovXZ= sum / ( NumbersNeighbor[i]-1) ;

			CovZX = CovXZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovYY = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovYZ = sum / ( NumbersNeighbor[i]-1) ;

			CovZY = CovYZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovZZ = sum / ( NumbersNeighbor[i]-1) ;

// Computing Eigenvalue and EigenVector
   Matrix3f Cov;
   Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

  SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
  if (eigensolver.info() != Success) abort();

  double EigenValue1 = eigensolver.eigenvalues()[0];
  double EigenValue2 = eigensolver.eigenvalues()[1];
  double EigenValue3 = eigensolver.eigenvalues()[2];

  double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;
  if (EigenValue1<  EigenValue2 ) { Smallest =  EigenValue1 ; } else { Smallest = EigenValue2 ; }
  if (EigenValue3<  Smallest ) { Smallest =  EigenValue3 ; }

  if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
	  Smallest = EigenValue1;
  if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
  else {Middle = EigenValue3; Largest = EigenValue2;}
  }

  if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
  {
	  Largest = EigenValue1;
  if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
  else {Smallest = EigenValue3; Middle = EigenValue2;}
  }

  if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
  {
	  Middle = EigenValue1;
  if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
  else{Largest = EigenValue3; Smallest = EigenValue2;}
  }

SmallestEigen[i]= Smallest ;
MiddleEigen[i]= Middle;
LargestEigen[i]= Largest;

DLS[i] = std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
DLM[i] = std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
DMS[i] = std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
	} // For each point of the cloud

// Color Map For the difference of the eigen values

	  double MaxD=0.00 ;
	  double MinD= cloud ->points.size ();
	  int Ncolors=256;

	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  	if (  Sigma [i] < MinD) MinD= Sigma [i];
	  	if (  Sigma[i] > MaxD) MaxD = Sigma [i];
	  }

	  std::cout<< " Minimum is :" << MinD<< std::endl;
	  std::cout<< " Maximum  is :" << MaxD << std::endl;

	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	  MaxD = ( 2 )* stddvtion;

/*
	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointCloud/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
if (  Sigma [i] < MaxD ) {
	    level = floor( (Sigma [i] - MinD ) /  step ) ;

	    cloud->points[i].r = code[ level ][0];
	    cloud->points[i].g =  code[ level ][1];
	    cloud->points[i].b =  code[ level ][2];
} // if sigma less than MAx
	    }
*/

	  int level = 0;
	  float step = ( ( MaxD -  MinD) / Ncolors ) ;
	 //  level = floor( (Sigma [i] - MinD ) /  step ) ;
for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  if ( Sigma [i] > ( MinD + ( 0.003* step) ) ) {   // for the 3 plane ( Sigma [i] > ( MinD + ( 0.003* step) ) )
	    cloud->points[i].r = 0;
	    cloud->points[i].g =  0 ;
	    cloud->points[i].b =  128;
		pcl::PointXYZRGBA basic_point2;
		basic_point2.x = cloud->points[i].x;
		basic_point2.y = cloud->points[i].y;
		basic_point2.z = cloud->points[i].z;
		basic_point2.r = 255;
		basic_point2.g = 255;
		basic_point2.b= 255;
		SelectedElements->points.push_back(basic_point2);
        }
     }

// Computing

SelectedElements->width = (int) SelectedElements->points.size ();
SelectedElements->height = 1;
std::cout <<"Size of the Edge  cloud by clustering method is:  " << SelectedElements->points.size ()<< std::endl;

// Compute the  True Positive by Comparing the two Clouds of Relevant and Selective
double TruePositive = 0.00 ;

	 for (size_t ii = 0; ii< RelevantElements ->points.size (); ++ii) {
 	 for (size_t jj = 0; jj< SelectedElements ->points.size (); ++jj) {
 		 if ( ((RelevantElements->points[ii].x) == (SelectedElements->points[jj].x)) &&   ((RelevantElements->points[ii].y) == (SelectedElements->points[jj].y)) &&  ((RelevantElements->points[ii].z) == (SelectedElements->points[jj].z)) ) {
			TruePositive += 1.00 ; 	 } // if
		     } // jj
 	 } // ii

std::cout <<"Numbers of True Positive  is:  " << TruePositive << std::endl;
double FalseNegative = (RelevantElements ->points.size () ) - TruePositive ;
std::cout <<"Numbers of False Negative  is:  " << FalseNegative << std::endl;
double FalsePositive = (SelectedElements ->points.size ()) - TruePositive;
std::cout <<"Numbers of False Positive  is:  " << FalsePositive << std::endl;
double TrueNegative = ((cloud ->points.size ()) -  (RelevantElements ->points.size () ) ) -  FalsePositive ;
std::cout <<"Numbers of True Negative  is:  " << TrueNegative << std::endl;

double Precision = (TruePositive / ( SelectedElements ->points.size () ) ) ;
std::cout <<"Numbers of Precision  is:  " << Precision << std::endl;
double Recall = (TruePositive / (RelevantElements ->points.size () ) ) ;
std::cout <<"Numbers of Recall   is:  " << Recall << std::endl;
double FScore = 2* (   ( Precision * Recall  ) / (Precision +  Recall )) ;
std::cout <<"F1 Score of True Positive  is:  " << FScore << std::endl;

  	pcl::PLYWriter writePLY;
  //	writePLY.write ("/Path/TO/CUbeEigenJetColor.ply", *cloud,  false);
	 //   writePLY.write ("/Path/TO/CloudEigeJnetTwirl.ply", *cloud,  false);
	// writePLY.write ("/Path/TO/CloudEigeJnetDragon.ply", *cloud,  false);
  	//  writePLY.write ("/Path/TO/CloudEigeJnetTwoPlane22.ply", *cloud,  false);

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
  viewer.showCloud(cloud);
  while (!viewer.wasStopped ())
  {}

  return 0;
  }


#pragma once
#include <Eigen/Core>
#include <memory>
#include <numeric>
#include <set>
#include <string>
#include "ear/layout.hpp"
#include "ear/metadata.hpp"

namespace ear {

  /** @brief An interface for objects that can calculate gains for some
   * positions, e.g. a triangle of loudspeakers.
   */
  class RegionHandler {
   public:
    RegionHandler(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions);
    virtual boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const = 0;
    virtual ~RegionHandler() = default;

    boost::optional<Eigen::VectorXd> handleRemap(Eigen::Vector3d position,
                                                 int numberOfChannels) const;
    Eigen::VectorXi outputChannels();

   protected:
    Eigen::VectorXi _outputChannels;
    Eigen::MatrixXd _positions;
  };

  /** @brief Region handler representing a triplet of loudspeakers, implementing
   * VBAP.
   *
   * This is implemented such that if handle(pos) returns array x:
   *
   *  - dot(x, positions) is collinear with pos
   *  - x[i] >= 0 for all i
   *  - norm(x) == 1
   *
   * Note that the positions are *not* normalised, as this is not always
   * desirable.
   */
  class Triplet : public RegionHandler {
   public:
    /** @brief Ctor
     *
     * @param  output_channels The channel numbers of the values returned by
     *   handle.
     * @param  positions  Cartesian positions of the three speakers; index order
     *   is speaker, axis.
     */
    Triplet(Eigen::Vector3i outputChannels, Eigen::Matrix3d positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Matrix3d _basis;
  };

  /** @brief Region handler representing n real loudspeakers and a central
   * virtual loudspeaker, whose gain is distributed to the real loudspeakers.
   *
   * Triplet regions are formed between the virtual speaker and pairs of real
   * speakers on the edge of the ngon. Any gain sent to the virtual speaker is
   * multiplied by centre_downmix and summed into the gains for the real
   * loudspeakers,which are then normalised.
   */
  class VirtualNgon : public RegionHandler {
   public:
    /** @brief Ctor
     *
     * @param  output_channels The channel numbers of the values returned by
     *   handle.
     * @param  positions Cartesian positions of the n loudspeakers.
     * @param  centrePosition Cartesian position of the central virtual
     *   loudspeaker
     * @param  centreDownmix Downmix coefficients for distributing gains from
     * the centre virtual loudspeaker to the loudspeakers defined by
     * positions.
     */
    VirtualNgon(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions,
                Eigen::Vector3d centrePosition, Eigen::VectorXd centreDownmix);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Vector3d _centrePosition;
    Eigen::VectorXd _centreDownmix;
    std::vector<std::unique_ptr<RegionHandler>> _regions;
  };

  class QuadRegion : public RegionHandler {
   public:
    QuadRegion(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Matrix3d _calcPolyBasis(Eigen::MatrixXd positions);

    boost::optional<double> _pan(Eigen::Vector3d position,
                                 Eigen::Matrix3d polyBasis) const;

    Eigen::VectorXi _order;
    Eigen::Matrix3d _polyBasisX;
    Eigen::Matrix3d _polyBasisY;
  };

  /** @brief Base class for all PointSourcePanner like classes
   */
  class PointSourcePanner {
   public:
    virtual boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) = 0;
    virtual int numberOfOutputChannels() const = 0;
  };

  /** @brief Wrapper around multiple regions.
   */
  class PolarPointSourcePanner : public PointSourcePanner {
   public:
    /** @brief Ctor
     *
     * @param  regions Regions used to handle a position.
     * @param  numberOfChannels Number of output channels; this is computed
     * from the output channels of the regions if not provided.
     */
    PolarPointSourcePanner(std::vector<std::unique_ptr<RegionHandler>> regions,
                           boost::optional<int> numberOfChannels = boost::none);

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;

    int numberOfOutputChannels() const override;

   private:
    int _numberOfRequiredChannels();

    const std::vector<std::unique_ptr<RegionHandler>> _regions;
    int _numberOfOutputChannels;
  };

  /** @brief Wrapper around a point source panner with an additional downmix.
   */
  class PointSourcePannerDownmix : public PointSourcePanner {
   public:
    /** @brief Ctor
     *
     * @param  psp Inner point source panner.
     * @param  downmix Downmix matrix (mxn) from m inputs to n outputs.
     */
    PointSourcePannerDownmix(std::shared_ptr<PointSourcePanner> psp,
                             Eigen::MatrixXd downmix);
    ~PointSourcePannerDownmix() = default;

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;

    int numberOfOutputChannels() const override;

   private:
    std::shared_ptr<PointSourcePanner> _psp;
    Eigen::MatrixXd _downmix;
  };

  /** @brief Generate extra loudspeaker positions to fill gaps in layers.
   *
   * @param  layout Original layout without the LFE channels
   *
   * @returns
   *   - list of extra channels (layout.Channel).
   *   - downmix matrix to mix the extra channel outputs to the real channels
   */
  std::pair<std::vector<Channel>, Eigen::MatrixXd> extraPosVerticalNominal(
      Layout layout);

  // given a layout, determine the full set of loudspeaker positions used for
  // panning. returns:
  //
  // - the real position of real and virtual loudspeakers
  // - the nominal position of the real and virtual loudspeakers
  // - the indices of the virtual loudspeakers in the two position lists
  // - a downmix matrix to be applied to the output of the real loudspeakers
  std::tuple<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>,
             std::set<int>, Eigen::MatrixXd>
  getAugmentedLayout(const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureFullPolarPanner(
      const Layout& layout);

  class StereoPannerDownmix : public RegionHandler {
   public:
    StereoPannerDownmix(Eigen::VectorXi outputChannels,
                        Eigen::MatrixXd positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    std::shared_ptr<PointSourcePanner> _psp;
  };

  class AllocentricPanner : public PointSourcePanner {
   public:
    AllocentricPanner() = default;
    ~AllocentricPanner() = default;

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;
    int numberOfOutputChannels() const override;
  };

  std::shared_ptr<PointSourcePanner> configureStereoPolarPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureFullPolarPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureAllocentricPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configurePolarPanner(const Layout& layout);

}  // namespace ear


/************************************************
增加发布速度的话题/mavros/setpoint_raw/local，
引入接收相对相机相对Apriltag码的话题/tag_detections
引入相机参数话题/camera/camera_info
实现从相机坐标系到像素坐标系的变换，根据Apriltag码大小
得出四个角点的像素坐标。引入图像距，计算得到期望
速度值，进行发布
Date:2022/04/12
Editor:Lizhicheng
***************************************************/

#include "ros/ros.h"

#include "mavros_msgs/PositionTarget.h"
#include<vector>

// #include "apriltags_ros/AprilTagDetectionArray.h"
#include "apriltag2_ros/Apriltags.h"

// #include "sensor_msgs/CameraInfo.h"
// #include <geometry_msgs/PoseStamped.h>

#include <opencv2/opencv.hpp>
#include <highgui.h>
#include <Eigen/Dense>
#include <eigen_conversions/eigen_msg.h>
#include <tf/transform_datatypes.h>
#include <cmath>

// void tagCallback(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tag)
// {
//     last_msg_=tag;
//     center_x = last_msg_->detections[0].pose.pose.pose.position.x;      //可视作Apriltag码中心点在相机坐标系下的位置
//     center_y = last_msg_->detections[0].pose.pose.pose.position.y;
//     center_z = last_msg_->detections[0].pose.pose.pose.position.z;

//     //使用相机内参矩阵将相机坐标系下的点转换为像素坐标

// }

ros::Publisher vel_pub;

double sq(double a)
{
    return a * a;
}

void cornerCallback(const apriltag2_ros::Apriltags& msg)
{

mavros_msgs::PositionTarget velmsg;
velmsg.header.stamp = ros::Time::now();
static int seq = 1;
velmsg.header.seq = seq++;
velmsg.header.frame_id = 1;
velmsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
velmsg.type_mask = mavros_msgs::PositionTarget::IGNORE_PX +
                    mavros_msgs::PositionTarget::IGNORE_PY +
                    mavros_msgs::PositionTarget::IGNORE_PZ +
                    mavros_msgs::PositionTarget::IGNORE_AFX +
                    mavros_msgs::PositionTarget::IGNORE_AFY +
                    mavros_msgs::PositionTarget::IGNORE_AFZ +
                    mavros_msgs::PositionTarget::FORCE +
                    mavros_msgs::PositionTarget::IGNORE_YAW +
                    mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

        cv::Mat r1 = cv::Mat(3, 1, CV_64FC1);
        cv::Mat r2 = cv::Mat(3, 1, CV_64FC1);
        cv::Mat r3 = cv::Mat(3, 1, CV_64FC1);
        cv::Mat r4 = cv::Mat(3, 1, CV_64FC1);

        tf::Matrix3x3 R_rp;

        cv::Matx33d mK(205.46963709898583, 0.0, 320.5, 0.0, 205.46963709898583, 180.5, 0.0, 0.0, 1.0);
                                // fx, 0.0, cx,
                                // 0.0, fy, cy,
                                // 0.0, 0.0, 1.0

        // cv::Vec4f distParam(params_.D.at<double>(0), params_.D.at<double>(1), params_.D.at<double>(2), params_.D.at<double>(3));
        cv::Vec4f distParam(0.0, 0.0, 0.0, 0.0);

    // cv::Vec3d rvec, tvec;
    //相机内参

        // geometry_msgs::PoseStamped vpdata_pose;  //20180425 image moment in the virtual plane

    //接收角点像素坐标
        r1.at<double>(0, 0) = msg.apriltags[0].corners[0].x; //
        r1.at<double>(1, 0) = msg.apriltags[0].corners[0].y; //
        r1.at<double>(2, 0) = 1;
        r2.at<double>(0, 0) = msg.apriltags[0].corners[1].x; //
        r2.at<double>(1, 0) = msg.apriltags[0].corners[1].y; //
        r2.at<double>(2, 0) = 1;
        r3.at<double>(0, 0) = msg.apriltags[0].corners[2].x; //
        r3.at<double>(1, 0) = msg.apriltags[0].corners[2].y; //
        r3.at<double>(2, 0) = 1;
        r4.at<double>(0, 0) = msg.apriltags[0].corners[3].x; //
        r4.at<double>(1, 0) = msg.apriltags[0].corners[3].y; //
        r4.at<double>(2, 0) = 1;

        ROS_INFO("r1 [0]%f",msg.apriltags[0].corners[0].x);
        ROS_INFO("r1 [1]%f",r1.at<double>(1,0));

        ROS_INFO("r2 [0]%f",msg.apriltags[0].corners[1].x);
        ROS_INFO("r2 [1]%f",r2.at<double>(1,0));

        ROS_INFO("r3 [0]%f",msg.apriltags[0].corners[2].x);
        ROS_INFO("r3 [1]%f",r3.at<double>(1,0));

        ROS_INFO("r4 [0]%f",msg.apriltags[0].corners[3].x);
        ROS_INFO("r4 [1]%f",r4.at<double>(1,0));

        cv::Matx33d mK_inv = mK.inv();//求逆//A矩阵相机固有参数矩阵
        //cv::Vec3d _r1 ;
        //cv::Vec3d  _r1= (mK_inv * r1);//标准化图像坐标系
        // cv::Vec3d _r2 = mK_inv * r2;
        // cv::Vec3d _r3 = mK_inv * r3;
        // cv::Vec3d _r4 = mK_inv * r4;

        cv::Mat  _r1= (mK_inv * r1);//标准化图像坐标系
        cv::Mat _r2 = mK_inv * r2;
        cv::Mat _r3 = mK_inv * r3;
        cv::Mat _r4 = mK_inv * r4;

        /*virtual image plane with full rotation*/

        // R_rp.setRPY(0,0,0);
        // tf::Vector3 swap_p;
        // double beta;
        // // swap_p.setValue(_r1[0], _r1[1], 1);//标准化
        // swap_p.setValue(_r1.at<double>(0,0), _r1.at<double>(1,0), 1);//标准化
        // // swap_p = R_rp * swap_p;//R_rp为实际平面到虚平面的旋转矩阵
        // beta = (double)1.0 / (swap_p.z());
        // // _r1[0] = beta * swap_p.x();
        // // _r1[1] = beta * swap_p.y();//m_v
        // _r1.at<double>(0,0) = beta * swap_p.x();
        // _r1.at<double>(1,0) = beta * swap_p.y();//m_v

        // // swap_p.setValue(_r2[0], _r2[1], 1);
        // swap_p.setValue(_r2.at<double>(0,0), _r2.at<double>(1,0), 1);
        // // swap_p = R_rp * swap_p;
        // beta = (double)1.0 / (swap_p.z());
        // // _r2[0] = beta * swap_p.x();
        // // _r2[1] = beta * swap_p.y();
        // _r2.at<double>(0,0)= beta * swap_p.x();
        // _r2.at<double>(1,0)= beta * swap_p.y();

        // // swap_p.setValue(_r3[0], _r3[1], 1);
        // swap_p.setValue(_r3.at<double>(0,0), _r3.at<double>(1,0), 1);
        // // swap_p = R_rp * swap_p;
        // beta = (double)1.0 / (swap_p.z());
        // // _r3[0] = beta * swap_p.x();
        // // _r3[1] = beta * swap_p.y();
        // _r3.at<double>(0,0) = beta * swap_p.x();
        // _r3.at<double>(1,0) = beta * swap_p.y();

        // // swap_p.setValue(_r4[0], _r4[1], 1);
        // swap_p.setValue(_r4.at<double>(0,0), _r4.at<double>(1,0), 1);
        // // swap_p = R_rp * swap_p;
        // beta = (double)1.0 / (swap_p.z());
        // // _r4[0] = beta * swap_p.x();
        // // _r4[1] = beta * swap_p.y();
        // _r4.at<double>(0,0) = beta * swap_p.x();
        // _r4.at<double>(1,0) = beta * swap_p.y();

        double cake = 0;
        double ug = 0,vg = 0;
        double az_u = 0, az_v = 0;
        double az = 0;
        double dug = 0, dvg = 0, daz = 0.125;

            ug = (_r1.at<double>(0, 0) + _r2.at<double>(0, 0) + _r3.at<double>(0, 0) + _r4.at<double>(0, 0)) * 0.25;//gravity center
            vg = (_r1.at<double>(1, 0) + _r2.at<double>(1, 0) + _r3.at<double>(1, 0) + _r4.at<double>(1, 0)) * 0.25;

            az_u = (sq(ug - _r1.at<double>(0, 0)) + sq(ug - _r2.at<double>(0, 0)) + sq(ug - _r3.at<double>(0, 0)) + sq(ug - _r4.at<double>(0, 0)));
            az_v = (sq(vg - _r1.at<double>(1, 0)) + sq(vg - _r2.at<double>(1, 0)) + sq(vg - _r3.at<double>(1, 0)) + sq(vg - _r4.at<double>(1, 0)));

        // ug = (_r1[0] + _r2[0] + _r3[0] + _r4[0]) * 0.25;
        // vg = (_r1[1] + _r2[1] + _r3[1] + _r4[1]) * 0.25;

        // az_u = (sq(ug - _r1[0]) + sq(ug - _r2[0]) + sq(ug - _r3[0]) + sq(ug - _r4[0]));
        // az_v = (sq(vg - _r1[1]) + sq(vg - _r2[1]) + sq(vg - _r3[1]) + sq(vg - _r4[1]));

        az = az_u + az_v;
        /* 1M  height */
        // daz = 0.055778;

        /* M  height */
        // daz = 0.087153125;
        //

        cake = sqrt(daz / az);
        // vpdata_pose.header.stamp = ros::Time::now();  //what is this?
        // vpdata_pose.pose.position.x = (cake * (dvg - vg));
        // vpdata_pose.pose.position.y = (cake * (dug - ug));
        // vpdata_pose.pose.position.z = (1 - cake);
        // vpdata_pose.pose.orientation = msg.apriltags[0].pose.orientation;//

        velmsg.velocity.x=  10*(cake * (dvg - vg));//发布速度
        velmsg.velocity.y=  10*(cake * (dug - ug));
        velmsg.velocity.z=   (1 - cake);

        vel_pub.publish(velmsg);
        ROS_INFO("velmsg.x is %f",velmsg.velocity.x);
        ROS_INFO("velmsg.y is %f",velmsg.velocity.y);
        ROS_INFO("velmsg.z is %f",velmsg.velocity.z);

        // pub_vppose.publish(vpdata_pose);

}

// void camCallback(const sensor_msgs::CameraInfo::ConstPtr& cam_info)
// {

// // # Intrinsic camera matrix for the raw (distorted) images.
// // #         [fx  0 cx]
// // # K = [ 0 fy cy]
// // #         [ 0  0  1]
// // # Projects 3D points in the camera coordinate frame to 2D pixel
// // # coordinates using the focal lengths (fx, fy) and principal point
// // # (cx, cy).

//     // fx = cam_info->K[0];  //相机内参矩阵，使用焦距（fx,fy）及主点坐标(cx,cy), 单位为像素
//     // fy = cam_info->K[4];
//     // cx = cam_info->K[2];
//     // cy = cam_info->K[5];
//     // height = cam_info->height;
//     // width = cam_info->width;

//     //cgo3_camera K: [205.46963709898583, 0.0, 320.5, 0.0, 205.46963709898583, 180.5, 0.0, 0.0, 1.0]
//     //                             D: [0.0, 0.0, 0.0, 0.0, 0.0]

//     // cv::Vec4f distParam(params_.D.at<double>(0), params_.D.at<double>(1), params_.D.at<double>(2), params_.D.at<double>(3)); // all 0?
//     // cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
//     // cv::Matx33f r;
//     // cv::Rodrigues(rvec, r);

// }

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ImageMoment");
    // uint32 height,width;
    // float64 fx,fy,cx,cy;
    //float64 center_x,center.y,center.z;

    tf::Matrix3x3 R_rp;

    cv::Matx33f mK(205.46963709898583, 0.0, 320.5, 0.0, 205.46963709898583, 180.5, 0.0, 0.0, 1.0);
                                // fx, 0.0, cx,
                                // 0.0, fy, cy,
                                // 0.0, 0.0, 1.0

    // cv::Vec4f distParam(params_.D.at<double>(0), params_.D.at<double>(1), params_.D.at<double>(2), params_.D.at<double>(3));
    cv::Vec4f distParam(0.0, 0.0, 0.0, 0.0);

    // cv::Vec3d rvec, tvec;
    //相机内参

    // geometry_msgs::PoseStamped vpdata_pose;  //20180425 image moment in the virtual plane

    // apriltags_ros::AprilTagDetectionArrayConstPtr last_msg_;

    ros::NodeHandle nh;

    vel_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 6);
    // ros::Subscriber tag_sub = nh.subscribe("/tag_detections",1000,tagCallback);
    ros::Subscriber corners_sub = nh.subscribe("/apriltag_detector/tags" , 1, cornerCallback);
    // ros::Subscriber cam_info_sub = nh.subscribe("/camera/camera_info",1000,camCallback);
    //ros::Publisher pub_vppose = nh.advertise<geometry_msgs::PoseStamped>("setpoint/relative_pos", 5);

    ros::Rate loop_rate(10);

    // mavros_msgs::PositionTarget msg;
    // msg.header.stamp = ros::Time::now();
    // static int seq = 1;
    // msg.header.seq = seq++;
    // msg.header.frame_id = 1;
    // msg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    // msg.type_mask = mavros_msgs::PositionTarget::IGNORE_PX +
    //                 mavros_msgs::PositionTarget::IGNORE_PY +
    //                 mavros_msgs::PositionTarget::IGNORE_PZ +
    //                 mavros_msgs::PositionTarget::IGNORE_AFX +
    //                 mavros_msgs::PositionTarget::IGNORE_AFY +
    //                 mavros_msgs::PositionTarget::IGNORE_AFZ +
    //                 mavros_msgs::PositionTarget::FORCE +
    //                 mavros_msgs::PositionTarget::IGNORE_YAW +
    //                 mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

    while (ros::ok())
    {

        // msg.velocity.x=     ;//发布速度
        // msg.velocity.y=    ;
        // msg.velocity.z=    ;

        // vel_pub.publish(msg);

        ros::spinOnce();

        loop_rate.sleep();

    }

    return 0;
}


#include "mp_class.h"
#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/mpl/int.hpp>
#include <utility>
#include <cmath>
#include <symengine/symengine_assert.h>
#include <symengine/symengine_exception.h>

using boost::multiprecision::numerator;
using boost::multiprecision::denominator;
using boost::multiprecision::miller_rabin_test;
using boost::multiprecision::detail::find_lsb;
using boost::mpl::int_;

#if SYMENGINE_INTEGER_CLASS == SYMENGINE_BOOSTMP

namespace SymEngine
{

integer_class pow(const integer_class &a, unsigned long b)
{
    return boost::multiprecision::pow(a, numeric_cast<unsigned>(b));
}

void mp_fdiv_qr(integer_class &q, integer_class &r, const integer_class &a,
                const integer_class &b)
{
    /*boost::multiprecision doesn't have a built-in fdiv_qr (floored division).
      Its divide_qr uses truncated division, as does its
      modulus operator. Thus, using boost::multiprecision::divide_qr we get:
      divide_qr(-5, 3, quo, rem) //quo == -1, rem == -2
      divide_qr(5, -3, quo, rem) //quo == -1, rem == 2
      but we want:
      mp_fdiv_r(quo, rem, -5, 3) //quo == -2, rem == 1
      mp_fdiv_r(quo, rem, 5, -3) //rem == -2, rem == -1
      The problem arises only when the quotient is negative.  To convert
      a truncated result into a floored result in this case, simply subtract
      one from the truncated quotient and add the divisor to the truncated
      remainder.
      */

    // must copy a and b before calling divide_qr because a or b may refer to
    // the same
    // object as q or r, causing incorrect results
    integer_class a_cpy = a, b_cpy = b;
    bool neg_quotient = ((a < 0 && b > 0) || (a > 0 && b < 0)) ? true : false;
    boost::multiprecision::divide_qr(a_cpy, b_cpy, q, r);
    // floor the quotient if necessary
    if (neg_quotient && r != 0) {
        q -= 1;
    }
    // remainder should have same sign as divisor
    if ((b_cpy > 0 && r < 0) || (b_cpy < 0 && r > 0)) {
        r += b_cpy;
        return;
    }
}

void mp_cdiv_qr(integer_class &q, integer_class &r, const integer_class &a,
                const integer_class &b)
{
    integer_class a_cpy = a, b_cpy = b;
    bool pos_quotient = ((a < 0 && b < 0) || (a > 0 && b > 0)) ? true : false;
    boost::multiprecision::divide_qr(a_cpy, b_cpy, q, r);
    // ceil the quotient if necessary
    if (pos_quotient && r != 0) {
        q += 1;
    }
    // remainder should have opposite sign as divisor
    if ((b_cpy > 0 && r > 0) || (b_cpy < 0 && r < 0)) {
        r -= b_cpy;
        return;
    }
}

void mp_gcdext(integer_class &gcd, integer_class &s, integer_class &t,
               const integer_class &a, const integer_class &b)
{

    integer_class this_s(1);
    integer_class this_t(0);
    integer_class next_s(0);
    integer_class next_t(1);
    integer_class this_r(a);
    integer_class next_r(b);
    integer_class q;
    while (next_r != 0) {
        // should use truncated division, so use
        // boost::multiprecision::divide_qr
        // beware of overwriting this_r during internal operations of divide_qr
        // copy it first
        integer_class this_r_cpy = this_r;
        boost::multiprecision::divide_qr(this_r_cpy, next_r, q, this_r);
        this_s -= q * next_s;
        this_t -= q * next_t;
        std::swap(this_s, next_s);
        std::swap(this_t, next_t);
        std::swap(this_r, next_r);
    }
    // normalize the gcd, s and t
    if (this_r < 0) {
        this_r *= -1;
        this_s *= -1;
        this_t *= -1;
    }
    gcd = std::move(this_r);
    s = std::move(this_s);
    t = std::move(this_t);
}

bool mp_invert(integer_class &res, const integer_class &a,
               const integer_class &m)
{
    integer_class gcd, s, t;
    mp_gcdext(gcd, s, t, a, m);
    if (gcd != 1) {
        res = 0;
        return false;
    } else {
        mp_fdiv_r(s, s, m); // reduce s modulo m.  undefined behavior when m ==
                            // 0, so don't need to check
        if (s < 0) {
            s += mp_abs(m);
        } // give the canonical representative of s
        res = s;
        return true;
    }
}

// floored modulus
integer_class fmod(const integer_class &a, const integer_class &mod)
{
    integer_class res = a % mod;
    if (res < 0) {
        res += mod;
    }
    return res;
}

void mp_pow_ui(rational_class &res, const rational_class &i, unsigned long n)
{
    integer_class num = numerator(i);   // copy
    integer_class den = denominator(i); // copy
    num = pow(num, n);
    den = pow(den, n);
    res = rational_class(std::move(num), std::move(den));
}

void mp_powm(integer_class &res, const integer_class &base,
             const integer_class &exp, const integer_class &m)
{
    // if exp is negative, interpret as follows
    // base**(exp) mod m 	== (base**(-1))**abs(exp) mod m
    // 						== (base**(-1) mod m) ** abs(exp) mod m
    // where base**(-1) mod m is the modular inverse
    if (exp < 0) {
        integer_class base_inverse;
        if (!mp_invert(base_inverse, base, m)) {
            throw SymEngine::SymEngineException("negative exponent undefined "
                                                "in powm if base is not "
                                                "invertible mod m");
        }
        res = boost::multiprecision::powm(base_inverse, mp_abs(exp), m);
        return;
    } else {
        res = boost::multiprecision::powm(base, exp, m);
        // boost's powm calculates base**exp % m, but uses truncated
        // modulus, e.g. powm(-2,3,5) == -3.  We want powm(-2,3,5) == 2
        if (res < 0) {
            res += m;
        }
    }
}

integer_class step(const unsigned long &n, const integer_class &i,
                   integer_class &x)
{
    SYMENGINE_ASSERT(n > 1);
    unsigned long m = n - 1;
    integer_class &&x_m = pow(x, m);
    return integer_class((integer_class(m * x) + integer_class(i / x_m)) / n);
}

bool positive_root(integer_class &res, const integer_class &i,
                   const unsigned long n)
{
    integer_class x
        = 1; // TODO: make a better starting guess based on (number of bits)/n
    integer_class y = step(n, i, x);
    do {
        x = y;
        y = step(n, i, x);
    } while (y < x);
    res = x;
    if (pow(x, n) == i) {
        return true;
    }
    return false;
}

// return true if i is a perfect nth power, i.e. res**i == n
bool mp_root(integer_class &res, const integer_class &i, const unsigned long n)
{
    if (n == 0) {
        throw std::runtime_error("0th root is undefined");
    }
    if (n == 1) {
        res = i;
        return true;
    }
    if (i == 0) {
        res = 0;
        return true;
    }
    if (i > 0) {
        return positive_root(res, i, n);
    }
    if (i < 0 && (n % 2 == 0)) {
        throw std::runtime_error("even root of a negative is non-real");
    }
    bool b = positive_root(res, -i, n);
    res *= -1;
    return b;
}

integer_class mp_sqrt(const integer_class &i)
{
    // as of 11/1/2016, boost::multiprecision::sqrt() is buggy:
    // https://svn.boost.org/trac/boost/ticket/12559
    // implement with mp_root for now
    integer_class res;
    mp_root(res, i, 2);
    return res;
}

void mp_rootrem(integer_class &a, integer_class &b, const integer_class &i,
                unsigned long n)
{
    mp_root(a, i, n);
    integer_class p = pow(a, n);
    ;
    b = i - p;
}

void mp_sqrtrem(integer_class &a, integer_class &b, const integer_class &i)
{
    a = mp_sqrt(i);
    b = i - boost::multiprecision::pow(a, 2);
}

// return nonzero if i is probably prime.
int mp_probab_prime_p(const integer_class &i, unsigned retries)
{
    if (i % 2 == 0)
        return (i == 2);
    return miller_rabin_test(i, retries);
}

void mp_nextprime(integer_class &res, const integer_class &i)
{
    // simple implementation:  just check all odds bigger than i for primality
    if (i < 2) {
        res = 2;
        return;
    }
    integer_class candidate;
    candidate = (i % 2 == 0) ? i + 1 : i + 2;
    // Knuth recommends 25 trials for a pretty strong likelihood that candidate
    // is prime
    while (!mp_probab_prime_p(candidate, 25)) {
        candidate += 2;
    }
    res = std::move(candidate);
}

unsigned long mp_scan1(const integer_class &i)
{
    if (i == 0) {
        return ULONG_MAX;
    }
    return find_lsb(i, int_<0>());
}

// define simple 2x2 matrix with exponentiation by repeated squaring
// to use in logarithmic-time fibonacci calculation

struct two_by_two_matrix {
    integer_class data[2][2]; // data[1][0] is row 1, column 0 entry of matrix

    two_by_two_matrix(integer_class a, integer_class b, integer_class c,
                      integer_class d)
        : data{{a, b}, {c, d}}
    {
    }
    two_by_two_matrix() : data{{0, 0}, {0, 0}}
    {
    }
    two_by_two_matrix &operator=(const two_by_two_matrix &other)
    {
        this->data[0][0] = other.data[0][0];
        this->data[0][1] = other.data[0][1];
        this->data[1][0] = other.data[1][0];
        this->data[1][1] = other.data[1][1];
        return *this;
    }
    two_by_two_matrix operator*(const two_by_two_matrix &other)
    {
        two_by_two_matrix res;
        res.data[0][0] = this->data[0][0] * other.data[0][0]
                         + this->data[0][1] * other.data[1][0];
        res.data[0][1] = this->data[0][0] * other.data[0][1]
                         + this->data[0][1] * other.data[1][1];
        res.data[1][0] = this->data[1][0] * other.data[0][0]
                         + this->data[1][1] * other.data[1][0];
        res.data[1][1] = this->data[1][0] * other.data[0][1]
                         + this->data[1][1] * other.data[1][1];
        return res;
    }
    // recursive repeated squaring
    two_by_two_matrix pow(unsigned long n)
    {
        if (n == 0) {
            return two_by_two_matrix::identity();
        }
        if (n == 1) {
            return *this;
        }
        if (n == 2) {
            return (*this) * (*this);
        }
        if (n % 2 == 0) {
            return (this->pow(n / 2)).pow(2);
        }
        return ((this->pow((n - 1) / 2)).pow(2)) * (*this);
    }

    static two_by_two_matrix identity()
    {
        return two_by_two_matrix(1, 0, 0, 1);
    }
};

inline two_by_two_matrix fib_matrix(unsigned long n)
{
    two_by_two_matrix x(1, 1, 1, 0);
    return x.pow(n);
}

void mp_fib_ui(integer_class &res, unsigned long n)
{
    // reference: https://www.nayuki.io/page/fast-fibonacci-algorithms
    res = fib_matrix(n).data[0][1];
}

// sets a = Fibonacci(n) and b = Fibonacci(n-1)
void mp_fib2_ui(integer_class &a, integer_class &b, unsigned long n)
{
    // reference: https://www.nayuki.io/page/fast-fibonacci-algorithms
    two_by_two_matrix result_matrix = fib_matrix(n);
    a = result_matrix.data[0][1];
    b = result_matrix.data[1][1];
}

inline two_by_two_matrix luc_matrix(unsigned long n)
{
    two_by_two_matrix multiplier(1, 1, 1, 0);
    two_by_two_matrix start(1, 0, 2, 0);
    return multiplier.pow(n) * start;
}

void mp_lucnum_ui(integer_class &res, unsigned long n)
{
    // implementation based on the following fact:
    // [[1,1],[1,0]]^(n-1)*[2,0,1,0] = [[L(n+1),0][L(n),0]]
    // where L(n) is the nth Lucas number
    res = luc_matrix(n).data[1][0];
}

void mp_lucnum2_ui(integer_class &a, integer_class &b, unsigned long n)
{
    if (n == 0) {
        throw std::runtime_error("index of lucas number cannot be negative");
    }
    two_by_two_matrix result_matrix = luc_matrix(n - 1);
    a = result_matrix.data[0][0];
    b = result_matrix.data[1][0];
}

void mp_fac_ui(integer_class &res, unsigned long n)
{
    // couldn't make boost's template version of factorial work,
    // so implement slow, naive version for now
    res = 1;
    for (unsigned long i = 2; i <= n; ++i) {
        res *= i;
    }
}

void mp_bin_ui(integer_class &res, const integer_class &n, unsigned long r)
{
    // slow, naive implementation
    integer_class x = n - r;
    res = 1;
    for (unsigned long i = 1; i <= r; ++i) {
        res *= x + i;
        res /= i;
    }
}

// this is extremely slow!
bool mp_perfect_power_p(const integer_class &i)
{
    if (i == 0 || i == 1 || i == -1) {
        return true;
    }
    // if i == a**k, with k == pq for some integers p,q
    // then i == a**(pq) == (a**p)**q == (a**q)**p
    // hence i is a pth power and a qth power
    // Hence if i == p**k, then i is a pth power for any p
    // in the prime factorization of k, and it suffices
    // to check whether i is a prime power.

    // the largest possible prime p would arise from the
    // case where i is a prime power of two
    // so check all prime roots up to log(i) base 2.

    unsigned long max = std::ilogb(i.convert_to<double>());

    // treat case p=2 separately b/c mp_root throws exception
    // with an even root of a negative
    if (mp_perfect_square_p(i)) {
        return true;
    }
    integer_class p(2);
    integer_class root(0);
    while (true) {
        mp_nextprime(p, p);
        if (p > max) {
            return false;
        }
        if (mp_root(root, i, p.convert_to<unsigned long>())) {
            return true;
        }
    }
}

bool mp_perfect_square_p(const integer_class &i)
{
    if (i < 0) {
        return false;
    }
    integer_class root;
    return mp_root(root, i, 2);
}

// according to the gmp documentation, the behavior of the
// corresponding function mpz_legendre is
// undefined if n is not a positive odd prime.
// hence we treat n as though it were a positive odd prime
// but it is up to the caller to very this.
int mp_legendre(const integer_class &a, const integer_class &n)
{
    integer_class res;
    mp_powm(res, a, integer_class((n - 1) / 2), n);
    return res <= 1 ? res.convert_to<int>() : -1;
}

// private function that computes jacobi symbols
// without checking that arguments satisfy a >= 0
// and n is odd.
int unchecked_jacobi(const integer_class &a, const integer_class &n)
{
    // https://en.wikipedia.org/wiki/Jacobi_symbol#Calculating_the_Jacobi_symbol
    if (a == 1) {
        return 1;
    }
    integer_class num = a;
    integer_class den = n;
    // (1) Reduce the "numerator" modulo the "denominator"
    num = fmod(num, den);

    // (2) Extract any factors of 2 from the "numerator"
    unsigned long factors_of_two = 0;
    while (num % 2 == 0 && num != 0) {
        num /= 2; // use a shift instead of division here? faster?
        ++factors_of_two;
    }
    int product_of_twos = 1; // (2 | den)**factors_of_two

    // (2 | den) is -1 iff den % 8 = 3 or den % 8 = 5.
    // If factors_of_two is odd and (2 | den) is -1,
    // then (2 | den)**factors_of_two is -1.
    // Otherwise, (2 | den)**factors_of_two is 1.
    int den_mod_8 = fmod(den, 8).convert_to<int>();
    if ((factors_of_two % 2 == 1) && (den_mod_8 == 3 || den_mod_8 == 5)) {
        product_of_twos = -1;
    }

    // (3) If the remaining "numerator" (after extraction of twos) is 1,
    // then the remaining jacobi symbol is 1.
    // If the "numerator" and "denominator" are not coprime, result is 0.
    if (num == 1) {
        return product_of_twos;
    }
    if (boost::multiprecision::gcd(num, den) != 1) {
        return 0;
    }

    // (4) Otherwise, the "numerator" and "denominator" are now odd
    // positive coprime integers, so we can flip the symbol
    // with quadratic reciprocity, then return to step (1)

    // (num | den) == (den | num), unless num % 4 and den %4 are both
    // 3, in which case (num | den) == (-1) * (den | num)
    int quadratic_reciprocity_factor = 1;
    if ((fmod(num, 4) == 3) && (fmod(den, 4) == 3)) {
        quadratic_reciprocity_factor = -1;
    }
    return product_of_twos * quadratic_reciprocity_factor
           * unchecked_jacobi(den, num);
}

// public interface for computing jacobi symbols.  performs checking.
int mp_jacobi(const integer_class &a, const integer_class &n)
{
    if (n < 0) {
        throw std::runtime_error("jacobi denominator must be positive");
    }
    if (n % 2 == 0) {
        throw std::runtime_error("jacobi denominator must be odd");
    }
    return unchecked_jacobi(a, n);
}

int mp_kronecker(const integer_class &a, const integer_class &n)
{
    /*
    https://en.wikipedia.org/wiki/Kronecker_symbol
    We compute the Kronecker symbol in terms of the Jacobi symbol.
    For an integer n!=0, let n = u * PRODUCT (p_i**e_i), where u = -1 or 1 and
    the p_i's and e_i's are the primes and corresponding powers
    in the prime factorization of |n|.
    Then for an integer a, the Kronecker symbol is given by
    (a | n) = (a | u) * PRODUCT [(a | p_i)**e_i]
    where
    (a | u) is -1 if u is -1 and a < 0. Otherwise it is 1.
    (a | 2) is 0 if a is even, 1 if (a mod 8) == 1 or 7, and -1 if (a mod 8) ==
    3 or 5.
    (a | p) is the Legendre symbol if p is an odd prime.

    Let j be the power of the prime 2 in n's prime factorization, and let
    m = |n|/(2**j) -- that is, m is |n| with all factors of 2 extracted.

    Notice that, if p_1 is 2, then
    PRODUCT [(a | p_i)**e_i]
    == (a | 2)**j * PRODUCT [(a | p_i)**e_i]	here the p_i's are the remaining
    (odd) prime factors
    == (a | 2)**j * (a | m), 					here (a | m) is the Jacobi
    symbol, because m>0 is odd

    Thus,
    (a | n) == (a | u) * (a | 2)**j * (a | m)	if n is even
    (a | n) == (a | u) * (a | m) 				if n is odd
    */

    if (n == 0) {
        throw std::runtime_error("second arg of Kronecker cannot be zero");
    }

    // Compute (a | u)
    int kr_a_u = 1;
    if (n.sign() == -1 && a < 0) {
        kr_a_u = -1;
    }

    // Compute m, j
    integer_class m = boost::multiprecision::abs(n);
    unsigned long j = 0;
    while (m % 2 == 0 && m != 0) { // while m is even
        m /= 2;                    // implement with shift?  faster?
        ++j;
    }

    // if n is even, compute (a | 2)**j
    int kr_a_2 = 0;
    int kr_a_2_to_j = 0;
    int a_mod_8 = fmod(a, 8).convert_to<int>();

    if (a % 2 != 0) {
        kr_a_2 = (a_mod_8 == 1 || a_mod_8 == 7) ? 1 : -1;
        kr_a_2_to_j = (kr_a_2 == -1 && (j % 2 != 0))
                          ? -1
                          : 1; //(-1)**odd == -1; (-1)**even=(1)**integer=1
    }

    if (n % 2 == 0) {
        return kr_a_u * kr_a_2_to_j * unchecked_jacobi(a, m);
    } else {
        return kr_a_u * unchecked_jacobi(a, m);
    }
}

} // SymEngine

#endif // SYMENGINE_INTEGER_CLASS == SYMENGINE_BOOSTMP