#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"
#include<termios.h>

using namespace  std;
using namespace  Eigen;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;
ros::Publisher pub29 ;
ros::Publisher pub30 ;
ros::Publisher pub31 ;

int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

void  SendGazebo(vector<double> q){

    std_msgs::Float64 data;
    data.data=q[1];
    pub1.publish(data);
    data.data=q[2];
    pub2.publish(data);
    data.data=q[3];
    pub3.publish(data);
    data.data=q[4];
    pub4.publish(data);
    data.data=q[5];
    pub5.publish(data);
    data.data=q[6];
    pub6.publish(data);
    data.data=q[7];
    pub7.publish(data);
    data.data=q[8];
    pub8.publish(data);
    data.data=q[9];
    pub9.publish(data);
    data.data=q[10];
    pub10.publish(data);
    data.data=q[11];
    pub11.publish(data);
    data.data=q[12];
    pub12.publish(data);
    data.data=q[13];
    pub13.publish(data);
    data.data=q[14];
    pub14.publish(data);
    data.data=q[15];
    pub15.publish(data);
    data.data=q[16];
    pub16.publish(data);
    data.data=q[17];
    pub17.publish(data);
    data.data=q[18];
    pub18.publish(data);
    data.data=q[19];
    pub19.publish(data);
    data.data=q[20];
    pub20.publish(data);
    data.data=q[21];
    pub21.publish(data);
    data.data=q[22];
    pub22.publish(data);
    data.data=q[23];
    pub23.publish(data);
    data.data=q[24];
    pub24.publish(data);
    data.data=q[25];
    pub25.publish(data);
    data.data=q[26];
    pub26.publish(data);
    data.data=q[27];
    pub27.publish(data);
    data.data=q[28];
    pub28.publish(data);
    data.data=q[29];
    pub29.publish(data);
    data.data=q[30];
    pub30.publish(data);
    data.data=q[31];
    pub31.publish(data);

}

int main(int argc, char **argv)
{
//hand_gui my_hand_gui;
//my_hand_gui.show();
    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",100);

    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",100);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",100);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",100);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",100);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",100);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",100);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",100);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",100);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",100);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",100);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",100);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",100);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",100);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",100);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",100);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",100);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",100);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",100);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",100);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",100);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",100);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",100);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",100);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",100);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",100);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",100);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",100);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",100);
    pub29 = nh.advertise<std_msgs::Float64>("rrbot/joint29_position_controller/command",1000);
    pub30 = nh.advertise<std_msgs::Float64>("rrbot/joint30_position_controller/command",1000);
    pub31 = nh.advertise<std_msgs::Float64>("rrbot/joint31_position_controller/command",1000);

    right_hand hand_funcs;

//VectorXd q_test(7);
//q_test<<1,
//        2,
//        3,
//        4,
//        5,
//        6,
//        7;
//hand_funcs.angle_fix_elbow=0;
//hand_funcs.angle_fix_shd=10*M_PI/180;
//hand_funcs.jacob(q_test);

//MatrixXd J(3,7);
//J=hand_funcs.J_right_palm;
//MatrixXd Jw(3,7);
//Jw=hand_funcs.J_w_right_palm;

//ROS_INFO("\nJ_right_palm=\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n",J(0,0),J(0,1),J(0,2),J(0,3),J(0,4),J(0,5),J(0,6),J(1,0),J(1,1),J(1,2),J(1,3),J(1,4),J(1,5),J(1,6),J(2,0),J(2,1),J(2,2),J(2,3),J(2,4),J(2,5),J(2,6));
//ROS_INFO("\nJ_w_right_palm=\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n%f\t%f\t%f\t%f\t%f\t%f\t%f\n",Jw(0,0),Jw(0,1),Jw(0,2),Jw(0,3),Jw(0,4),Jw(0,5),Jw(0,6),Jw(1,0),Jw(1,1),Jw(1,2),Jw(1,3),Jw(1,4),Jw(1,5),Jw(1,6),Jw(2,0),Jw(2,1),Jw(2,2),Jw(2,3),Jw(2,4),Jw(2,5),Jw(2,6));

// getch();

    VectorXd r_target(3);

    MatrixXd R_target(3,3);
    //shaking

    r_target<<.4,
            -.1,
            .3;
   // R_target=hand_funcs.rot(2,90*M_PI/180,3)*hand_funcs.rot(1,-10*M_PI/180+atan2(r_target(1),r_target(0)),3);

//        r_target<<.4,
//                -0.2,
//                0.20;
//R_target=hand_funcs.rot(2,-170*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3);
//R_target=hand_funcs.rot(1,-90*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3);
R_target=hand_funcs.rot(2,-160*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3);
//    R_target=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,-90*M_PI/180,3);
//    r_target<<0,
//            -.550,
//            0;

//from base to end

  // R_target=hand_funcs.rot(3,-90*M_PI/180,3)*hand_funcs.rot(2,-90*M_PI/180,3);

 //R_target=hand_funcs.rot(2,90*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3);
//R_target=hand_funcs.rot(2,90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3);

VectorXd q0(7);

q0<<6*M_PI/180,
        -4*M_PI/180,
        0,
        -20*M_PI/180,
        0,
        0,
        0;

//q0<<-50*M_PI/180,
//        0*M_PI/180,
//        -30*M_PI/180,
//        -20*M_PI/180,
//        0,
//        0,
//        0;

double v0=0;
double v_target =.4;
right_hand hand0(q0,r_target,R_target,0,0);
double d0=hand0.dist;
double d=d0;
double d_des=hand0.d_des;
double theta=hand0.theta;
double theta_target=hand0.theta_target;
double sai=hand0.sai;
double sai_target=hand0.sai_target;
double phi=hand0.phi;
double phi_target=hand0.phi_target;
//ROS_INFO("theta_target=%f,sai_target=%f,phi_target=%f",theta_target,sai_target,phi_target);
ROS_INFO("\nr_target=\n%f\n%f\n%f",r_target(0),r_target(1),r_target(2));
ROS_INFO("\nR_target=\n%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n",R_target(0,0),R_target(0,1),R_target(0,2),R_target(1,0),R_target(1,1),R_target(1,2),R_target(2,0),R_target(2,1),R_target(2,2));
ROS_INFO("\nsai_target=%f\tphi_target=%f\ttheta_target=%f",sai_target,phi_target,theta_target);
ROS_INFO("press any key to start!");

getch();

QVector<double> qr1;
QVector<double> qr2;
QVector<double> qr3;
QVector<double> qr4;
QVector<double> qr5;
QVector<double> qr6;
QVector<double> qr7;

int i=0;
VectorXd q_ra;
q_ra=q0;

qr1.append(q_ra(0));
qr2.append(q_ra(1));
qr3.append(q_ra(2));
qr4.append(q_ra(3));
qr5.append(q_ra(4));
qr6.append(q_ra(5));
qr7.append(q_ra(6));

vector<double> q_init(30);
for (int i = 0; i < 30; ++i) {
    q_init[i]=0;
}
q_init[15]=qr1[0];
q_init[16]=qr2[0];
q_init[17]=qr3[0];
q_init[18]=qr4[0];
q_init[19]=qr5[0];
q_init[20]=qr6[0];
q_init[21]=qr7[0];

//SendGazebo(q_init);

int q_motor[8];
for (int var = 0; var < 8; ++var) {
    q_motor[var]=0;
}

vector<double> q(30);

