#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdLbeta,FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<var> x(3.0,1.3);
  fvar<var> z(6.0,1.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) + digamma(6.0) - (1.0 + 1.3) *
                  digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0),g[0]);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0),g[1]);
}
TEST(AgradFwdLbeta,FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<var> x(3.0,1.3);
  double z(6.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) - (1.3) *
                  digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0),g[0]);
}
TEST(AgradFwdLbeta,Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  double x(3.0);
  fvar<var> z(6.0,1.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0),g[0]);
}
TEST(AgradFwdLbeta,FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<var> x(3.0,1.3);
  fvar<var> z(6.0,1.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) + digamma(6.0) - (1.0 + 1.3) *
                  digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(1.3 * 0.39493407 - 2.3 * 0.11751201,g[0]);
  EXPECT_FLOAT_EQ(0.18132296 - 2.3 * 0.11751201,g[1]);
}
TEST(AgradFwdLbeta,FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<var> x(3.0,1.3);
  double z(6.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(1.3 * digamma(3.0) - (1.3) *
                  digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(1.3 * 0.39493407 - 1.3 * 0.11751201,g[0]);
}
TEST(AgradFwdLbeta,Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  double x(3.0);
  fvar<var> z(6.0,1.0);
  fvar<var> a = lbeta(x,z);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(3.0 + 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201,g[0]);
}
TEST(AgradFwdLbeta,FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[1]);
}
TEST(AgradFwdLbeta,FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta,Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), g[0]);
}
TEST(AgradFwdLbeta,FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.39493407 - 0.11751201, g[0]);
  EXPECT_FLOAT_EQ(-0.11751202, g[1]);
}
TEST(AgradFwdLbeta,FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.11751202, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.11751202, g[0]);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201, g[1]);
}
TEST(AgradFwdLbeta,FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(digamma(3.0) - digamma(9.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.39493407 - 0.11751201, g[0]);
}
TEST(AgradFwdLbeta,Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  EXPECT_FLOAT_EQ(lbeta(3.0,6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(digamma(6.0) - digamma(9.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.18132296 - 0.11751201, g[0]);
}
TEST(AgradFwdLbeta,FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(0.013793319, g[0]);
  EXPECT_FLOAT_EQ(0.013793319, g[1]);
}
TEST(AgradFwdLbeta,FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(6.0);

  fvar<fvar<var> > a = lbeta(x,y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.140320487123420796890184645287, g[0]);
}
TEST(AgradFwdLbeta,Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using boost::math::digamma;
  using stan::math::lbeta;

  double x(3.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = lbeta(x,y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.0189964130493467228161105712126, g[0]);
}

struct lbeta_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return lbeta(arg1,arg2);
  }
};

TEST(AgradFwdLbeta, nan) {
  lbeta_fun lbeta_;
  test_nan_mix(lbeta_,3.0,5.0,false);
}


#include <boost/test/unit_test.hpp>
#include "functions/zero.hh"
#include "functions/std_functions.hh"
#include "functions/operators.hh"
#include "functions/all_simplifications.hh"

BOOST_AUTO_TEST_CASE(zero_function_test) {
  using namespace manifolds;

  Zero z;
  Sin s;
  static_assert(std::is_same<decltype(s + z), Sin>::value,
                "Failed to simplify Sin + 0");

  static_assert(std::is_same<decltype(z + s), Sin>::value,
                "Failed to simplify 0 + Sin");

  static_assert(std::is_same<decltype(z + z), Zero>::value,
                "Failed to simplify 0 + 0");

  static_assert(std::is_same<decltype(s * z), Zero>::value,
                "Failed to simplify Sin * 0");

  static_assert(std::is_same<decltype(z * s), Zero>::value,
                "Failed to simplify 0 * Sin");

  static_assert(std::is_same<decltype(z * z), Zero>::value,
                "Failed to simplify 0 * 0");
}


#include "pose_2d.hpp"
#include "scan_simulator_2d.hpp"
#include "car_state.hpp"
#include "car_params.hpp"
#include "car_odom.hpp"
#include "car_obs.hpp"
#include "ackermann_kinematics.hpp"
#include "ks_kinematics.hpp"
#include "st_kinematics.hpp"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <vector>
#include <math.h>
using namespace racecar_simulator;
class RaceCar {
public:
    RaceCar(double time_step, double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass, bool is_ego);
    virtual ~RaceCar();
    void set_map(std::vector<double> &map, int map_height, int map_width, double map_resolution, double origin_x, double origin_y, double free_threshold);
    void reset();
    void reset_bypose(Pose2D pose);
    CarObs update_scan();
    void update_pose();
    void update_op_poses(const std::vector<Pose2D> &op_poses);
    void set_velocity(double vel);
    void set_steering_angle(double ang);
    Pose2D get_pose();
    double get_accel();
    double get_steer_vel();
    void update_params(double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
private:
    CarState state;
    CarParams params;
    CarOdom odom;
    Pose2D pose;
    ScanSimulator2D scan_simulator;
    bool map_exists;
    bool ego;
    double scan_distance_to_base_link;
    double car_width, car_length;
    double max_speed, max_steering_angle;
    double max_accel, max_decel, max_steering_vel;
    double accel, steer_angle_vel;
    double width;
    double delt_t;
    std::vector<double> cosines;
    double scan_fov, scan_ang_incr;
    std::vector<double> scan_angles;
    std::vector<double> current_scan;
    bool in_collision = false;
    double collision_angle;
    double ttc_threshold;
    double map_free_threshold;
    const double PI = 3.141592653;
    std::vector<double> car_distances;
    std::vector<Pose2D> opponent_poses;
    int steering_delay_buffer_length;
    std::vector<double> steer_buffer;
    Eigen::Matrix4d get_transformation_matrix(const Pose2D &pose);
    Pose2D transform_between_frames(const Pose2D &p1, const Pose2D &p2);
    void ray_cast_opponents(std::vector<double> &scan, const Pose2D &scan_pose);
    void check_ttc();
    void set_accel(double acceleration);
    void set_steering_angle_vel(double steer_vel);
    double compute_steer_vel(double desired_angle);
    void compute_accel(double desired_velocity);
    double get_range(const Pose2D &pose, double beam_theta, Eigen::Vector2d line_segment_a, Eigen::Vector2d line_segment_b);
    bool are_collinear(Eigen::Vector2d pt_a, Eigen::Vector2d pt_b, Eigen::Vector2d pt_c);
    double cross(Eigen::Vector2d v1, Eigen::Vector2d v2);
};


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

#include <Common.h>
#include <miosix.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iomanip>
#include <iostream>

#include "drivers/HardwareTimer.h"

using namespace std;
using namespace Eigen;
using namespace miosix;
using miosix::Thread;

void nProducts2Mat(int n, MatrixXd& m1, MatrixXd& m2)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    int i       = 0;
    uint32_t t1 = hrclock.tick();

    for (i = 0; i < n; i++)
        MatrixXd m = m1 * m2;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for %d products using 2 matrices: %f [ms] \n\n", n, time);
}

void nProducts3Mat(int n, MatrixXd& m1, MatrixXd& m2, MatrixXd& m3)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    int i       = 0;
    uint32_t t1 = hrclock.tick();

    for (i = 0; i < n; i++)
        MatrixXd m = m1 * m2 * m3;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for %d products using 3 matrices: %f [ms] \n\n", n, time);
}

void kalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, MatrixXd& m4,
                      MatrixXd& m5, MatrixXd& eye, MatrixXd& v1, MatrixXd& v2)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    auto x = v1;
    auto P = m2;
    auto Q = m3;
    auto R = m5;
    auto y = v2;

    uint32_t t1 = hrclock.tick();

    auto F     = 0.5 * m1;
    x          = F * x;
    P          = F * P * (F.transpose()) + Q;
    auto H     = 2 * m4;
    auto K     = P * H.transpose() * ((H * P * H.transpose() + R).inverse());
    auto U     = K * (y.transpose() - H * x);
    auto x_new = x + U;
    auto P_new = (eye - K * H) * P;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for a single kalman cycle: %f [ms] \n\n", time);
}

void sparseKalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3,
                            MatrixXd& m4, MatrixXd& m5, MatrixXd& eye,
                            MatrixXd& v1, MatrixXd& v2)
{
    // H, P and R can't be sparse since their product needs to be inverted.
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    auto x = v1;
    auto P = m2;
    auto Q = m3.sparseView();
    auto R = m5;
    auto y = v2;

    uint32_t t1 = hrclock.tick();

    auto F = 0.5 * m1.sparseView();
    x      = F * x;
    P      = F * P * (F.transpose()) + Q;
    auto H = 2 * m4;
    auto K = (P * (H.transpose()) * ((H * P * H.transpose() + R).inverse()))
                 .sparseView();
    auto U     = (K * (y.transpose() - H * x)).sparseView();
    auto x_new = x + U;
    auto P_new = (eye - K * H) * P;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE(
        "\nTime for a single kalman cycle with some sparse matrices: %f [ms] "
        "\n\n",
        time);
}

void determinant(MatrixXd& m1)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    uint32_t t1 = hrclock.tick();

    float det = m1.determinant();

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime to find the determinant: %f [ms] \n\n", time);
}

int main()
{
    static const int ROWS = 10;
    static const int COL  = 10;
    static const int N    = 100;

    MatrixXd m1  = MatrixXd::Random(ROWS, COL);
    MatrixXd m2  = MatrixXd::Random(ROWS, COL);
    MatrixXd m3  = MatrixXd::Random(ROWS, COL);
    MatrixXd m4  = MatrixXd::Random(ROWS, COL);
    MatrixXd m5  = MatrixXd::Random(ROWS, COL);
    MatrixXd eye = MatrixXd::Identity(ROWS, ROWS);

    MatrixXd v1 = MatrixXd::Random(ROWS, 1);
    MatrixXd v2 = MatrixXd::Random(1, ROWS);

    determinant(m1);
    nProducts2Mat(N, m1, m2);
    nProducts3Mat(N, m1, m2, m3);
    kalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2);
    sparseKalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2);

    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE weierstrass_precomputation_components_test

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>

#include <nil/crypto3/zk/components/algebra/pairing/detail/mnt4.hpp>
#include <nil/crypto3/zk/components/algebra/pairing/detail/mnt6.hpp>

#include <nil/crypto3/zk/components/algebra/pairing/weierstrass/precomputation.hpp>

#include <nil/crypto3/algebra/random_element.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::algebra;