ros::Rate loop_rate(200);
int count = 0;
double waving_time=0;
double q_waving=0;
double wavingcount=0;
    while (ros::ok())
    {
//        for (int k = 0; k < 7; ++k) {
//            if (k%2==1){r_target=r_shaking;}
//            else{r_target=r_shaking2;}
//right_hand hand00(q_ra,r_target,R_target,i,d0);
//d=hand00.dist;
//        while ((d>d0/2)  || (abs(theta-theta_target)>.02)   || (abs(sai-sai_target)>.02)  || (abs(phi-phi_target)>.02)  ) {
//while (1){
        if(q_ra(0)>-40*M_PI/180){
            q_ra(0)-=.15*M_PI/180;
            q_ra(3)-=.15*M_PI/180;
             q_ra(1)-=.075*M_PI/180;
            qr1.append(q_ra(0));
            qr2.append(q_ra(1));
            qr3.append(q_ra(2));
            qr4.append(q_ra(3));
            qr5.append(q_ra(4));
            qr6.append(q_ra(5));
            qr7.append(q_ra(6));
        }

        else if ((d>d_des)  || (abs(theta-theta_target)>.5)   || (abs(sai-sai_target)>.8)  || (abs(phi-phi_target)>.5)  )
{

        right_hand hand(q_ra,r_target,R_target,i,d0);
//hand.Right_palm_orientation_power=0;

        hand.doQP(q_ra);
        q_ra=hand.q_next;

        qr1.append(q_ra(0));
        qr2.append(q_ra(1));
        qr3.append(q_ra(2));
        qr4.append(q_ra(3));
        qr5.append(q_ra(4));
        qr6.append(q_ra(5));
        qr7.append(q_ra(6));
        q_waving=q_ra(2);
        d=hand.dist;
        theta=hand.theta;
        sai=hand.sai;
        phi=hand.phi;

        i++;

        for (int i = 0; i < 30; ++i) {
            q[i]=0;
        }

//         q[15]=-M_PI/2;
//         q[16]=-10*M_PI/180;
//         q[17]=-10*M_PI/180;

    //   ROS_INFO("T=%f upper:%f %f lower:%f %f qdot=%f q=%f %f",hand.T,(hand.toRad(10)+q_ra(1))/hand.T,hand.qdot_max,(hand.toRad(-90)+q_ra(1))/hand.T,-hand.qdot_max,hand.qdot(2),180/M_PI*q[16],180/M_PI*q_ra(1));

}

        else if(wavingcount<2){

            q_ra(2)=q_waving+M_PI/10*(-1+sin(waving_time*3+M_PI/2));

           // ROS_INFO("q_ra(2)=%f",q_ra(2));
            waving_time+=hand0.T;

            wavingcount=2*waving_time*3/(2*M_PI);
            qr1.append(q_ra(0));
            qr2.append(q_ra(1));
            qr3.append(q_ra(2));
            qr4.append(q_ra(3));
            qr5.append(q_ra(4));
            qr6.append(q_ra(5));
            qr7.append(q_ra(6));

                   }
        else{
            count-=2;
        }
        ++count;
        if(count==0){break;}
//       ROS_INFO("q1=%f\tq2=%f\tq3=%f\tq4=%f\tq5=%f\tq6=%f\tq7=%f\t",180/M_PI*q[15],180/M_PI*q[16],180/M_PI*q[17],180/M_PI*q[18],180/M_PI*q[19],180/M_PI*q[20],180/M_PI*q[21]);
//       ROS_INFO("phi=%f(%f)\ttheta=%f(%f)\tsai=%f(%f)\t",phi,phi_target,theta,theta_target,sai,sai_target);
//       ROS_INFO("x=%f\ty=%f\tz=%f\t",hand.r_right_palm(0),hand.r_right_palm(1),hand.r_right_palm(2));
//       ROS_INFO("w_x=%f,w_y=%f,w_z=%f",hand.w_right_palm(0),hand.w_right_palm(1),hand.w_right_palm(2));
        q[15]=qr1[count];
        q[16]=qr2[count];
        q[17]=qr3[count];
        q[18]=qr4[count];

        q[19]=qr5[count];
        q[20]=qr6[count];
        q[21]=qr7[count];
      //  SendGazebo(q);

        msg.data.clear();

       for(int  i = 0;i < 12;i++)
       {
           msg.data.push_back(0);
       }

       q_motor[0]=-int(10*(qr1[count]-q0(0))*360/M_PI);
       q_motor[1]=int(10*(qr2[count]-q0(1))*360/M_PI);
       q_motor[2]=-int(7*(qr3[count]-q0(2))*300/M_PI);
       q_motor[3]=int(7*(qr4[count]-q0(3))*300/M_PI);
//ROS_INFO("%f %d",qr4[count]*180/M_PI,q_motor[3]);
       q_motor[4]=int((qr5[count]-q0(4))*(3000-2300)*4/M_PI);
       q_motor[5]=int((qr6[count]-q0(5))*(4000-2050)/(23*M_PI/180));
       q_motor[6]=int((qr7[count]-q0(6))*(4000-2050)/(23*M_PI/180));
       q_motor[7]=7;

       //right hand epose
       msg.data.push_back(q_motor[0]);//12 -y  a,z
       msg.data.push_back(q_motor[1]);//13 +x
       msg.data.push_back(q_motor[2]);//14 -z
       msg.data.push_back(q_motor[3]);//15 +y
       //right hand dynamixel + fingers
       msg.data.push_back(q_motor[4]);//16
       msg.data.push_back(q_motor[5]);//17
       msg.data.push_back(q_motor[6]);//18
       msg.data.push_back(q_motor[7]);//19
       //left hand epose
       msg.data.push_back(0);//20 +y
       msg.data.push_back(0);//21 +x
       msg.data.push_back(0);//22 -z
       msg.data.push_back(0);//23 -y
       //left hand dynamixel + fingers
       msg.data.push_back(0);//24
       msg.data.push_back(0);//25
       msg.data.push_back(0);//26
       msg.data.push_back(0);//27

         chatter_pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();

           }
    return 0;
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_DISTANCE_INFO_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_DISTANCE_INFO_HPP

#include <boost/type_traits/remove_const.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>

#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>

namespace boost { namespace geometry
{

template <typename Point>
struct distance_info_result
{
    typedef Point point_type;
    typedef typename default_distance_result<Point>::type distance_type;

    bool on_segment;
    bool within_geometry;
    distance_type real_distance;
    Point projected_point1; // A on B
    Point projected_point2; // B on A
    distance_type projected_distance1;
    distance_type projected_distance2;
    distance_type fraction1;
    distance_type fraction2;
    segment_identifier seg_id1;
    segment_identifier seg_id2;

    inline distance_info_result()
        : on_segment(false)
        , within_geometry(false)
        , real_distance(distance_type())
        , projected_distance1(distance_type())
        , projected_distance2(distance_type())
        , fraction1(distance_type())
        , fraction2(distance_type())
    {}
};

namespace strategy { namespace distance
{

template
<
    typename CalculationType = void,
    typename Strategy = pythagoras<CalculationType>
>
struct calculate_distance_info
{
public :
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.
    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
              typename strategy::distance::services::return_type
                  <
                      Strategy,
                      Point,
                      PointOfSegment
                  >::type
          >
    {};

public :

    // Helper function
    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply_point_point(Point1 const& p1, Point2 const& p2) const
    {
        Strategy point_point_strategy;
        boost::ignore_unused_variable_warning(point_point_strategy);
        return point_point_strategy.apply(p1, p2);
    }

    template <typename Point, typename PointOfSegment, typename Result>
    inline void apply(Point const& p,
                    PointOfSegment const& p1, PointOfSegment const& p2,
                    Result& result) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        typedef typename calculation_type<Point, PointOfSegment>::type calculation_type;

        //// A projected point of points in Integer coordinates must be able to be
        //// represented in FP.
        typedef model::point
            <
                calculation_type,
                dimension<PointOfSegment>::value,
                typename coordinate_system<PointOfSegment>::type
            > fp_point_type;

        // For convenience
        typedef fp_point_type fp_vector_type;

        // For source-code-comments, see "cartesian/distance_distance_info.hpp"
        fp_vector_type v, w;

        geometry::convert(p2, v);
        geometry::convert(p, w);
        subtract_point(v, p1);
        subtract_point(w, p1);

        calculation_type const zero = calculation_type();

        calculation_type const c1 = dot_product(w, v);
        calculation_type const c2 = dot_product(v, v);

        result.on_segment = c1 >= zero && c1 <= c2;

        Strategy point_point_strategy;
        boost::ignore_unused_variable_warning(point_point_strategy);

        if (geometry::math::equals(c2, zero))
        {
            geometry::convert(p1, result.projected_point1);
            result.fraction1 = 0.0;
            result.on_segment = false;
            result.projected_distance1 = result.real_distance = apply_point_point(p, p1);
            return;
        }

        calculation_type const b = c1 / c2;
        result.fraction1 = b;

        geometry::convert(p1, result.projected_point1);
        multiply_value(v, b);
        add_point(result.projected_point1, v);
        result.projected_distance1 = apply_point_point(p, result.projected_point1);
        result.real_distance
                    = c1 < zero ? apply_point_point(p, p1)
                    : c1 > c2 ? apply_point_point(p, p2)
                    : result.projected_distance1;
    }
};

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_DISTANCE_INFO_HPP


#include <boost/geometry/strategies/spherical/disjoint_segment_box.hpp>


/*******************************************************************************
Copyright(c) 2015-2020 Parker Hannifin Corp. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/SampleUtils.h"
#include "mscl/MicroStrain/SampleRate.h"
#include "mscl/Exceptions.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(SampleUtils_Test)

BOOST_AUTO_TEST_CASE(SampleUtils_convertToSampleRate_sampleRateOption)
{
    //check that the rates turn into the actual SampleRate that they should
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_10kHz).prettyStr(), "10kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_20kHz).prettyStr(), "20kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_30kHz).prettyStr(), "30kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_40kHz).prettyStr(), "40kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_50kHz).prettyStr(), "50kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_60kHz).prettyStr(), "60kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_70kHz).prettyStr(), "70kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_80kHz).prettyStr(), "80kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_90kHz).prettyStr(), "90kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_100kHz).prettyStr(), "100kHz");

    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_4096Hz).prettyStr(), "4.096kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_2048Hz).prettyStr(), "2.048kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_1024Hz).prettyStr(), "1.024kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_512Hz).prettyStr(), "512Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_256Hz).prettyStr(), "256Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_128Hz).prettyStr(), "128Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_64Hz).prettyStr(), "64Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_32Hz).prettyStr(), "32Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_16Hz).prettyStr(), "16Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_8Hz).prettyStr(), "8Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_4Hz).prettyStr(), "4Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_2Hz).prettyStr(), "2Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_1Hz).prettyStr(), "1Hz");

    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_2Sec).prettyStr(), "Every 2 sec");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_5Sec).prettyStr(), "Every 5 sec");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_10Sec).prettyStr(), "Every 10 sec");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_30Sec).prettyStr(), "Every 30 sec");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_1Min).prettyStr(), "Every 1 min");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_2Min).prettyStr(), "Every 2 min");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_5Min).prettyStr(), "Every 5 min");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_10Min).prettyStr(), "Every 10 min");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_30Min).prettyStr(), "Every 30 min");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_60Min).prettyStr(), "Every 1 hr");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(WirelessTypes::sampleRate_24Hours).prettyStr(), "Every 24 hr");

    //check that an invalid sample rate throws an Error_UnknownSampleRate exception
    WirelessTypes::WirelessSampleRate rt = static_cast<WirelessTypes::WirelessSampleRate>(0);
    BOOST_CHECK_THROW(SampleUtils::convertToSampleRate(rt), Error_UnknownSampleRate);
}

BOOST_AUTO_TEST_CASE(SampleUtils_convertToSampleRate_eepromVal)
{
    //check that the rates turn into the actual SampleRate that they should
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(71).prettyStr(), "10kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(80).prettyStr(), "100kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(101).prettyStr(), "4.096kHz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(113).prettyStr(), "1Hz");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(114).prettyStr(), "Every 2 sec");
    BOOST_CHECK_EQUAL(SampleUtils::convertToSampleRate(123).prettyStr(), "Every 1 hr");

    //check that an invalid sample rate throws an Error_UnknownSampleRate exception
    BOOST_CHECK_THROW(SampleUtils::convertToSampleRate(967), Error_UnknownSampleRate);
}

BOOST_AUTO_TEST_CASE(SampleUtils_convertFromSampleRate)
{
    //check that the Sample Rate turns into the eeprom value that it should
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::KiloHertz(10)), 71);
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::KiloHertz(100)), 80);
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::Hertz(4096)), 101);
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::Hertz(1)), 113);
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::Seconds(2)), 114);
    BOOST_CHECK_EQUAL(SampleUtils::convertFromSampleRate(SampleRate::Seconds(3600)), 123);

    //check that an invalid sample rate throws an Error_UnknownSampleRate exception
    BOOST_CHECK_THROW(SampleUtils::convertFromSampleRate(SampleRate::Hertz(784)), Error_UnknownSampleRate);
}

BOOST_AUTO_TEST_SUITE_END();


//          Copyright surrealwaffle 2018 - 2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include "bot_ai.hpp"
#include "bot_control.hpp"
#include "game_context.hpp"
#include "graph.hpp"

#include <algorithm>
#include <functional>
#include <iterator>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <sentutil/all.hpp>

namespace {

simulacrum::navigation_graph nav_graph;

} // namespace (anonymous)

namespace simulacrum { namespace ai {

void reset()
{
    recalculate_navigation(std::nullopt);
}

bool load()
{
    return true;
}

void recalculate_navigation(std::optional<std::string_view> cache_name)
{
    nav_graph = simulacrum::navigation_graph(simulacrum::this_collision_bsp);
}

void update(float seconds, long ticks)
{
    if (!game_context.local_unit || game_context.live_enemies.empty()) {
        control::immediate_goals.clear();
        return;
    }

    [[maybe_unused]]
    sentinel::player& local_player = game_context.local_player.value();
    sentinel::unit&   local_unit   = game_context.local_unit.value();
    sentinel::biped&  local_biped  = reinterpret_cast<sentinel::biped&>(local_unit);

    sentinel::player& nearest_enemy_player = *game_context.live_enemies.begin();
    sentinel::unit&   nearest_enemy_unit   = *nearest_enemy_player.unit;

    auto get_position = [] (const sentinel::unit& unit) -> const sentinel::real3d& {
        return unit.object.parent ? unit.object.parent->object.position
                                  : unit.object.position;
    };

    control::immediate_goals.target_player = std::ref(nearest_enemy_player);

    auto start_vertex = [get_position, local_biped] {
        const navigation_graph_node node{{},
                                         navigation_graph_node::type_surface,
                                         local_biped.biped.cbsp_surface_index};
        auto opt = nav_graph.get_node(node);
        return opt ? opt.value() : nav_graph.nearest_node(get_position(local_biped));
    }();
    auto goal_vertex = nav_graph.nearest_node(get_position(nearest_enemy_unit));

    if (!start_vertex || !goal_vertex)
        return; // OK to use previous pathing goals

    auto heuristic = [] (const navigation_graph_node& node,
                         const navigation_graph_node& goal)
                         { return norm(goal.point - node.point); };

    auto visitor = [count = 0] (auto&&... ) mutable { return ++count < 30000; };

    auto test_edge = [] (const navigation_graph_node& start,
                         const navigation_graph_node& end,
                         const auto& edge) {
        return std::true_type();
    };

    auto search_result = astar_search(nav_graph.get_graph(),
                                      start_vertex.value(),
                                      goal_vertex.value(),
                                      heuristic,
                                      visitor,
                                      test_edge);
    auto path_opt = get_path(start_vertex.value(),
                             goal_vertex.value(),
                             search_result);

    if (!path_opt)
        return;

    auto& path = path_opt.value();
    auto& dest = control::immediate_goals.target_position;
    std::fill(std::transform(path.cbegin(),
                             path.cbegin() + std::min(std::size(path), std::size(dest)),
                             dest.begin(),
                             [] (const auto& vertex) { return vertex->first.point; }),
              dest.end(),
              std::nullopt);
}

} } // namespace simulacrum::ai


// Copyright (c) 2016-2020 The ZCash developers
// Copyright (c) 2020 The PIVX developers
// Copyright (c) 2020-2021 The Sapphire Core Developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "test/test_pivx.h"

#include "sapling/address.hpp"
#include "sapling/key_io_sapling.h"
#include <string>
#include <vector>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(sapling_key_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(ps_address_test)
{
    SelectParams(CBaseChainParams::REGTEST);

    std::vector<unsigned char, secure_allocator<unsigned char>> rawSeed(32);
    HDSeed seed(rawSeed);
    auto m = libzcash::SaplingExtendedSpendingKey::Master(seed);

    for (uint32_t i = 0; i < 1000; i++) {
        auto sk = m.Derive(i);
        {
            std::string sk_string = KeyIO::EncodeSpendingKey(sk);
            BOOST_CHECK(sk_string.compare(0, 26, Params().Bech32HRP(CChainParams::SAPLING_EXTENDED_SPEND_KEY)) == 0);

            libzcash::SpendingKey spendingkey2 = KeyIO::DecodeSpendingKey(sk_string);
            BOOST_CHECK(IsValidSpendingKey(spendingkey2));

            BOOST_ASSERT(boost::get<libzcash::SaplingExtendedSpendingKey>(&spendingkey2) != nullptr);
            auto sk2 = boost::get<libzcash::SaplingExtendedSpendingKey>(spendingkey2);
            BOOST_CHECK(sk == sk2);
        }
        {
            libzcash::SaplingPaymentAddress addr = sk.DefaultAddress();

            std::string addr_string = KeyIO::EncodePaymentAddress(addr);
            BOOST_CHECK(addr_string.compare(0, 12, Params().Bech32HRP(CChainParams::SAPLING_PAYMENT_ADDRESS)) == 0);

            auto paymentaddr2 = KeyIO::DecodePaymentAddress(addr_string);
            BOOST_CHECK(IsValidPaymentAddress(paymentaddr2));

            BOOST_ASSERT(boost::get<libzcash::SaplingPaymentAddress>(&paymentaddr2) != nullptr);
            auto addr2 = boost::get<libzcash::SaplingPaymentAddress>(paymentaddr2);
            BOOST_CHECK(addr == addr2);
        }
    }
}

BOOST_AUTO_TEST_CASE(EncodeAndDecodeSapling)
{
    SelectParams(CBaseChainParams::MAIN);

    std::vector<unsigned char, secure_allocator<unsigned char>> rawSeed(32);
    HDSeed seed(rawSeed);
    libzcash::SaplingExtendedSpendingKey m = libzcash::SaplingExtendedSpendingKey::Master(seed);

    for (uint32_t i = 0; i < 1000; i++) {
        auto sk = m.Derive(i);
        {
            std::string sk_string = KeyIO::EncodeSpendingKey(sk);
            BOOST_CHECK(
                    sk_string.substr(0, 26) ==
                    Params().Bech32HRP(CChainParams::SAPLING_EXTENDED_SPEND_KEY));

            auto spendingkey2 = KeyIO::DecodeSpendingKey(sk_string);
            BOOST_CHECK(IsValidSpendingKey(spendingkey2));

            BOOST_CHECK(boost::get<libzcash::SaplingExtendedSpendingKey>(&spendingkey2) != nullptr);
            auto sk2 = boost::get<libzcash::SaplingExtendedSpendingKey>(spendingkey2);
            BOOST_CHECK(sk == sk2);
        }
        {
            auto addr = sk.DefaultAddress();

            std::string addr_string = KeyIO::EncodePaymentAddress(addr);
            BOOST_CHECK(
                    addr_string.substr(0, 2) ==
                    Params().Bech32HRP(CChainParams::SAPLING_PAYMENT_ADDRESS));

            auto paymentaddr2 = KeyIO::DecodePaymentAddress(addr_string);
            BOOST_CHECK(IsValidPaymentAddress(paymentaddr2));

            BOOST_CHECK(boost::get<libzcash::SaplingPaymentAddress>(&paymentaddr2) != nullptr);
            auto addr2 = boost::get<libzcash::SaplingPaymentAddress>(paymentaddr2);
            BOOST_CHECK(addr == addr2);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

// Copyright (c) 2007-2020 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POLICY_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POLICY_HPP

#include <string>
#include <tuple>

#include <boost/geometry/policies/relate/direction.hpp>
#include <boost/geometry/policies/relate/intersection_points.hpp>

#include <boost/geometry/strategies/side_info.hpp>

namespace boost { namespace geometry
{

namespace policies { namespace relate
{

template <typename IntersectionPointsReturnType>
struct segments_intersection_policy
{
private:
    typedef policies::relate::segments_intersection_points
        <
            IntersectionPointsReturnType
        > pts_policy;
    typedef policies::relate::segments_direction dir_policy;

public:
    struct return_type
    {
        typedef typename pts_policy::return_type intersection_points_type;
        typedef typename dir_policy::return_type direction_type;

        return_type(intersection_points_type const& pts_result,
                    direction_type const& dir_result)
            : intersection_points(pts_result)
            , direction(dir_result)
        {}

        intersection_points_type intersection_points;
        direction_type direction;
    };

    template <typename Segment1, typename Segment2, typename SegmentIntersectionInfo>
    static inline return_type segments_crosses(side_info const& sides,
                                        SegmentIntersectionInfo const& sinfo,
                                        Segment1 const& s1, Segment2 const& s2)
    {
        return return_type
            (
                pts_policy::segments_crosses(sides, sinfo, s1, s2),
                dir_policy::segments_crosses(sides, sinfo, s1, s2)
            );
    }

    template <typename Segment1, typename Segment2, typename Ratio>
    static inline return_type segments_collinear(
                                        Segment1 const& segment1,
                                        Segment2 const& segment2,
                                        bool opposite,
                                        int pa1, int pa2, int pb1, int pb2,
                                        Ratio const& ra1, Ratio const& ra2,
                                        Ratio const& rb1, Ratio const& rb2)
    {
        return return_type
            (
                pts_policy::segments_collinear(segment1, segment2,
                                               opposite,
                                               pa1, pa2, pb1, pb2,
                                               ra1, ra2, rb1, rb2),
                dir_policy::segments_collinear(segment1, segment2,
                                               opposite,
                                               pa1, pa2, pb1, pb2,
                                               ra1, ra2, rb1, rb2)
            );
    }

    template <typename Segment>
    static inline return_type degenerate(Segment const& segment,
                                         bool a_degenerate)
    {
        return return_type
            (
                pts_policy::degenerate(segment, a_degenerate),
                dir_policy::degenerate(segment, a_degenerate)
            );
    }

    template <typename Segment, typename Ratio>
    static inline return_type one_degenerate(Segment const& segment,
                                             Ratio const& ratio,
                                             bool a_degenerate)
    {
        return return_type
            (
                pts_policy::one_degenerate(segment, ratio, a_degenerate),
                dir_policy::one_degenerate(segment, ratio, a_degenerate)
            );
    }

    static inline return_type disjoint()
    {
        return return_type
            (
                pts_policy::disjoint(),
                dir_policy::disjoint()
            );
    }

    static inline return_type error(std::string const& msg)
    {
        return return_type
            (
                pts_policy::error(msg),
                dir_policy::error(msg)
            );
    }
};

}} // namespace policies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POLICY_HPP


#include <ancse/fvm_rate_of_change.hpp>

#include <Eigen/Dense>
#include <ancse/numerical_flux.hpp>
#include <ancse/reconstruction.hpp>
#include <fmt/format.h>

#define REGISTER_NUMERICAL_FLUX(token, FluxType, flux)                         \
    if (config["flux"] == (token)) {                                           \
        return std::make_shared<FVMRateOfChange<FluxType, Reconstruction>>(    \
            grid, model, flux, reconstruction);                                       \
    }

template <class Reconstruction>
std::shared_ptr<RateOfChange>
deduce_numerical_flux(const nlohmann::json &config,
                      const Grid &grid,
                      const std::shared_ptr<Model> &model,
                      const std::shared_ptr<SimulationTime> &simulation_time,
                      const Reconstruction &reconstruction)
{
    REGISTER_NUMERICAL_FLUX("central_flux", CentralFlux, CentralFlux(model))

    // Register the other numerical fluxes.

    throw std::runtime_error(
        fmt::format("Unknown numerical flux. {}", std::string(config["flux"])));
}
#undef REGISTER_NUMERICAL_FLUX

#define REGISTER_RECONSTRUCTION(token, reconstruction)                         \
    if (config["reconstruction"] == token) {                                   \
        return deduce_numerical_flux(                                          \
            config, grid, model, simulation_time, reconstruction);                     \
    }

std::shared_ptr<RateOfChange> make_fvm_rate_of_change(
    const nlohmann::json &config,
    const Grid &grid,
    const std::shared_ptr<Model> &model,
    const std::shared_ptr<SimulationTime> &simulation_time)
{
    REGISTER_RECONSTRUCTION("o1", PWConstantReconstruction{})

    // Register piecewise linear reconstructions.

    throw std::runtime_error(fmt::format(
        "Unknown reconstruction. [{}]", std::string(config["reconstruction"])));
}

#undef REGISTER_RECONSTRUCTION


/*

Copyright (c) 2005-2019, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef CELLCYCLEMODELODEHANDLER_HPP_
#define CELLCYCLEMODELODEHANDLER_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>

#include "ChasteSerialization.hpp"
#include "AbstractOdeSystem.hpp"
#include "AbstractCellCycleModelOdeSolver.hpp"
#include "SimulationTime.hpp"

/**
 * This class contains the functionality for running ODEs as part of a cell cycle
 * or SRN model.  It is designed to be used as an additional base class for models
 * which require this functionality.
 */
class CellCycleModelOdeHandler
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & mpOdeSystem;
        archive & mpOdeSolver;
        archive & mLastTime;
        archive & mDt;
        archive & mFinishedRunningOdes;
    }