template<typename CurveType>
void test_element_g1_precomp() {
    components::blueprint<typename CurveType::scalar_field_type> bp;
    typename CurveType::pairing::pair_curve_type::g1_type::value_type g_val =
        algebra::random_element<typename CurveType::pairing::pair_curve_type::scalar_field_type>() *
        CurveType::pairing::pair_curve_type::g1_type::value_type::one();

    element_g1<CurveType> g(bp);
    g1_precomputation<CurveType> precomp;
    precompute_G1_component<CurveType> do_precomp(bp, g, precomp);
    do_precomp.generate_r1cs_constraints();

    g.generate_r1cs_witness(g_val);
    do_precomp.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename CurveType::pairing::g1_precomp const_precomp(bp, g_val);

    typename CurveType::pairing::pair_curve_type::pairing::affine_ate_g1_precomp native_precomp =
        CurveType::pairing::pair_curve_type::affine_ate_precompute_g1(g_val);
    BOOST_CHECK(precomp.PY_twist_squared->get_element() == native_precomp.PY_twist_squared);
    BOOST_CHECK(const_precomp.PY_twist_squared->get_element() == native_precomp.PY_twist_squared);
}

template<typename CurveType>
void test_element_g2_precomp() {
    components::blueprint<typename CurveType::scalar_field_type> bp;
    typename CurveType::pairing::pair_curve_type::g2_type::value_type g_val =
        algebra::random_element<typename CurveType::pairing::pair_curve_type::scalar_field_type>() *
        CurveType::pairing::pair_curve_type::g2_type::value_type::one();

    element_g2<CurveType> g(bp);
    g2_precomputation<CurveType> precomp;
    precompute_G2_component<CurveType> do_precomp(bp, g, precomp);
    do_precomp.generate_r1cs_constraints();

    g.generate_r1cs_witness(g_val);
    do_precomp.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename CurveType::pairing::pair_curve_type::pairing::affine_ate_g2_precomp native_precomp =
        CurveType::pairing::pair_curve_type::affine_ate_precompute_g2(g_val);

    BOOST_CHECK(precomp.coeffs.size() - 1 ==
                native_precomp.coeffs.size());    // the last precomp is unused, but remains for convenient programming
    for (std::size_t i = 0; i < native_precomp.coeffs.size(); ++i) {
        BOOST_CHECK(precomp.coeffs[i]->RX->get_element() == native_precomp.coeffs[i].old_RX);
        BOOST_CHECK(precomp.coeffs[i]->RY->get_element() == native_precomp.coeffs[i].old_RY);
        BOOST_CHECK(precomp.coeffs[i]->gamma->get_element() == native_precomp.coeffs[i].gamma);
        BOOST_CHECK(precomp.coeffs[i]->gamma_X->get_element() == native_precomp.coeffs[i].gamma_X);
    }

    std::cout << "number of constraints for G2 precomp: " << bp.num_constraints() << std::endl;
}

BOOST_AUTO_TEST_SUITE(weierstrass_precomputation_components_test_suite)

BOOST_AUTO_TEST_CASE(weierstrass_precomputation_components_test) {

    test_all_set_commitment_components<curves::mnt4<298>>();
    test_all_set_commitment_components<curves::mnt6<298>>();
}

BOOST_AUTO_TEST_SUITE_END()


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------
#define BOOST_SIMD_NO_DENORMALS
#define BOOST_SIMD_NO_INVALIDS
#include <simd_bench.hpp>
#include <boost/simd/function/simd/log.hpp>
#include <boost/simd/pack.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;

DEFINE_SIMD_BENCH(simd_musl_log, bs::musl_(bs::log));

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_musl_log, NS_BENCH_IEEE_TYPES>(0, 1000);
}


#include <CGAL/Timer.h>

#include <nanoflann.hpp>

#include <boost/lexical_cast.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <CGAL/Memory_sizer.h>
#include <CGAL/IO/io.h>

using namespace std;
using namespace nanoflann;

template <typename T>
struct Point
{
  Point(T x, T y, T z)
    : x(x), y(y), z(z)
  {}

  T   x,y,z;
};

template <typename T>
ostream& operator <<(ostream& os, const Point<T>& p)
{
  os << p.x << " " << p.y << " " << p.z ;
  return os;
}

// This is an exampleof a custom data set class
template <typename T>
struct PointCloud
{
  std::vector<Point<T> >  pts;

        // Must return the number of data points
        inline size_t kdtree_get_point_count() const { return pts.size(); }

        // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
        inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t /* size */) const
        {
                const T d0=p1[0]-pts[idx_p2].x;
                const T d1=p1[1]-pts[idx_p2].y;
                const T d2=p1[2]-pts[idx_p2].z;
                return d0*d0+d1*d1+d2*d2;
        }

        // Returns the dim'th component of the idx'th point in the class:
        // Since this is inlined and the "dim" argument is typically an immediate value, the
        //  "if/else's" are actually solved at compile time.
        inline T kdtree_get_pt(const size_t idx, int dim) const
        {
                if (dim==0) return pts[idx].x;
                else if (dim==1) return pts[idx].y;
                else return pts[idx].z;
        }

        // Optional bounding-box computation: return false to default to a standard bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
        //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
        template <class BBOX>
        bool kdtree_get_bbox(BBOX & /* bb */) const { return false; }

};

template <typename T>
void generateRandomPointCloud(PointCloud<T> &point, istream& is, int n)
{
  T x, y, z;
  for(int i=0; i < n; i++){
    CGAL::read(is,x);
    CGAL::read(is,y);
    CGAL::read(is,z);

    point.pts.push_back(Point<T>(x,y,z));
  }
  std::cout << "Read "<< point.pts.size() << " points\n";
}