    /**
     * Prevent copy-assignment of this class, or its subclasses.
     * Note that we do not define this method, therefore statements like "CellCycleModelOdeHandler new = old;" will not compile.
     * We do not inherit from boost::noncopyable because we *do* define a protected copy-constructor, for use by CreateCellCycleModel
     * and CreateSrnModel.
     *
     * @return the new ODE handler.
     */
    CellCycleModelOdeHandler& operator=(const AbstractCellCycleModelOdeSolver&);

protected:

    /**
     * Protected copy-constructor for use by CreateCellCycleModel and CreateSrnModel.
     *
     * @param rHandler ODE handler to copy.
     */
    CellCycleModelOdeHandler(const CellCycleModelOdeHandler& rHandler);

    /**
     * Timestep to use when solving the ODE system.
     * For some adaptive solvers (e.g. CVODE) this is the maximum step to use.
     */
    double mDt;

    /** A system of ODEs. */
    AbstractOdeSystem* mpOdeSystem;

    /** A shared pointer to an ODE solver object. */
    boost::shared_ptr<AbstractCellCycleModelOdeSolver> mpOdeSolver;

    /** The last time the ODE system was evaluated. */
    double mLastTime;

    /**
     * Whether the model is currently in a delay (not solving ODEs).
     */
    bool mFinishedRunningOdes;

    /**
     * Solves the ODE system to a given time.
     *
     * @param currentTime the current time
     *
     * @return whether a stopping event occurred.
     */
    bool SolveOdeToTime(double currentTime);

    /**
     * Adjust any ODE parameters needed before solving until currentTime.
     * Defaults to do nothing.
     *
     * @param currentTime  the time up to which the system will be solved.
     */
    virtual void AdjustOdeParameters(double currentTime);

public:

    /**
     * Constructor.
     *
     * @param lastTime  The birth time of the cell / last time model was evaluated (defaults to the current SimulationTime)
     * @param pOdeSolver An optional pointer to a cell-cycle model ODE solver object (allows the use of different ODE solvers)
     */
    CellCycleModelOdeHandler(double lastTime = SimulationTime::Instance()->GetTime(),
                             boost::shared_ptr<AbstractCellCycleModelOdeSolver> pOdeSolver = boost::shared_ptr<AbstractCellCycleModelOdeSolver>());

    /**
     * Destructor.
     */
    virtual ~CellCycleModelOdeHandler();

    /**
     * @return #mpOdeSystem.
     */
    AbstractOdeSystem* GetOdeSystem() const;

    /**
     * Set mpOdeSystem. Used in CreateCellCycleModel().
     *
     * @param pOdeSystem the ODE system
     */
    void SetOdeSystem(AbstractOdeSystem* pOdeSystem);

    /**
     * @return mpOdeSolver
     */
    const boost::shared_ptr<AbstractCellCycleModelOdeSolver> GetOdeSolver() const;

    /**
     * Set mLastTime.
     *
     * @param lastTime the new value of mLastTime
     */
    void SetLastTime(double lastTime);

    /**
     * @return #mDt.  This sets it to a default value if it hasn't
     * been set by calling SetDt.
     */
    double GetDt();

    /**
     * Set the time step to use to solve the ODE system.
     * For some adaptive solvers (e.g. CVODE) this is the maximum step to use.
     *
     * @param timeStep  time step to use
     */
    void SetDt(double timeStep);

    /**
     * Set the values of the state variables in the cell-cycle model's ODE system.
     *
     * @param rStateVariables vector containing values for the state variables
     */
    void SetStateVariables(const std::vector<double>& rStateVariables);

    /**
     * @return the protein concentrations at the current time (useful for tests)
     *
     * NB: Will copy the vector - you can't use this to modify the concentrations.
     */
    std::vector<double> GetProteinConcentrations() const;

    /**
     * Sets the protein concentrations and time when the model was last evaluated - should only be called by tests
     *
     * @param lastTime the SimulationTime at which the protein concentrations apply
     * @param proteinConcentrations a standard vector of doubles of protein concentrations
     *
     */
    void SetProteinConcentrationsForTestsOnly(double lastTime, std::vector<double> proteinConcentrations);
};

#endif /*CELLCYCLEMODELODEHANDLER_HPP_*/


// Copyright (c) 2019 The YaCoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#ifdef _MSC_VER
    #include "msvc_warnings.push.h"
#endif

#include <stdint.h>
#include <stdlib.h> // for rand()
#include <stdio.h>  // for fclose()
#include "util.h"   // for GetArg()(

//#include <boost/filesystem.hpp>

#ifndef YACOIN_RANDOM_NONCE_H
#include "random_nonce.h"
#endif

CRandomNonce Big;
//_____________________________________________________________________________
static ::uint32_t
    get_a_random_16bit_nonce( void )
{
    ::uint32_t
        value;

    value = ( rand() & 0x00007fff )
            |
            (
             ( ( rand() & 0x0001 ) ? 0x8000 : 0x0000 )
            );
    return value;
}
//_____________________________________________________________________________
static ::uint32_t
    get_a_random_32bit_value( void )
{
    ::uint32_t
        value;

    // for RAND_MAX = (0, 0x7fff), then 2nd 0x8000 | (0,0x7fff) gives (0, 0xffff)
    // for RAND_MAX = (0, 0x7fff), then 3rd 0x8000 | (0,0x7fff) gives (0, 0xffff)

    value =
        get_a_random_16bit_nonce( )
        |
        ( get_a_random_16bit_nonce( ) << 16 );

    return value;
}

//_____________________________________________________________________________
::uint32_t
    CRandomNonce::get_a_nonce( unsigned int & nNonceReference )
{
    return (0 == ++nNonceReference)? ++nNonceReference: nNonceReference;
}
//_____________________________________________________________________________
void
    CRandomNonce::randomize_the_nonce( unsigned int & nNonceReference )
{
    ::uint32_t
        n32value;

    // perform lazy initialization here
    if( !initialized )
    {
        initialized = true;
        // do any initialization here
    }
    srand( ( unsigned int )time( NULL ) );
    // can comment this out to get repeating random sequence

    // and can put our own test valuess below also to test
    n32value = get_a_random_32bit_value();
    nNonceReference = n32value;
}
//_____________________________________________________________________________
#ifdef _MSC_VER
    #include "msvc_warnings.pop.h"
#endif


#include <algorithm>
#include <numeric>
#include <limits>
#include <unordered_set>

#include <boost/lambda/lambda.hpp>

#include "Conditioning.h"
#include "LogSumExp.h"
#include "FactorGraphPartialObservation.h"
#include "VAcyclicDecomposition.h"
#include "FactorGraphStructurizer.h"
#include "StructuredMeanFieldInference.h"

using namespace boost::lambda;

namespace Grante {

StructuredMeanFieldInference::StructuredMeanFieldInference(
	const FactorGraph* fg, FactorConditioningTable* fcond_tab,
	DecompositionType decomp_type)
	: InferenceMethod(fg), fcond_tab(fcond_tab),
		log_z(std::numeric_limits<double>::signaling_NaN()),
		verbose(true), conv_tol(1.0e-6), max_iter(50) {
	// 1. Compute a v-acyclic decomposition of the factor graph
	VAcyclicDecomposition vac(fg);
	std::vector<double> factor_weight(fg->Factors().size(), 1.0);

	// Total correlation based weights
	if (decomp_type == TotalCorrelationWeights) {
		const std::vector<Factor*>& factors = fg->Factors();
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			double tcorr = factors[fi]->TotalCorrelation();
			assert(tcorr >= -1.0e-10);
			// Add an epsilon so it is always better to add factors than to
			// leave them out.
			factor_weight[fi] = std::min(1.0e-3, tcorr);
		}
	}
	std::vector<bool> factor_is_removed;
	vac.ComputeDecompositionSP(factor_weight, factor_is_removed);