template <typename num_t>
void kdtree_demo(int argc, char** argv)
{
        PointCloud<num_t> cloud;
        int n;

        // Generate points:
        std::ifstream input(argv[1], std::ios::in | std::ios::binary);
        CGAL::IO::set_binary_mode(input);
        //        input >> n >> n; // dimension and # of points
        CGAL::read(input,n);
        CGAL::read(input,n);
        generateRandomPointCloud(cloud, input, n);

        std::vector<Point<double> > queries;
        std::ifstream queries_stream(argv[2], std::ios::in | std::ios::binary);
        CGAL::IO::set_binary_mode(queries_stream);
        CGAL::read(queries_stream,n);
        CGAL::read(queries_stream,n);
        // queries_stream >> n >> n;
        double x,y,z;
        for(int i=0; i < n; i++){
          CGAL::read(queries_stream,x);
          CGAL::read(queries_stream,y);
          CGAL::read(queries_stream,z);
          queries.push_back(Point<double>(x,y,z));
        }

        int runs = (argc>3) ? boost::lexical_cast<int>(argv[3]) : 1;
        std::cerr << "runs = "  << runs <<std::endl;

        int bucketsize = (argc>4) ? boost::lexical_cast<int>(argv[4]) : 10;
        std::cerr << "bucketsize = "  << bucketsize <<std::endl;

        num_t query_pt[3] = { 0, 0, 0};

        CGAL::Timer timer;
        timer.start();
        // construct a kd-tree index:
        typedef KDTreeSingleIndexAdaptor<
                L2_Simple_Adaptor<num_t, PointCloud<num_t> > ,
                PointCloud<num_t>,
                3 /* dim */
                > my_kd_tree_t;

        my_kd_tree_t   index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(bucketsize /* max leaf */) );
        index.buildIndex();

        timer.stop();
        std::cout << "construction time: " << timer.time() << " sec" << std::endl;
        std::cerr << "Tree statistics:" << std::endl;
        std::cerr << "Number of items stored: "
          << index.items << std::endl;
        std::cerr << "Number of nodes: "
          << index.internals << std::endl;
        std::cerr << " Tree depth: " << index.depth() << std::endl;
        // do a knn search
        const size_t num_results = 10;
        size_t ret_index[num_results];
        num_t out_dist_sqr[num_results];
        int size = queries.size();

        std::cout << "start search" << std::endl;
        bool dump = true;
        double sum = 0;

        for(int i=0;i<runs;++i){

          nanoflann::KNNResultSet<num_t> resultSet(num_results);

          for(int i = 0 ; i < size; i++){
            query_pt[0] = queries[i].x;
            query_pt[1] = queries[i].y;
            query_pt[2] = queries[i].z;
            resultSet.init(ret_index, out_dist_sqr );
            timer.reset();
          timer.start();
            index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10,0));
            timer.stop();

            for (size_t k=0; k<num_results; ++k){
              if(dump)
                std::cerr <<cloud.pts[ret_index[k]] << std::endl;
            }
                    dump=false;
                    sum += timer.time();
          }

        }
        std::cerr << index.count_items <<" items\n";
        std::cerr << index.count_leafs <<" leaf\n";
        std::cerr << index.count_internals <<" internals visited\n";

        std::cerr<<std::endl << "total: " << sum << " sec\n";
        if(runs>1){
          std::cerr << "average: " << sum/runs << " sec\n";
        }
          std::cerr << "done\n";
}

int main(int argc, char** argv)
{
  kdtree_demo<double>(argc, argv);

  return 0;
}


#include "line.hpp"

#include <Eigen/Geometry>

namespace neon
{
line2::line2(line_quadrature::point const p)
    : line_interpolation(std::make_unique<line_quadrature>(p), 2)
{
    this->precompute_shape_functions();
}

void line2::precompute_shape_functions()
{
    using coordinates_type = std::tuple<int, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<coordinates_type, 2> constexpr local_coordinates{{{0, -1.0}, {1, 1.0}}};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 2);

    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, xi] = coordinates;

        vector N(2);
        matrix dN(2, 1);

        N(0) = 1.0 / 2.0 * (1.0 - xi);
        N(1) = 1.0 / 2.0 * (1.0 + xi);

        dN(0, 0) = -1.0 / 2.0;
        dN(1, 0) = 1.0 / 2.0;

        local_quadrature_coordinates(l, 0) = xi;

        N_matrix.row(l) = N;

        return std::make_tuple(N, dN);
    });

    // Compute extrapolation algorithm matrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 2);

    for (auto const& [a, xi_a] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = xi_a;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double line2::compute_measure(matrix const& nodal_coordinates) const
{
    return (nodal_coordinates.col(0) - nodal_coordinates.col(1)).norm();
}

line3::line3(line_quadrature::point const p)
    : line_interpolation(std::make_unique<line_quadrature>(p), 3)
{
    this->precompute_shape_functions();
}

void line3::precompute_shape_functions()
{
    using coordinates_type = std::tuple<int, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<coordinates_type, 3> constexpr local_coordinates{{{0, -1.0}, {1, 0.0}, {2, 1.0}}};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 2);

    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, xi] = coordinates;

        vector N(3);
        matrix rhea(3, 1);

        N(0) = 1.0 / 2.0 * xi * (xi - 1.0);
        N(1) = 1.0 - std::pow(xi, 2);
        N(2) = 1.0 / 2.0 * xi * (xi + 1.0);

        rhea(0, 0) = 1.0 / 2.0 * (2.0 * xi - 1.0);
        rhea(1, 0) = -2.0 * xi;
        rhea(2, 0) = 1.0 / 2.0 * (2.0 * xi + 1.0);

        local_quadrature_coordinates(l, 0) = xi;

        N_matrix.row(l) = N;

        return std::make_tuple(N, rhea);
    });

    // Compute extrapolation algorithm matrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 2);

    for (auto const& [a, xi_a] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = xi_a;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double line3::compute_measure(matrix const& nodal_coordinates) const
{
    return (nodal_coordinates.col(0) - nodal_coordinates.col(2)).norm();
}
}