	InitializeVAC(factor_is_removed);
}

StructuredMeanFieldInference::StructuredMeanFieldInference(
	const FactorGraph* fg, FactorConditioningTable* fcond_tab,
	const std::vector<bool>& factor_is_removed)
	: InferenceMethod(fg), fcond_tab(fcond_tab),
		log_z(std::numeric_limits<double>::signaling_NaN()),
		verbose(true), conv_tol(1.0e-6), max_iter(50) {
	// TODO: assert this yields a v-acyclic decomposition
	assert(factor_is_removed.size() == fg->Factors().size());
	InitializeVAC(factor_is_removed);
}

StructuredMeanFieldInference::~StructuredMeanFieldInference() {
	// Delete component factor graphs (created in InitializeVAC)
	assert(mf_comp.size() == mf_comp_inf.size());
	for (unsigned int ci = 0; ci < mf_comp.size(); ++ci) {
		delete (mf_comp[ci]);
		delete (mf_comp_inf[ci]);
	}
}

void StructuredMeanFieldInference::InitializeVAC(
	const std::vector<bool>& factor_is_removed) {
	// Create simple factor to fi lookup table
	const std::vector<Factor*>& factors = fg->Factors();
	for (unsigned int fi = 0; fi < factors.size(); ++fi)
		fac_to_fi.insert(std::pair<Factor*, unsigned int>(factors[fi], fi));

	// Decompose factor graph into components
	std::vector<unsigned int> cc_var_label;
	unsigned int cc_count = FactorGraphStructurizer::ConnectedComponents(
		fg, factor_is_removed, cc_var_label);

	// Instantiate a conditioned factor graph for each connected component
	mf_comp.resize(cc_count);
	mf_comp_inf.resize(cc_count);
	for (unsigned int ci = 0; ci < cc_count; ++ci) {
		// Collect all variables in this component
		std::vector<unsigned int> cond_var_set;
		std::unordered_set<unsigned int> cond_var;
		for (unsigned int vi = 0; vi < cc_var_label.size(); ++vi) {
			// Variable in this component -> do not condition
			if (cc_var_label[vi] == ci)
				continue;

			// Condition on all variables not in this component
			cond_var_set.push_back(vi);
			cond_var.insert(vi);
		}

		// Collect all factors in which both conditioned and unconditioned
		// variables appear and initialize the expectation to uniform.
		std::vector<unsigned int> cond_fac_subset;
		std::vector<std::vector<double> > cond_expect;
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			bool has_cond = false;	// Factor has conditioned-on variables
			bool has_uncond = false;	// Factor has unconditioned variables
			const std::vector<unsigned int>& fac_vars =
				factors[fi]->Variables();
			const std::vector<unsigned int>& fac_card =
				factors[fi]->Cardinalities();
			unsigned int cond_prod_card = 1;
			for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi) {
				if (cond_var.count(fac_vars[fvi]) == 0) {
					has_uncond = true;
				} else {
					has_cond = true;
					cond_prod_card *= fac_card[fvi];
				}
			}
			// Fully conditioned and fully unconditioned factors -> skip
			if (has_cond == false || has_uncond == false)
				continue;

			// We have a mixed factor, produce a uniform expectation
			// observation
			std::vector<double> cur_cond_marg(cond_prod_card,
				1.0/static_cast<double>(cond_prod_card));

			// Add this factor as to be conditioned
			cond_fac_subset.push_back(fi);
			cond_expect.push_back(cur_cond_marg);
		}

		// Build a partial observation from all the conditioned factors
		FactorGraphPartialObservation pobs(cond_var_set, cond_fac_subset,
			cond_expect);

		// Condition and add
		std::vector<unsigned int> var_new_to_orig;
		FactorGraph* fg_cond = Conditioning::ConditionFactorGraph(fcond_tab,
			fg, &pobs, var_new_to_orig);

		// Store conditioned factor graph and create an inference object.
		// The conditioned factor graph is always tree structured.
		mf_comp[ci] = fg_cond;
		mf_comp_inf[ci] = new TreeInference(fg_cond);

		// Collect all mean-field factors
		std::unordered_set<unsigned int>
			cond_fac_subset_hs(cond_fac_subset.begin(), cond_fac_subset.end());
		const std::vector<Factor*>& comp_factors = fg_cond->Factors();
		for (unsigned int comp_fi = 0; comp_fi < comp_factors.size();
			++comp_fi) {
			Factor* comp_fac = comp_factors[comp_fi];
			// Is conditioned?  No -> skip
			if (cond_fac_subset_hs.count(
				fac_to_fi[fcond_tab->OriginalFactor(comp_fac)]) == 0)
				continue;

			meanfield_factor_set.insert(comp_fac);
			cfac_to_mfi[comp_fac] = ci;
			cfac_to_cfi[comp_fac] = comp_fi;
		}
	}

	// Create lookup tables needed during inference
	for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi) {
		const std::vector<Factor*>& mfi_factors = mf_comp[mfi]->Factors();
		for (unsigned int comp_fi = 0; comp_fi < mfi_factors.size();
			++comp_fi) {
			Factor* new_factor = mfi_factors[comp_fi];
			bool is_conditioned = meanfield_factor_set.count(new_factor) != 0;
			if (is_conditioned == false)
				continue;

			unsigned int orig_fi =
				fac_to_fi[fcond_tab->OriginalFactor(new_factor)];
			fi_to_condfac[orig_fi].insert(new_factor);
		}
	}
}

InferenceMethod* StructuredMeanFieldInference::Produce(
	const FactorGraph* fg) const {
	StructuredMeanFieldInference* smf =
		new StructuredMeanFieldInference(fg, fcond_tab);
	smf->SetParameters(verbose, conv_tol, max_iter);

	return (smf);
}

void StructuredMeanFieldInference::SetParameters(bool verbose,
	double conv_tol, unsigned int max_iter) {
	this->verbose = verbose;
	assert(conv_tol >= 0.0);
	this->conv_tol = conv_tol;
	this->max_iter = max_iter;
}

void StructuredMeanFieldInference::PerformInference() {
	// 1. Compute initial log partition function
	log_z = 0.0;
	for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi) {
		mf_comp[mfi]->ForwardMap();
		mf_comp_inf[mfi]->PerformInference();
	}

	ProduceMarginals();
	log_z = ComputeLogPartitionFunction();

	// 2. Iterate mean field on components
	double conv_measure = std::numeric_limits<double>::infinity();
	for (unsigned int iter = 1; (max_iter == 0 || iter <= max_iter) &&
		conv_measure >= conv_tol; ++iter)
	{
		if (verbose) {
			std::cout << "iter " << iter << ", logZ " << log_z
				<< ", conv " << conv_measure << std::endl;
		}

		// For each component,
		for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi) {
			// i) update all conditioning expectation information
			UpdateComponentEnergies(mfi);
			mf_comp[mfi]->ForwardMap();

			// ii) Perform inference
			mf_comp_inf[mfi]->PerformInference();
		}
		// Produce all marginals, either as copy or product
		ProduceMarginals();

		// Compute bound to the log-partition function
		double log_z_prev = log_z;
		log_z = ComputeLogPartitionFunction();
		assert(log_z >= (log_z_prev - 1.0e-11));
		conv_measure = std::max(0.0, log_z - log_z_prev);
	}
	if (verbose)
		std::cout << "Converged with tol " << conv_measure << "." << std::endl;

	// 3. Delete component inference results
	for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi)
		mf_comp_inf[mfi]->ClearInferenceResult();
}

// This method has the responsibility to update all information related to
// component 'mfi'.  In particular, the factors that are part of mfi but
// conditioned on other component inference results need to be processed.  The
// result of this processing are new expectations applied to the conditioning
// factors, producing new energies.
void StructuredMeanFieldInference::UpdateComponentEnergies(unsigned int mfi) {
	FactorGraph* comp = mf_comp[mfi];
	const std::vector<Factor*>& mfi_factors = comp->Factors();

	// For each factor in mfi
	for (unsigned int comp_fi = 0; comp_fi < mfi_factors.size();
		++comp_fi) {
		Factor* new_factor = mfi_factors[comp_fi];
		bool is_conditioned = meanfield_factor_set.count(new_factor) != 0;
		if (is_conditioned == false)
			continue;	// Skip unconditioned factors

		// It is a conditioned factor
		Factor* orig_factor = fcond_tab->OriginalFactor(new_factor);
		assert(orig_factor != 0);
		assert(fac_to_fi.count(orig_factor) > 0);
		unsigned int orig_fi = fac_to_fi[orig_factor];
		assert(fi_to_condfac.count(orig_fi) > 0);
		// Set of all factors derived by conditioning this original factor
		const std::unordered_set<Factor*>& cond_facs =
			fi_to_condfac[orig_fi];

		// Schematic
		//
		//       (0) ---[F]--- (1)     // original factor
		//       (0) ---[G]            // conditioned factor 1
		//              [H]--- (1)     // conditioned factor 2
		//
		// new_factor is G, orig_factor is F, cond_facs is {G,H}.
		//
		// We now build new expectations used to update factor G.  We build
		// these from the marginals of cond_facs \ {new_factor}, hence from H.
		std::vector<double> ext_marginals(
			orig_factor->Type()->ProdCardinalities(), 1.0);
		size_t cond_expect_prodcard = 1;
		for (std::unordered_set<Factor*>::const_iterator
			cfi = cond_facs.begin(); cfi != cond_facs.end(); ++cfi) {
			// Ignore own marginals
			if (*cfi == new_factor)
				continue;

			// Obtain the conditional marginals
			assert(cfac_to_mfi.find(new_factor) != cfac_to_mfi.end());
			assert(cfac_to_cfi.find(new_factor) != cfac_to_cfi.end());
			const std::vector<double>& cfi_marg =
				mf_comp_inf[cfac_to_mfi[*cfi]]->Marginal(cfac_to_cfi[*cfi]);

			// Extend and multiply
			std::vector<double> cfi_ext_marginals(
				orig_factor->Type()->ProdCardinalities(), 0.0);
			fcond_tab->ExtendMarginals(*cfi, cfi_marg,
				cfi_ext_marginals, true);
			std::transform(cfi_ext_marginals.begin(), cfi_ext_marginals.end(),
				ext_marginals.begin(), ext_marginals.begin(), _1 * _2);
			cond_expect_prodcard *= (*cfi)->Type()->ProdCardinalities();
		}

		// Project to conditioned-on expectations
		assert(cond_expect_prodcard ==
			(orig_factor->Type()->ProdCardinalities() /
			new_factor->Type()->ProdCardinalities()));
		std::vector<double> cond_var_expect(cond_expect_prodcard);
		fcond_tab->ProjectExtendedMarginalsCond(new_factor, ext_marginals,
			cond_var_expect);

		// Update conditioned factor with new expectations
		fcond_tab->UpdateConditioningInformation(new_factor, cond_var_expect);
	}
}

void StructuredMeanFieldInference::ProduceMarginals() {
	// Initialize all marginals to 1.0 because unconditioned factors will
	// overwrite the marginals and conditioned factors multiply the values
	// (factorial meanfield assumption).
	const std::vector<Factor*>& factors = fg->Factors();
	marginals.resize(factors.size());
	std::unordered_map<Factor*, unsigned int> fac_to_fi;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		marginals[fi].resize(factors[fi]->Type()->ProdCardinalities());
		std::fill(marginals[fi].begin(), marginals[fi].end(), 1.0);
		fac_to_fi.insert(std::pair<Factor*, unsigned int>(factors[fi], fi));
	}
	for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi) {
		const std::vector<Factor*>& mfi_factors = mf_comp[mfi]->Factors();
		for (unsigned int comp_fi = 0; comp_fi < mfi_factors.size();
			++comp_fi) {
			Factor* new_factor = mfi_factors[comp_fi];
			// Original factor index
			unsigned int orig_fi =
				fac_to_fi[fcond_tab->OriginalFactor(new_factor)];
			// Marginal from the component (possibly conditioned)
			const std::vector<double>& comp_marg =
				mf_comp_inf[mfi]->Marginal(comp_fi);

			bool is_conditioned = meanfield_factor_set.count(new_factor) != 0;
			if (is_conditioned == false) {
				// Unconditioned factor: copy marginals
				assert(comp_marg.size() == marginals[orig_fi].size());
				std::copy(comp_marg.begin(), comp_marg.end(),
					marginals[orig_fi].begin());
				continue;
			}

			// Conditioned factor: extend to full marginals and multiply
			std::vector<double> ext_marginals(marginals[orig_fi].size(), 0.0);
			fcond_tab->ExtendMarginals(new_factor, comp_marg,
				ext_marginals, true);
			std::transform(ext_marginals.begin(), ext_marginals.end(),
				marginals[orig_fi].begin(), marginals[orig_fi].begin(),
				_1 * _2);
		}
	}
}

double StructuredMeanFieldInference::ComputeLogPartitionFunction() {
	const std::vector<Factor*>& factors = fg->Factors();
	double mu_theta = 0.0;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		// <\theta,\mu>, where \theta=-E in our notation.
		mu_theta -= std::inner_product(marginals[fi].begin(),
			marginals[fi].end(), factors[fi]->Energies().begin(), 0.0);
	}
	// + \sum_m H_m(\mu)
	double H_sum = 0.0;
	for (unsigned int mfi = 0; mfi < mf_comp.size(); ++mfi)
		H_sum += mf_comp_inf[mfi]->Entropy();
#if 0
	std::cout << "mu_theta = " << mu_theta << std::endl;
	std::cout << "H_sum = " << H_sum << std::endl;
	std::cout << "log Z >= " << (mu_theta + H_sum) << std::endl;
#endif

	return (mu_theta + H_sum);
}

void StructuredMeanFieldInference::ClearInferenceResult() {
	marginals.clear();
}

const std::vector<double>& StructuredMeanFieldInference::Marginal(
	unsigned int factor_id) const {
	assert(factor_id < marginals.size());
	return (marginals[factor_id]);
}