#ifndef HEADER_SBGATTRAJECTORY
#define HEADER_SBGATTRAJECTORY

#include <vector>
#include <armadillo>

/**
  @class  SBGATTrajectory
  @author Benjamin Bercovici
  @author Jay McMahon

  @brief  Trajectory-generation class

  @details This class can be used to generate trajectories. Trajectories can be loaded from a file
 or generated under a Keplerian dynamics assumption

*/
class SBGATTrajectory{

public:

	/**
	Generates a keplerian trajectory at the prescribed time
	@param positions each element in positions holds the inertial cartesian position the corresponding time
	@param velocities each element in velocities holds the inertial cartesian velocity the corresponding time
	@param times vector of times. First time defines the trajectory epoch
	@param elements vector of orbital elements (a,e,i,Omega,omega,M0) with
	- a : semi-major axis (m)
	- e : eccentricity
	- i : inclination in [0,pi] (rad)
	- Omega : right-ascension of ascending node in [0,2pi] (rad)
	- omega : longitude of perigee in [0,2pi] (rad)
	- M0 : mean anomaly at epoch (rad)
	@param mu standard gravitational parameter of orbited body (kg^3/s^2)
	*/
	void GenerateKeplerianTrajectory(
		std::vector<arma::vec> & positions,
		std::vector<arma::vec> & velocities,
		const std::vector<double> &  times,
		const arma::vec & elements,
		const double & mu);

protected:

};

#endif


#include "my_nuutila.cpp"
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/transitive_closure.hpp>

using namespace boost;

#ifndef TYPEDEF
#define TYPEDEF

typedef adjacency_list <vecS, vecS, directedS> Graph;
typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
typedef typename graph_traits<Graph>::vertex_iterator vertex_iter;
typedef graph_traits<adjacency_list<vecS, vecS, directedS> >::vertex_descriptor Vertex;
typedef typename property_map<Graph, vertex_index_t>::type IndexMap;

#endif

int main(int, char*[])
{
    //An example of how to design a graph "by hand" without using Graphml and stdin
    /*enum { A, B, C, D, E, F, G, H, I, N };
    const int num_nodes = N;
    const char* name = "ABCDEFGHI";

    // writing out the edges in the graph
    typedef std::pair<int, int> Edge;
    Edge edge_array[] =
            { Edge(A,B), Edge(B,A), Edge(A,C), Edge(C,A), Edge(B,D), Edge(C,D),
              Edge(E,C), Edge(E,F), Edge(F,D), Edge(D,F), Edge(E,H), Edge(H,F), Edge(H,G), Edge(G,E),
              Edge(I,G), Edge(I,H), Edge(I,I)};

    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    Graph g(edge_array, edge_array + num_arcs, num_nodes);*/

    //This function takes a graph formatted by graphml fashion from the stdin
    Graph g;
    dynamic_properties dp;
    read_graphml(std::cin, g, dp);

    //Printing the graph
    std::cout << "A directed graph:" << std::endl;
    print_graph(g, get(vertex_index,g));
    std::cout << std::endl;

    //NuutilaClass instance
    NuutilaClass<typeInt, typeInt, typeBool> nuutila(&g);
    std::vector<int>* root = nuutila.nuutila_scc();

    //Printing the results
    IndexMap index = get(vertex_index,g);

    for (int i = 0; i != root->size(); ++i){
        std::cout << index[i] << " -> " << index[(*root)[i]] << std::endl;
    }
    return 0;
}


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

The MIT License (MIT)

Copyright (c) 2013-2017 Joel E. Merritt

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.

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

//  TODO: Move testing to test.cpp.

//  This repo implements a Gauss-Newton non-linear least squares solver
//  to do camera resection. That is, the solver determines the camera's
//  position, orientation and pixel scale given the 2d screen
//  coordinates and 3d cartesian coordinates of a number of points.
//
//  The algorithm uses a Gauss-Newton non-linear least squares solver.
//
//  https://en.wikipedia.org/wiki/Gauss-Newton_algorithm
//
//  This implementation uses zero-based arrays unlike the Wikipedia form.
//
//  Given there are m functions r[0],r[1],r[2],...r[m - 1], each a
//  function of n variables beta[0],beta[1],beta[2],...beta[n - 1],
//
//             m-1
//   minimize  Sum  ( r[i](beta[0],beta[1]...) )^2
//             i=0
//
//  To implement this two classes are defined. The first,
//  FunctionObject, represents each element of the r function vector.
//  The second, GNSolver (Gauss-Newton Solver) implements the actual
//  solver.
//
//  In this implementation the r vector of functions is defined as
//  follows:
//
//  for r[i] where i is even, r[i] is the distance between the measured x
//  screen coordinate of point number i/2  and the x coordinate of its
//  back-projection
//  for r[i] where i is odd, r[i] is the distance between the measured y
//  screen coordinate of point (i - 1)/2 and the y coordinate of its
//  back-projection
//
//  The beta variables correspond to the camera parameters to be
//  determined.
//
//  In this case, we adjust the camera parameters to minimize the sum of
//  the square of the distances between each 2d point in the image and
//  its calculated 2d position based on its 3d coordinates and the
//  camera parameters.
//
//  This main function tests the FunctionObject and GNSolver classes by
//  constructing objects to test a set of points with known screen
//  coordinates and 3d coordinates to see if the solver produces a
//  reasonable solution.
//

#include <iostream>
#include <iomanip>
#include <vector>
#include <Eigen/Dense>

#include "function_object.h"
#include "g_n_solver.h"

#define NUM_POINTS 6

typedef struct PointData
{
  double xp;
  double yp;
  double x;
  double y;
  double z;
} PointData;

typedef std::vector<PointData> PointDataList;

//
//  Function: PointDataListPrint3
//
//  Print point data.
//  screen x, y; 3d coords x, y, z.
//
//  Input parameters:
//
//  a -- list of points
//
// Print x_screen y_screen x_3d y_3d z_3d for each point

void PointDataListPrint3(PointDataList const &a)
{
  std::cout << std::endl << "type: Point_Data_List2" << std::endl;
  int size = a.size();
  std::cout << "num elements: " << size << std::endl;

  std::cout.setf(std::ios::fixed, std::ios::floatfield);
  std::cout.precision(6);
  for(int i = 0;i < size;i++)
  {
    std::cout << std::setw(12) << a[i].xp << " " << \
      std::setw(12) << a[i].yp << "   " << \
      std::setw(12) << a[i].x << " " << \
      std::setw(12) << a[i].y << " " << \
      std::setw(12) << a[i].z << std::endl;
  }

  return;
}

//
//  Function: PointDataListPrint4
//
//  Print point data.
//  screen x, y; backprojected x, y; 3d coords x, y, z.
//
//  Input parameters:
//
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

void PointDataListPrint4(FunctionObjectList const &r,
                         Eigen::VectorXd const &beta)
{
  std::cout << std::endl << "type: Point_Data_List2" << std::endl;
  int size = r.size() >> 1;
  std::cout << "num elements: " << size << std::endl;

  std::cout.setf(std::ios::fixed, std::ios::floatfield);
  std::cout.precision(6);
  for(int i = 0;i < size;i++)
  {
    int i2 = 2 * i;
    std::cout << std::setw(12) << r[i2].xp() << " " << \
      std::setw(12) << r[i2].yp() << "   (" << \
      std::setw(12) << r[i2].EvalBackProject(beta) << \
      " " << std::setw(12) << \
      r[i2 + 1].EvalBackProject(beta) << ")    " << \
      std::setw(12) << r[i2].x3d() << " " << \
      std::setw(12) << r[i2].y3d() << " " << \
      std::setw(12) << r[i2].z3d() << std::endl;
  }

  return;
}

//
//  Function: main
//
//  Execute main program function.
//
//  main sets up the current hard-wired test
//
//  Input parameters:
//
//  argc -- argument count
//  argv -- argument vector
//