const std::vector<std::vector<double> >&
StructuredMeanFieldInference::Marginals() const {
	return (marginals);
}

double StructuredMeanFieldInference::LogPartitionFunction() const {
	return (log_z);
}

void StructuredMeanFieldInference::Sample(
	std::vector<std::vector<unsigned int> >& states,
	unsigned int sample_count) {
	// NOT IMPLEMENTED
	assert(0);
}

double StructuredMeanFieldInference::MinimizeEnergy(
	std::vector<unsigned int>& state) {
	// NOT IMPLEMENTED
	assert(0);
	return (std::numeric_limits<double>::signaling_NaN());
}

}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <metaCommand.h>
#include "mitkCommandLineParser.h"
#include <usAny.h>
#include <mitkIOUtil.h>
#include <boost/lexical_cast.hpp>
#include <mitkCoreObjectFactory.h>
#include <mitkPlanarFigure.h>
#include <mitkPlanarFigureComposite.h>
#include <mitkFiberBundle.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <mitkStandaloneDataStorage.h>

#define _USE_MATH_DEFINES
#include <math.h>

using namespace std;

/*!
\brief Extract fibers from a tractogram using planar figure ROIs
*/
int main(int argc, char* argv[])
{
    mitkCommandLineParser parser;

    parser.setTitle("Fiber Extraction");
    parser.setCategory("Fiber Tracking and Processing Methods");
    parser.setContributor("MBI");
    parser.setDescription("Extract fibers from a tractogram using planar figure ROIs");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::String, "Input:", "input tractogram (.fib/.trk)", us::Any(), false);
    parser.addArgument("out", "o", mitkCommandLineParser::String, "Output:", "output tractogram", us::Any(), false);
    parser.addArgument("planfirgure1", "pf1", mitkCommandLineParser::String, "Figure 1:", "first ROI", us::Any(), false);
    parser.addArgument("planfirgure2", "pf2", mitkCommandLineParser::String, "Figure 2:", "second ROI", us::Any());
    parser.addArgument("operation", "op", mitkCommandLineParser::String, "Operation:", "logical operation (AND, OR, NOT)", us::Any());

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    string inFib = us::any_cast<string>(parsedArgs["input"]);
    string outFib = us::any_cast<string>(parsedArgs["out"]);
    string pf1_path = us::any_cast<string>(parsedArgs["planfirgure1"]);

    string operation("");
    string pf2_path("");
    if (parsedArgs.count("operation"))
    {
        operation = us::any_cast<string>(parsedArgs["operation"]);
        if (parsedArgs.count("planfirgure2") && (operation=="AND" || operation=="OR"))
            pf2_path = us::any_cast<string>(parsedArgs["planfirgure2"]);
    }

    try
    {
        typedef itk::Image<unsigned char, 3>                                    ItkUcharImgType;

        // load fiber bundle
        mitk::FiberBundle::Pointer inputTractogram = dynamic_cast<mitk::FiberBundle*>(mitk::IOUtil::LoadDataNode(inFib)->GetData());
        mitk::FiberBundle::Pointer result;

        mitk::StandaloneDataStorage::Pointer storage = mitk::StandaloneDataStorage::New();
        mitk::DataNode::Pointer input1 = mitk::IOUtil::LoadDataNode(pf1_path);

        if (input1.IsNotNull())
        {
            mitk::PlanarFigureComposite::Pointer pfc = mitk::PlanarFigureComposite::New();
            mitk::DataNode::Pointer pfcNode = mitk::DataNode::New();
            pfcNode->SetData(pfc);
            mitk::DataStorage::SetOfObjects::Pointer set1 = mitk::DataStorage::SetOfObjects::New();
            set1->push_back(pfcNode);
            storage->Add(pfcNode);

            mitk::DataNode::Pointer input2;
            if (!pf2_path.empty())
            {
                input2 = mitk::IOUtil::LoadDataNode(pf2_path);
            }

            if (operation.empty())
            {
                result = inputTractogram->ExtractFiberSubset(input1, NULL);
            }
            else if (operation=="NOT")
            {
                pfc->setOperationType(mitk::PlanarFigureComposite::NOT);
                storage->Add(input1, set1);
                result = inputTractogram->ExtractFiberSubset(pfcNode, storage);
            }
            else if (operation=="AND" && input2.IsNotNull())
            {
                pfc->setOperationType(mitk::PlanarFigureComposite::AND);
                storage->Add(input1, set1);
                storage->Add(input2, set1);
                result = inputTractogram->ExtractFiberSubset(pfcNode, storage);
            }
            else if (operation=="OR" && input2.IsNotNull())
            {
                pfc->setOperationType(mitk::PlanarFigureComposite::OR);
                storage->Add(input1, set1);
                storage->Add(input2, set1);
                result = inputTractogram->ExtractFiberSubset(pfcNode, storage);
            }
            else
            {
                std::cout << "Could not process input:";
                std::cout << pf1_path;
                std::cout << pf2_path;
                std::cout << operation;
            }
        }
        else
        {
            ItkUcharImgType::Pointer itkMaskImage = ItkUcharImgType::New();
            mitk::Image::Pointer mitkMaskImage = dynamic_cast<mitk::Image*>(mitk::IOUtil::LoadDataNode(pf1_path)->GetData());
            mitk::CastToItkImage<ItkUcharImgType>(mitkMaskImage, itkMaskImage);

            if (operation=="NOT")
                result = inputTractogram->ExtractFiberSubset(itkMaskImage, true, true);
            else
                result = inputTractogram->ExtractFiberSubset(itkMaskImage, true, false);
        }

        if (result.IsNotNull())
            mitk::IOUtil::SaveBaseData(result, outFib);
        else
            std::cout << "No valid fiber bundle extracted.";
    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}


#ifndef TRIANGLE_HPP
#define TRIANGLE_HPP

#include <string>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "surface.hpp"
#include "ray.hpp"

class Triangle : public Surface {
  public:
    Triangle(Eigen::Vector4d p1, Eigen::Vector4d p2, Eigen::Vector4d p3);
    std::string getType();
    double calculateIntersectionTime(Ray ray);
    Eigen::Vector4d calculateIntersectionNormal(Eigen::Vector4d intersectionPoint);

  private:
    Eigen::Vector4d p1;
    Eigen::Vector4d p2;
    Eigen::Vector4d p3;
};

#endif // TRIANGLE_HPP


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_MUTATION_GENERATOR_HPP_
#define CLOTHO_MUTATION_GENERATOR_HPP_

#include <boost/property_tree/ptree.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "clotho/data_spaces/allele_space/allele_generator.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class GeneticSpaceType >
class MutationGenerator {
public:
    typedef GeneticSpaceType                            genetic_space_type;
    typedef typename genetic_space_type::allele_type    allele_type;

    typedef RNG                                 random_engine_type;

    typedef boost::random::uniform_int_distribution< size_t >   sequence_generator_type;
    typedef AlleleGenerator< random_engine_type, allele_type >  allele_generator_type;

    MutationGenerator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_allele_gen( rng, config )
    {}

    void operator()( genetic_space_type * child, size_t N ) {
        m_seq_gen.param( typename sequence_generator_type::param_type( 0, child->sequence_count() - 1 ) );

        while( N-- ) {
            size_t seq_idx = m_seq_gen( *m_rand );
            size_t all_idx = child->getAlleleSpace().next_free();

            child->getSequenceSpace().flip( seq_idx, all_idx );
            m_allele_gen( child->getAlleleSpace(), all_idx );
        }
    }

    virtual ~MutationGenerator() {}

protected:
    random_engine_type      * m_rand;
    allele_generator_type   m_allele_gen;
    sequence_generator_type m_seq_gen;
};

}   // namespace genetics
}   // namespace clotho

#include "clotho/data_spaces/mutation/mutation_generator_spec.hpp"

#endif  // CLOTHO_MUTATION_GENERATOR_HPP_