int main(int argc, char **argv)
{
  PointDataList points;

  double k = 1024.0;
  double tx = 10.0;
  double ty = 0.0;
  double tz = 10.0;
  double rx = 0.0;
  double ry = 45.0;
  double rz = 0.0;
  Eigen::VectorXd beta(7);
  Eigen::VectorXd beta_solved;

  if(argc < 2)
  {
    std::cerr << "usage: " << argv[0] << " pointDataFile" << std::endl;

    return -1;
  }

  FILE *file = fopen(argv[1], "r");
  if (file == NULL)
  {
    std::cerr << "can't open " << argv[1] << std::endl;

    return -1;
  }

  //  Initialize beta vector with initial estimate of camera parameters.
  beta << k, tx, ty, tz, rx, ry, rz;

  //  Initialize r function vector, constructing the function of each
  //  element.
  FunctionObjectList r;
  int i = 0;
  while(1)
  {
    PointData cur_point;
    int ret = fscanf(file, "%lf %lf %lf %lf %lf", &cur_point.xp,
        &cur_point.yp, &cur_point.x, &cur_point.y, &cur_point.z);
    if(ret != 5)
    {
      break;
    }
    cur_point.xp -= 1632.0;
    cur_point.yp -= 1224.0;
    FunctionObject r0(cur_point.xp, cur_point.yp, cur_point.x,
        cur_point.y, cur_point.z, 2 * i);
    FunctionObject r1(cur_point.xp, cur_point.yp, cur_point.x,
        cur_point.y, cur_point.z, 2 * i + 1);

    points.push_back(cur_point);
    r.push_back(r0);
    r.push_back(r1);

    i++;
  }
  fclose(file);

  PointDataListPrint3(points);

  std::cout << std::endl;

  // Print initial beta vector.
  std::cout << "**********INPUT************" << std::endl;
  std::cout << "BETA" << std::endl;
  std::cout << beta << std::endl;
  std::cout << std::endl;

  // Solve the problem.
  beta_solved = GNSolver()(r, beta);

  // Print solved beta vector.
  std::cout << "BETA SOLVED" << std::endl;
  std::cout << beta_solved << std::endl;
  std::cout << std::endl;

  printf("r %f\n", r[11](beta_solved));

  PointDataListPrint4(r, beta_solved);

  return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2009 Trustees of Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek, Michael Hansen
//
// 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 <iostream>
#include <vector>

#include <boost/foreach.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/pending/disjoint_sets.hpp>

/*

  This example shows how to use the disjoint set data structure
  to compute the connected components of an undirected, changing
  graph.

  Sample output:

  An undirected graph:
  0 <--> 1 4
  1 <--> 0 4
  2 <--> 5
  3 <-->
  4 <--> 1 0
  5 <--> 2

  representative[0] = 1
  representative[1] = 1
  representative[2] = 5
  representative[3] = 3
  representative[4] = 1
  representative[5] = 5

  component 0 contains: 4 1 0
  component 1 contains: 3
  component 2 contains: 5 2

 */

using namespace boost;

int main(int argc, char* argv[])
{
    typedef adjacency_list< vecS, vecS, undirectedS > Graph;
    typedef graph_traits< Graph >::vertex_descriptor Vertex;
    typedef graph_traits< Graph >::vertices_size_type VertexIndex;

    const int VERTEX_COUNT = 6;
    Graph graph(VERTEX_COUNT);

    std::vector< VertexIndex > rank(num_vertices(graph));
    std::vector< Vertex > parent(num_vertices(graph));

    typedef VertexIndex* Rank;
    typedef Vertex* Parent;

    disjoint_sets< Rank, Parent > ds(&rank[0], &parent[0]);

    initialize_incremental_components(graph, ds);
    incremental_components(graph, ds);

    graph_traits< Graph >::edge_descriptor edge;
    bool flag;

    boost::tie(edge, flag) = add_edge(0, 1, graph);
    ds.union_set(0, 1);

    boost::tie(edge, flag) = add_edge(1, 4, graph);
    ds.union_set(1, 4);

    boost::tie(edge, flag) = add_edge(4, 0, graph);
    ds.union_set(4, 0);

    boost::tie(edge, flag) = add_edge(2, 5, graph);
    ds.union_set(2, 5);

    std::cout << "An undirected graph:" << std::endl;
    print_graph(graph, get(boost::vertex_index, graph));
    std::cout << std::endl;

    BOOST_FOREACH (Vertex current_vertex, vertices(graph))
    {
        std::cout << "representative[" << current_vertex
                  << "] = " << ds.find_set(current_vertex) << std::endl;
    }

    std::cout << std::endl;

    typedef component_index< VertexIndex > Components;

    // NOTE: Because we're using vecS for the graph type, we're
    // effectively using identity_property_map for a vertex index map.
    // If we were to use listS instead, the index map would need to be
    // explicitly passed to the component_index constructor.
    Components components(parent.begin(), parent.end());

    // Iterate through the component indices
    BOOST_FOREACH (VertexIndex current_index, components)
    {
        std::cout << "component " << current_index << " contains: ";

        // Iterate through the child vertex indices for [current_index]
        BOOST_FOREACH (VertexIndex child_index, components[current_index])
        {
            std::cout << child_index << " ";
        }

        std::cout << std::endl;
    }

    return (0);
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "strand_disequilibrium.hpp"

#include <boost/lexical_cast.hpp>

#include "io/variant/vcf_record.hpp"
#include "io/variant/vcf_spec.hpp"
#include "basics/aligned_read.hpp"
#include "utils/read_stats.hpp"
#include "utils/maths.hpp"
#include "../facets/samples.hpp"
#include "../facets/overlapping_reads.hpp"

namespace octopus { namespace csr {

const std::string StrandDisequilibrium::name_ = "SD";

std::unique_ptr<Measure> StrandDisequilibrium::do_clone() const
{
    return std::make_unique<StrandDisequilibrium>(*this);
}

Measure::ResultType StrandDisequilibrium::get_default_result() const
{
    return std::vector<double> {};
}

void StrandDisequilibrium::do_set_parameters(std::vector<std::string> params)
{
    if (params.size() != 1) {
        throw BadMeasureParameters {this->name(), "only has one parameter (tail mass)"};
    }
    try {
        tail_mass_ = boost::lexical_cast<decltype(tail_mass_)>(params.front());
    } catch (const boost::bad_lexical_cast&) {
        throw BadMeasureParameters {this->name(), "given parameter \"" + params.front() + "\" cannot be parsed"};
    }
    if (tail_mass_ < 0 || tail_mass_ > 1) {
        throw BadMeasureParameters {this->name(), "tail mass must be between 0 and 1"};
    }
}

std::vector<std::string> StrandDisequilibrium::do_parameters() const
{
    return {utils::to_string(tail_mass_, 2)};
}

Measure::ResultType StrandDisequilibrium::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& reads = get_value<OverlappingReads>(facets.at("OverlappingReads"));
    std::vector<double> result {};
    result.reserve(samples.size());
    for (const auto& sample : samples) {
        const auto direction_counts = count_directions(reads.at(sample), mapped_region(call));
        const auto tail_probability = maths::beta_tail_probability(direction_counts.first + 0.5, direction_counts.second + 0.5, tail_mass_);
        result.push_back(tail_probability);
    }
    return result;
}

Measure::ResultCardinality StrandDisequilibrium::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& StrandDisequilibrium::do_name() const
{
    return name_;
}

std::string StrandDisequilibrium::do_describe() const
{
    return "Strand bias of reads overlapping the site; probability mass in tails of Beta distribution";
}

std::vector<std::string> StrandDisequilibrium::do_requirements() const
{
    return {"Samples", "OverlappingReads"};
}

bool StrandDisequilibrium::is_equal(const Measure& other) const noexcept
{
    return tail_mass_ == static_cast<const StrandDisequilibrium&>(other).tail_mass_;
}

} // namespace csr
} // namespace octopus


#ifndef INCLUDED_scheme_nest_maps_EulerAnglesMap_HH
#define INCLUDED_scheme_nest_maps_EulerAnglesMap_HH

#include "scheme/util/SimpleArray.hh"
#include "scheme/numeric/euler_angles.hh"

#include <boost/static_assert.hpp>
// #include <iostream>
#include <vector>

namespace scheme {
namespace nest {
namespace pmap {

template<
	int DIM,
	class Value=util::SimpleArray<DIM,double>,
	class Index=size_t,
	class Float=double
>
struct EulerAnglesMap {
	static int const DIMENSION = DIM;
	typedef Value ValueType ;
	typedef Float FloatType ;
	typedef Index IndexType ;
	typedef util::SimpleArray<DIM,Index> Indices;
	typedef util::SimpleArray<DIM,Float> Params;

	BOOST_STATIC_ASSERT_MSG(DIM==3,"EulerAnglesMap DIM must be == 3");
	///@brief constructor
	EulerAnglesMap() {}
	///@brief
	///@return false iff invalid parameters
	bool params_to_value(
		Params const & params,
		Index /*cell_index*/,
		Index /*resl*/,
		Value & value
	) const {
		for(size_t i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
		// assert( params[0] <= (Float)num_cells_ );
		for(size_t i = 1; i < DIM; ++i) assert( params[i] <= 1.0 );
		if( params[2] > 0.5 ) return false; // convention is 'W' component is >= 0
		Params euler = params*2.0*boost::math::constants::pi<Float>();
		numeric::from_euler_angles(euler,value);
		return true;
	}
	///@brief
	///@note necessary for value lookup and neighbor lookup
	bool value_to_params(
		Value const & value,
		Index resl,
		Params & params,
		Index & cell_index
	) const {
		///@note neighbor lookups require out of bounds mappings to be valid
		cell_index = 0;
		value_to_params_for_cell(value,resl,params,0);
		return true;
	}
	///@brief
	///@note necessary only for neighbor lookup
	void value_to_params_for_cell(
		Value const & value,
		Index /*resl*/,
		Params & params,
		Index /*cell_index*/
	) const {
		numeric::euler_angles(value,params);
		// std::cout << params << std::endl;
		params = params / 2.0 / boost::math::constants::pi<Float>();
		for(size_t i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
		for(size_t i = 1; i < DIM; ++i) assert( 1.0 >= params[i] );
	}
	///@brief
	///@note delta parameter is in "Parameter Space"
	template<class OutIter>
	void get_neighboring_cells(
		Value const & value,
		Index /*resl*/,
		Float param_delta,
		OutIter out
	) const {
		// // Float param_delta = 1.0 / (Float)(1<<resl);
		// // this BIG thing is to ensure rounding goes down
		// int const BIG = 12345678;
		// int lb = std::max(                0, static_cast<int>( value[0]-param_delta + BIG ) - BIG );
		// int ub = std::min((int)num_cells_-1, static_cast<int>( value[0]+param_delta + BIG ) - BIG );
		// // std::cout << "lb " << lb << " ub "  << ub << std::endl;
		// // assert(lb<=ub);
		// for(int i = lb; i <= ub; ++i) *(out++) = i;
	}
	///@brief aka covering radius max distance from bin center to any value within bin
	Float bin_circumradius(Index resl) const { return 2.0/(Float)(1<<resl); }
	///@brief maximum distance from the bin center which must be within the bin
	Float bin_inradius(Index resl) const { return 3.0/(Float)(1<<resl); }
	///@brief cell size
	Index num_cells() const { return 1; }
	virtual ~EulerAnglesMap(){}
 private:
};

}
}
}

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLINT_2_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLINT_2_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/real.hpp>

#include <nt2/toolbox/polynomials/function/scalar/impl/horner.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/oneminus.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellint_2_, tag::cpu_,
                          (A0),
                          (arithmetic_<A0>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellint_2_(tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename NT2_RETURN_TYPE(1)::type type;
      return nt2::ellint_2(type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellint_2_, tag::cpu_,
                          (A0),
                          (real_<A0>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellint_2_(tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename NT2_RETURN_TYPE(1)::type type;
      type x = nt2::abs(a0);
      if (x>One<A0>())    return Nan<A0>();
      if (x == One<A0>()) return x;
      return boost::math::ellint_2(a0);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
// NT2_REGISTER_DISPATCH(tag::ellint_2_, tag::cpu_,
//                           (A0),
//                           (float_<A0>)
//                          )

// namespace nt2 { namespace ext
// {
//   template<class Dummy>
//   struct call<tag::ellint_2_(tag::float_),
//               tag::cpu_, Dummy> : callable
//   {
//     template<class Sig> struct result;
//     template<class This,class A0>
//     struct result<This(A0)> :
//       std::tr1::result_of<meta::floating(A0)>{};

//     NT2_FUNCTOR_CALL(1)
//     {
//       A0 x = nt2::abs(a0);
//       if (x>One<A0>()) return Nan<A0>();
//       if (x == One<A0>()) return x;
//       const A0 a = nt2::sqrt(oneminus(x));
//       return horner< NT2_HORNER_COEFF_T(A0, 11,
//                               (0x392102f5,
//                                0x3b246c1b,
//                                0x3c0e578f,
//                                0x3c2fe240,
//                                0x3bfebca9,
//                                0x3bf882cf,
//                                0x3c3d8b3f,
//                                0x3cb2d89a,
//                                0x3d68ac90,
//                                0x3ee2e430,
//                                0x3f800000) ) > (a)
//                 -log(a)*a*horner< NT2_HORNER_COEFF_T(A0, 10,
//                                     (0x38098de4,
//                                      0x3a84557e,
//                                      0x3bd53114,
//                                      0x3c8a54f6,
//                                      0x3cd67118,
//                                      0x3d0925e1,
//                                      0x3d2ef92b,
//                                      0x3d6fffe9,
//                                      0x3dc00000,
//                                      0x3e800000
//                                      ) ) > (a);
//     }
//   };
// } }

#endif
// modified by jt the 26/12/2010