#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 <sensor_msgs/JointState.h>

#include<termios.h>
#include <iostream>

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 ;

void matrix_view(MatrixXd M){

    std_msgs::String str;
    str.data="\n";
for (int i = 0; i <M.rows() ; ++i) {

    for (int j = 0; j <M.cols() ; ++j) {
   str.data+=std::to_string(M(i,j));
   str.data+="\t";
    }
    str.data+="\n";
}
ROS_INFO("%s",str.data.c_str());
}

void matrix_view(VectorXd M){
    std_msgs::String str;
    str.data="\n";

for (int i = 0; i <M.rows() ; ++i) {
    str.data+=std::to_string(M(i));str.data+="\t";}
ROS_INFO("%s",str.data.c_str());
}

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;
}

VectorXd absolute_q0(7);

void abs_read(const sensor_msgs::JointState & msg){
    VectorXi absolute_sensor(7);
    for (int i = 0; i < 7; ++i) {
        absolute_sensor(i)=msg.position[i+13];
        absolute_q0(i)=0;
    }

    absolute_q0(0)=double(absolute_sensor(0)-2859)/8192*2*M_PI;
    absolute_q0(1)=double(absolute_sensor(1)-0427)/8192*2*M_PI;
    absolute_q0(2)=double(absolute_sensor(2)-4051)/8192*2*M_PI;
    absolute_q0(3)=double(absolute_sensor(3)-2355)/8192*2*M_PI;
}

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);

}

bool simulation;
int main(int argc, char **argv)
{
    simulation=true;

    if (simulation){    ros::init(argc, argv, "rrbot");}
    else{ros::init(argc, argv, "jointdata");}

    ros::NodeHandle nh("~");

    int movenum;
    nh.getParam("movenum", movenum);

 //   ROS_INFO("scenario: %s",scenario.c_str());

    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("/qc",1000);
    ros::Subscriber abs_sensor = nh.subscribe("/surena/abs_joint_state", 1000, abs_read);

    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);

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

    right_hand hand_funcs;
    int n=5;

    VectorXd r_target[n];

   // VectorXd r_middle(3);
    MatrixXd R_target[n];

    //**********************
    for (int i = 0; i < n; ++i) {
        r_target[i].resize(3,1);
        R_target[i].resize(3,3);
    }

//    r_target[0]<<.2,
//            -0.1,
//            -0.45;
//    R_target[0]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(3,-70*M_PI/180,3)*hand_funcs.rot(2,-20*M_PI/180,3);
    r_target[0]<<.2,
            -0.1,
            -0.4;
    R_target[0]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-25*M_PI/180,3)*hand_funcs.rot(3,-25*M_PI/180,3);

    r_target[1]<<.35,
            -0.1,
            -0.35;
    R_target[1]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(3,-70*M_PI/180,3)*hand_funcs.rot(2,-20*M_PI/180,3);

    r_target[2]<<.4,
            -0.0,
            -0.15;
    R_target[2]=hand_funcs.rot(2,-100*M_PI/180,3)*hand_funcs.rot(3,-40*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);

    r_target[3]<<.2,
            -0.1,
            -0.4;
    R_target[3]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-25*M_PI/180,3)*hand_funcs.rot(3,-25*M_PI/180,3);

    r_target[3]<<.1,
            -0.25,
            -0.45;
    R_target[3]=hand_funcs.rot(3,-70*M_PI/180,3)*hand_funcs.rot(2,-50*M_PI/180,3);

    r_target[4]<<.25,
            -0.25,
            -0.35;
    R_target[4]=hand_funcs.rot(2,-80*M_PI/180,3)*hand_funcs.rot(1,-50*M_PI/180,3);

    //            //self recognition
    //            if(scenario=="a"){

    //                r_target<<.4,
    //                        0.1,
    //                        -0.15;
    //                R_target=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(2,-40*M_PI/180,3);
    //                r_middle<<.3,
    //                        -.0,
    //                        -.3;
    //            }

    //            //pointing
    //            if(scenario=="b"){

    //                r_target<<.05,
    //                        -0.56,
    //                        -0;
    //                R_target=hand_funcs.rot(1,-90*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);
    //                r_middle<<.1,
    //                        -.35,
    //                        -.3;
    //            }

    //            // mini touch
    //            if(scenario=="c"){

    //                r_target<<.4,
    //                        -0.3,
    //                        -0.2;
    //                R_target=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(2,20*M_PI/180,3);
    //                r_middle<<.2,
    //                        -.25,
    //                        -.35;
    //            }

    //            // looking at horizon
    //            if(scenario=="d"){

    //                r_target<<.33,
    //                        0.08,
    //                        0.24;
    //                R_target=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(1,-28*M_PI/180,3);
    //                r_middle<<.4,
    //                        -.2,
    //                        -0.05;
    //            }

    //            // respct
    //            if(scenario=="e"){

    //                r_target<<.25,
    //                        0.15,
    //                        -0.3;
    //                R_target=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,65*M_PI/180,3);
    //                r_middle<<.4,
    //                        -0.05,
    //                        -0.3;
    //            }

    //            // hand-shake
    //            if(scenario=="f"){

    //                r_target<<.45,
    //                        0.0,
    //                        -0.25;
    //                R_target=hand_funcs.rot(2,-80*M_PI/180,3);
    //                r_middle<<.3,
    //                        -0.05,
    //                        -0.4;
    //            }

    //            // waving
    //            if(scenario=="g"){

    //                r_target<<.35,
    //                        -0.1,
    //                        0.3;
    //                R_target=hand_funcs.rot(2,-180*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3);
    //                r_middle<<.4,
    //                        -0.2,
    //                        -0.2;

    VectorXd q0(7);

    double d0;
    double d  ;
    double d_des;
    double theta;
    double theta_target;
    double sai;
    double sai_target;
    double phi;
    double phi_target;

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

    MinimumJerkInterpolation coef_generator;
    MatrixXd X_coef;
    MatrixXd Y_coef;
    MatrixXd Z_coef;

    MatrixXd t(1,2);
    MatrixXd P_x(1,2);
    MatrixXd V_x(1,2);
    MatrixXd A_x(1,2);

    MatrixXd P_y(1,2);
    MatrixXd V_y(1,2);
    MatrixXd A_y(1,2);

    MatrixXd P_z(1,2);
    MatrixXd V_z(1,2);
    MatrixXd A_z(1,2);

    VectorXd q_ra;

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

    vector<double> q(31);

    ros::Rate loop_rate(200);
    int count = 0;
    double time=0.0;
    int num;
    int move_count=0;
    bool initializing=true;

    VectorXd qr_end(7);
    VectorXd P(3);
    VectorXd V(3);
    VectorXd A(3);
    while (ros::ok())
    {
        if(initializing){

            if(simulation){
                q0<<10*M_PI/180,
                        -4*M_PI/180,
                        0,
                        -20*M_PI/180,
                        0,
                        0,
                        0;
            }
            else{q0=absolute_q0;}
ROS_INFO("q0= %f, %f, %f, %f, %f, %f, %f",q0(0),q0(1),q0(2),q0(3),q0(4),q0(5),q0(6));

            double v0=0;
            double v_target =.4;
            right_hand hand0(q0,r_target[0],R_target[0],0,0);
            ROS_INFO("r0=%f,%f,%f",hand0.r_right_palm(0),hand0.r_right_palm(1),hand0.r_right_palm(2));

            d0=hand0.dist;
            d=d0;
            d_des=hand0.d_des;
            theta=hand0.theta;
            theta_target=hand0.theta_target;
            sai=hand0.sai;
            sai_target=hand0.sai_target;
            phi=hand0.phi;
            phi_target=hand0.phi_target;
            hand0.HO_FK_right_palm(q0);

            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));

            if (simulation)
            {
                vector<double> q_init(31);
                for (int i = 0; i < 31; ++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);
            }

            //****path generation

            t<<0,3;
            P_x<< hand0.r_right_palm(0),r_target[0](0);
            P_y<< hand0.r_right_palm(1),r_target[0](1);
            P_z<< hand0.r_right_palm(2),r_target[0](2);
            V_x<<0,INFINITY;
            V_y<<0,INFINITY;
            V_z<<0,INFINITY;
            A_x<<0,INFINITY;
            A_y<<0,INFINITY;
            A_z<<0,INFINITY;

            X_coef=coef_generator.Coefficient(t,P_x,V_x,A_x);
            Y_coef=coef_generator.Coefficient(t,P_y,V_y,A_y);
            Z_coef=coef_generator.Coefficient(t,P_z,V_z,A_z);
           // ROS_INFO("%d,%d",X_coef.rows(),X_coef.cols());
            //ROS_INFO("\ncoeff_x=%f\t%f\t%f\t%f\t%f\t%f",X_coef(0)
            //ROS_INFO("\n
            //ROS_INFO("\n

            //home
//            if(scenario=="h"){
//                 qr_end=q0;
////                qr_end<<0,
////                        0,
////                        0,
////                        0,
////                        0,
////                        0,
////                        0;
//            }

            //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("press any key to start!");
            getch();
            initializing=false;
        }

        else {

//ROS_INFO("time=%f",time);
            //time=double(count)*.005;
            time+=.005;
            if(time>t(1)){

                time=0;
                move_count++;
                if (move_count>movenum){break;}
ROS_INFO("%d",num);
matrix_view(r_target[num]);
                num=rand()%(n-1)+1;
                right_hand hand_pose(q_ra,r_target[num],R_target[num]);
                P=hand_pose.r_right_palm;

matrix_view(P);
//ROS_INFO("num=%d",num);
                P_x<<P(0),r_target[num](0);
                P_y<<P(1),r_target[num](1);
                P_z<<P(2),r_target[num](2);
                V_x<<V(0),INFINITY;
                V_y<<V(1),INFINITY;
                V_z<<V(2),INFINITY;
                A_x<<A(0),INFINITY;
                A_y<<A(1),INFINITY;
                A_z<<A(2),INFINITY;

                X_coef=coef_generator.Coefficient(t,P_x,V_x,A_x);
                Y_coef=coef_generator.Coefficient(t,P_y,V_y,A_y);
                Z_coef=coef_generator.Coefficient(t,P_z,V_z,A_z);

//matrix_view(X_coef);

            }
         //   if(scenario=="h"){time+=t(2);}

            P<<     coef_generator.GetAccVelPos(X_coef.row(0),time,0,5)(0,0),
                    coef_generator.GetAccVelPos(Y_coef.row(0),time,0,5)(0,0),
                    coef_generator.GetAccVelPos(Z_coef.row(0),time,0,5)(0,0);

            V<<     coef_generator.GetAccVelPos(X_coef.row(0),time,0,5)(0,1),
                    coef_generator.GetAccVelPos(Y_coef.row(0),time,0,5)(0,1),
                    coef_generator.GetAccVelPos(Z_coef.row(0),time,0,5)(0,1);
            A<<     coef_generator.GetAccVelPos(X_coef.row(0),time,0,5)(0,2),
                    coef_generator.GetAccVelPos(Y_coef.row(0),time,0,5)(0,2),
                    coef_generator.GetAccVelPos(Z_coef.row(0),time,0,5)(0,2);
                right_hand hand(q_ra,V,r_target[num],R_target[num]);
                hand.doQP(q_ra);
                q_ra=hand.q_next;
                d=hand.dist;
                theta=hand.theta;
                sai=hand.sai;
                phi=hand.phi;

//            else if (time<t(2)){
//                P<<coef_generator.GetAccVelPos(X_coef.row(1),time,t(1),5)(0,0),
//                        coef_generator.GetAccVelPos(Y_coef.row(1),time,t(1),5)(0,0),
//                        coef_generator.GetAccVelPos(Z_coef.row(1),time,t(1),5)(0,0);

//                V<<coef_generator.GetAccVelPos(X_coef.row(1),time,t(1) ,5)(0,1),
//                        coef_generator.GetAccVelPos(Y_coef.row(1),time,t(1),5)(0,1),
//                        coef_generator.GetAccVelPos(Z_coef.row(1),time,t(1),5)(0,1);
//                right_hand hand(q_ra,V,r_target,R_target);
//                hand.doQP(q_ra);
//                q_ra=hand.q_next;
//                d=hand.dist;
//                theta=hand.theta;
//                sai=hand.sai;
//                phi=hand.phi;
//                qr_end=q_ra;
//                ROS_INFO("%f\t%f\t%f\t%f\t%f\t%f\t%f\t",qr_end(0),qr_end(1),qr_end(2),qr_end(3),qr_end(4),qr_end(5),qr_end(6));

//            }
//            else if (time<t(2)*2){

//                if(scenario=="g" && time<8){q_ra(2)=qr_end(2)+15*M_PI/180*sin((time-t(2))/2*(2*M_PI));}
//                else if(scenario=="f" && time<8){q_ra(3)=qr_end(3)+5*M_PI/180*sin((time-t(2))/2*(2*M_PI));}
//                else{

//                    ROS_INFO_ONCE("reached!");
//if(scenario=="h")
//{
//    MatrixXd t_r(1,2);
//    MatrixXd p_r(7,2);
//    MatrixXd z_r(1,2);
//    MatrixXd r_coeff;
//    t_r<<t(2),t(2)*2;
//    z_r<<0,0;

//    p_r<<qr_end(0),10*M_PI/180,
//            qr_end(1),-4*M_PI/180,
//            qr_end(2),0,
//            qr_end(3),-20*M_PI/180,
//            qr_end(4),0,
//            qr_end(5),0,
//            qr_end(6),0;

//    for (int i = 0; i < 7; ++i) {
//        r_coeff=coef_generator.Coefficient(t_r,p_r.row(i),z_r,z_r);
//        q_ra(i)=coef_generator.GetAccVelPos(r_coeff.row(0),time,t_r(0) ,5)(0,0);

//    }
//}
 //else{break;}

                    //                    r_coeff=(coef_generator.Coefficient(t_r,p_r,z_r,z_r));
                    //                    q_ra(0)=coef_generator.GetAccVelPos(r_coeff.row(0),time,t_r(0) ,5)(0,0);

                    // ROS_INFO("  %f\t%f\t%f\t%f\t%f\t%f\t%f\t",q_ra(0),q_ra(1),q_ra(2),q_ra(3),q_ra(4),q_ra(5),q_ra(6));

                }

                //else{break;}

          //  else{break;}

            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));

            //   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("t=%f\nx=%f\ty=%f\tz=%f\t\nX_des=%f\tY_des=%f\tZ_des=%f",time,hand.r_right_palm(0),hand.r_right_palm(1),hand.r_right_palm(2),
            //                  P(0),P(1),P(2));
            //        ROS_INFO("psi=%f(%f)\ttheta=%f(%f)\tphi=%f(%f)\t",hand.sai,hand.sai_target,hand.theta,hand.theta_target,hand.phi,hand.phi_target);

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

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

                    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];

                //     ROS_INFO("%f,%f,%f,%f,%f,%f,%f",q[15],q[16],q[17],q[18],q[19],q[20],q[21]);
                //   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));

                SendGazebo(q);

            }

            msg.data.clear();

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

            q_motor[0]=-int(7*(qr1[count]-q0(0))*360/M_PI);
            q_motor[1]=int(7*(qr2[count]-q0(1))*360/M_PI);
            q_motor[2]=-int(10*(qr3[count]-q0(2))*300/M_PI);
            q_motor[3]=int(10*(qr4[count]-q0(3))*300/M_PI);

            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))-1200;
            q_motor[6]=int((qr7[count]-q0(6))*(4000-2050)/(23*M_PI/180))-200;

            q_motor[7]=8;

            //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(8);//27
            ++count;
            chatter_pub.publish(msg);
        }

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

    return 0;
}


/*
 * Copyright 2016 Maikel Nadolski
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "hidden-markov-models.t.h"

#include <fstream>
#include <Eigen/Dense>
#include "maikel/hmm/io.h"
#include "maikel/hmm/hidden_markov_model.h"

CASE ( "Can read the text file 'A.dat' and create a dynamic allocated matrix." )
{
  std::ifstream in("A.dat");
  size_t n, m;
  std::tie(n, m) = maikel::hmm::getdims<size_t>(in);
  Eigen::MatrixXf matrix = maikel::hmm::read_ascii_matrix<float>(in, n, m);
  Eigen::MatrixXf checking(3,3);
  checking << 0.33333, 0.33333, 0.33333,
              0.4, 0.4, 0.2,
              0.5, 0.0, 0.5;
  EXPECT(checking == matrix);
}

CASE ( "Can see that model data is not stochastical." )
{
  std::ifstream in("no_good_model.dat");
  EXPECT_THROWS_AS(
      auto hmm = maikel::hmm::read_hidden_markov_model<float>(in),
      maikel::hmm::hidden_markov_model<float>::arguments_not_probability_arrays
  );
}

CASE ( "Reads a valid model" )
{
  std::ifstream in("model.dat");
  EXPECT_NO_THROW(
      auto hmm = maikel::hmm::read_hidden_markov_model<float>(in)
  );
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/core/plugin.h>
#include <mitsuba/core/statistics.h>
#include <mitsuba/core/chisquare.h>
#include <mitsuba/core/fresolver.h>
#include <mitsuba/render/testcase.h>
#include <boost/bind.hpp>

/* Statistical significance level of the test. Set to
   1/4 percent by default -- we want there to be strong
   evidence of an implementaiton error before failing
   a test case */
#define SIGNIFICANCE_LEVEL 0.0025f

/* Relative bound on what is still accepted as roundoff
   error -- be quite tolerant */
#if defined(SINGLE_PRECISION)
    #define ERROR_REQ 1e-2f
#else
    #define ERROR_REQ 1e-5
#endif

MTS_NAMESPACE_BEGIN

/**
 * This testcase checks if the sampling methods of various BSDF & phase
 * function & emitter implementations really do what they promise in
 * their pdf() methods
 */
class TestChiSquare : public TestCase {
public:
    MTS_BEGIN_TESTCASE()
    MTS_DECLARE_TEST(test01_BSDF)
    MTS_DECLARE_TEST(test02_PhaseFunction)
    MTS_DECLARE_TEST(test03_EmitterDirect)
    MTS_END_TESTCASE()

    /**
     * Replayable fake sampler
     */
    class FakeSampler : public Sampler {
    public:
        FakeSampler(Sampler *sampler)
            : Sampler(Properties()), m_sampler(sampler) { }

        Float next1D() {
            while (m_sampleIndex >= m_values.size())
                m_values.push_back(m_sampler->next1D());
            return m_values[m_sampleIndex++];
        }

        Point2 next2D() {
            return Point2(next1D(), next1D());
        }

        void clear() {
            m_values.clear();
            m_sampleIndex = 0;
        }

        void rewind() {
            m_sampleIndex = 0;
        }

        ref<Sampler> clone() {
            SLog(EError, "Not supported!");
            return NULL;
        }

        std::string toString() const { return "FakeSampler[]"; }
    private:
        ref<Sampler> m_sampler;
        std::vector<Float> m_values;
    };

    /// Adapter to use BSDFs in the chi-square test
    class BSDFAdapter {
    public:
        BSDFAdapter(const BSDF *bsdf, Sampler *sampler, const Vector &wi, int component)
            : m_bsdf(bsdf), m_sampler(sampler), m_wi(wi), m_component(component),
              m_largestWeight(0) {
            m_fakeSampler = new FakeSampler(m_sampler);
            m_its.uv = Point2(0.0f);
            m_its.dpdu = Vector(1, 0, 0);
            m_its.dpdv = Vector(0, 1, 0);
            m_its.dudx = m_its.dvdy = 0.01f;
            m_its.dudy = m_its.dvdx = 0.00f;
            m_its.shFrame = Frame(Normal(0, 0, 1));
//          m_isSymmetric = true;
        }

        boost::tuple<Vector, Float, EMeasure> generateSample() {
            Point2 sample(m_sampler->next2D());
            BSDFSamplingRecord bRec(m_its, m_fakeSampler);
            bRec.mode = EImportance;
            bRec.component = m_component;
            bRec.wi = m_wi;

            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif

            Float pdfVal, sampledPDF;

            /* Check the various sampling routines for agreement
               amongst each other */
            m_fakeSampler->clear();
            Spectrum sampled = m_bsdf->sample(bRec, sampledPDF, sample);
            m_fakeSampler->rewind();
            Spectrum sampled2 = m_bsdf->sample(bRec, sample);
            EMeasure measure = ESolidAngle;
            if (!sampled.isZero())
                measure = BSDF::getMeasure(bRec.sampledType);

            if (sampled.isZero() && sampled2.isZero())
                return boost::make_tuple(Vector(0.0f), 0.0f, measure);

            Spectrum f = m_bsdf->eval(bRec, measure);
            pdfVal = m_bsdf->pdf(bRec, measure);
            Spectrum manual = f/pdfVal;

#if 0
            if (m_isSymmetric) {
                /* Check for non-symmetry */
                BSDFSamplingRecord bRecRev(bRec);
                bRecRev.reverse();
                bRec.mode = EImportance;
                Spectrum fFwd = f;
                Spectrum fRev = m_bsdf->eval(bRecRev, measure);
                if (measure == ESolidAngle) {
                    fFwd /= std::abs(Frame::cosTheta(bRec.wo));
                    fRev /= std::abs(Frame::cosTheta(bRecRev.wo));
                }
                Float max = std::max(fFwd.max(), fRev.max());
                if (max > 0) {
                    Float err = (fFwd-fRev).max() / max;
                    if (err > Epsilon) {
                        Log(EWarn, "Non-symmetry in %s: %s vs %s, %s", m_bsdf->toString().c_str(),
                            fFwd.toString().c_str(), fRev.toString().c_str(), bRec.toString().c_str());
                        m_isSymmetric = false;
                    }
                }
            }
#endif

            if (!sampled.isValid() || !sampled2.isValid() || !manual.isValid()) {
                Log(EWarn, "Oops: sampled=%s, sampled2=%s, manual=%s, sampledPDF=%f, "
                    "pdf=%f, f=%s, bRec=%s, measure=%i", sampled.toString().c_str(),
                    sampled2.toString().c_str(), manual.toString().c_str(),
                    sampledPDF, pdfVal, f.toString().c_str(), bRec.toString().c_str(),
                    measure);
                return boost::make_tuple(bRec.wo, 0.0f, ESolidAngle);
            }

            bool mismatch = false;
            for (int i=0; i<SPECTRUM_SAMPLES; ++i) {
                Float a = sampled[i], b = sampled2[i], c = manual[i];
                Float min = std::min(std::min(a, b), c);
                Float err = std::max(std::max(std::abs(a - b), std::abs(a - c)), std::abs(b - c));
                m_largestWeight = std::max(m_largestWeight, a);

                if (min < ERROR_REQ && err > ERROR_REQ) // absolute error threshold
                    mismatch = true;
                else if (min > ERROR_REQ && err/min > ERROR_REQ) // relative error threshold
                    mismatch = true;
            }

            if (mismatch)
                Log(EWarn, "Potential inconsistency: sampled=%s, sampled2=%s, manual=%s, sampledPDF=%f, "
                    "pdf=%f, f=%s, bRec=%s, measure=%i", sampled.toString().c_str(),
                    sampled2.toString().c_str(), manual.toString().c_str(),
                    sampledPDF, pdfVal, f.toString().c_str(), bRec.toString().c_str(),
                    measure);

            mismatch = false;
            Float min = std::min(pdfVal, sampledPDF);
            Float err = std::abs(pdfVal - sampledPDF);

            if (min < ERROR_REQ && err > ERROR_REQ) // absolute error threshold
                mismatch = true;
            else if (min > ERROR_REQ && err/min > ERROR_REQ) // relative error threshold
                mismatch = true;

            if (mismatch)
                Log(EWarn, "Potential inconsistency: pdfVal=%f, sampledPDF=%f",
                    pdfVal, sampledPDF);

            #if defined(MTS_DEBUG_FP)
                disableFPExceptions();
            #endif

            return boost::make_tuple(bRec.wo, 1.0f, measure);
        }

        Float pdf(const Vector &wo, EMeasure measure) {
            BSDFSamplingRecord bRec(m_its, m_wi, wo);
            bRec.mode = EImportance;
            bRec.component = m_component;

            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif

            if (m_bsdf->eval(bRec, measure).isZero())
                return 0.0f;

            Float result = m_bsdf->pdf(bRec, measure);

            #if defined(MTS_DEBUG_FP)
                disableFPExceptions();
            #endif
            return result;
        }

        inline Float getLargestWeight() const { return m_largestWeight; }
//      inline bool isSymmetric() const { return m_isSymmetric; }
    private:
        Intersection m_its;
        ref<const BSDF> m_bsdf;
        ref<Sampler> m_sampler;
        ref<FakeSampler> m_fakeSampler;
        Vector m_wi;
        int m_component;
        Float m_largestWeight;
//      bool m_isSymmetric;
    };

    /// Adapter to use Phase functions in the chi-square test
    class PhaseFunctionAdapter {
    public:
        PhaseFunctionAdapter(const MediumSamplingRecord &mRec,
                const PhaseFunction *phase, Sampler *sampler, const Vector &wi)
            : m_mRec(mRec), m_phase(phase), m_sampler(sampler), m_wi(wi),
              m_largestWeight(0) {
            m_fakeSampler = new FakeSampler(m_sampler);
        }

        boost::tuple<Vector, Float, EMeasure> generateSample() {
            PhaseFunctionSamplingRecord pRec(m_mRec, m_wi);

            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif
            Float sampledPDF, pdfVal;

            /* Check the various sampling routines for agreement amongst each other */
            m_fakeSampler->clear();
            Float sampled = m_phase->sample(pRec, sampledPDF, m_fakeSampler);
            m_fakeSampler->rewind();
            Float sampled2 = m_phase->sample(pRec, m_fakeSampler);
            Float f = m_phase->eval(pRec);
            pdfVal = m_phase->pdf(pRec);
            Float manual = f/pdfVal;

            if (std::isnan(sampled) || std::isnan(sampled2) || std::isnan(manual) ||
                sampled < 0 || sampled2 < 0 || manual < 0) {
                Log(EWarn, "Oops: sampled=%f, sampled2=%f, manual=%f, sampledPDF=%f, "
                    "pdf=%f, f=%f, pRec=%s", sampled, sampled2, manual,
                    sampledPDF, pdfVal, f, pRec.toString().c_str());
                return boost::make_tuple(pRec.wo, 0.0f, ESolidAngle);
            }

            bool mismatch = false;
            Float min = std::min(std::min(sampled, sampled2), manual);
            Float err = std::max(std::max(std::abs(sampled - sampled2),
                    std::abs(sampled - manual)), std::abs(sampled2 - manual));
            m_largestWeight = std::max(m_largestWeight, sampled);

            if (min < ERROR_REQ && err > ERROR_REQ) // absolute error threshold
                mismatch = true;
            else if (min > ERROR_REQ && err/min > ERROR_REQ) // relative error threshold
                mismatch = true;

            if (mismatch)
                Log(EWarn, "Potential inconsistency: sampled=%f, sampled2=%f, manual=%s, "
                    "sampledPDF=%f, pdf=%f, f=%f, pRec=%s", sampled, sampled2, manual,
                    sampledPDF, pdfVal, f, pRec.toString().c_str());

            mismatch = false;
            min = std::min(pdfVal, sampledPDF);
            err = std::abs(pdfVal - sampledPDF);

            if (min < ERROR_REQ && err > ERROR_REQ) // absolute error threshold
                mismatch = true;
            else if (min > ERROR_REQ && err/min > ERROR_REQ) // relative error threshold
                mismatch = true;

            if (mismatch)
                Log(EWarn, "Potential inconsistency: pdfVal=%f, sampledPDF=%f",
                    pdfVal, sampledPDF);

            return boost::make_tuple(pRec.wo,
                sampled == 0 ? 0.0f : 1.0f, ESolidAngle);
        }

        Float pdf(const Vector &wo, EMeasure measure) const {
            if (measure != ESolidAngle)
                return 0.0f;

            PhaseFunctionSamplingRecord pRec(m_mRec, m_wi, wo);
            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif
            if (m_phase->eval(pRec) == 0)
                return 0.0f;
            Float result = m_phase->pdf(pRec);
            #if defined(MTS_DEBUG_FP)
                disableFPExceptions();
            #endif
            return result;
        }

        inline Float getLargestWeight() const { return m_largestWeight; }
    private:
        const MediumSamplingRecord &m_mRec;
        ref<FakeSampler> m_fakeSampler;
        ref<const PhaseFunction> m_phase;
        ref<Sampler> m_sampler;
        Vector m_wi;
        Float m_largestWeight;
    };

    /// Adapter to use direct illumination sampling in the chi-square test
    class EmitterAdapter {
    public:
        EmitterAdapter(const Emitter *emitter, Sampler *sampler)
            : m_emitter(emitter), m_sampler(sampler), m_pRec(0.0f) {
            emitter->samplePosition(m_pRec, m_sampler->next2D());
        }

        boost::tuple<Vector, Float, EMeasure> generateSample() {
            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif

            DirectSamplingRecord dRec(Point(0.0f), 0);
            m_emitter->sampleDirect(dRec, m_sampler->next2D());

            #if defined(MTS_DEBUG_FP)
                disableFPExceptions();
            #endif

            return boost::make_tuple(dRec.d, 1.0f, dRec.measure);
        }

        Float pdf(const Vector &d, EMeasure measure) const {
            if (measure != ESolidAngle)
                return 0.0f;

            DirectSamplingRecord dRec(Point(0.0f), 0);
            dRec.d = d;
            dRec.measure = ESolidAngle;

            #if defined(MTS_DEBUG_FP)
                enableFPExceptions();
            #endif

            Float result = m_emitter->pdfDirect(dRec);

            #if defined(MTS_DEBUG_FP)
                disableFPExceptions();
            #endif

            return result;
        }

    private:
        ref<const Emitter> m_emitter;
        ref<Sampler> m_sampler;
        PositionSamplingRecord m_pRec;
    };

    void test01_BSDF() {
        /* Load a set of BSDF instances to be tested from the following XML file */
        FileResolver *resolver = Thread::getThread()->getFileResolver();
        const fs::path scenePath =
            resolver->resolveAbsolute("data/tests/test_bsdf.xml");
        ref<Scene> scene = loadScene(scenePath);

        const ref_vector<ConfigurableObject> &objects = scene->getReferencedObjects();
        int thetaBins = 10, wiSamples = 20, failureCount = 0, testCount = 0;
        ref<Sampler> sampler = static_cast<Sampler *> (PluginManager::getInstance()->
                createObject(MTS_CLASS(Sampler), Properties("independent")));
        ProgressReporter *progress = new ProgressReporter("Checking", wiSamples, NULL);

        Log(EInfo, "Verifying BSDF sampling routines ..");
        for (size_t i=0; i<objects.size(); ++i) {
            if (!objects[i]->getClass()->derivesFrom(MTS_CLASS(BSDF)))
                continue;

            const BSDF *bsdf = static_cast<const BSDF *>(objects[i].get());
            Float largestWeight = 0;

            Log(EInfo, "Processing BSDF model %s", bsdf->toString().c_str());

            Log(EInfo, "Checking the model for %i incident directions and 2D sampling", wiSamples);
            progress->reset();
#if 1
            /* Test for a number of different incident directions */
            for (int j=0; j<wiSamples; ++j) {
                Vector wi;

                if (bsdf->getType() & BSDF::EBackSide)
                    wi = warp::squareToUniformSphere(sampler->next2D());
                else
                    wi = warp::squareToCosineHemisphere(sampler->next2D());

                BSDFAdapter adapter(bsdf, sampler, wi, -1);
                ref<ChiSquare> chiSqr = new ChiSquare(thetaBins, 2*thetaBins, wiSamples);
                chiSqr->setLogLevel(EDebug);

                // Initialize the tables used by the chi-square test
                chiSqr->fill(
                    boost::bind(&BSDFAdapter::generateSample, &adapter),
                    boost::bind(&BSDFAdapter::pdf, &adapter, _1, _2)
                );

                // (the following assumes that the distribution has 1 parameter, e.g. exponent value)
                ChiSquare::ETestResult result = chiSqr->runTest(SIGNIFICANCE_LEVEL);
                if (result == ChiSquare::EReject) {
                    std::string filename = formatString("failure_%i.m", failureCount++);
                    chiSqr->dumpTables(filename);
                    failAndContinue(formatString("Uh oh, the chi-square test indicates a potential "
                        "issue for wi=%s. Dumped the contingency tables to '%s' for user analysis",
                        wi.toString().c_str(), filename.c_str()));
                } else {
                    succeed();
                }
                largestWeight = std::max(largestWeight, adapter.getLargestWeight());
                ++testCount;
                progress->update(j+1);

//              if (!adapter.isSymmetric())
//                  Log(EWarn, "****** BSDF is non-symmetric! ******");
            }
            Log(EInfo, "The largest encountered importance weight was = %.2f", largestWeight);

#endif
            largestWeight = 0;

            if (bsdf->getComponentCount() > 1) {
                for (int comp=0; comp<bsdf->getComponentCount(); ++comp) {
                    progress->reset();
                    Log(EInfo, "Individually checking BSDF component %i", comp);

                    /* Test for a number of different incident directions */
                    for (int j=0; j<wiSamples; ++j) {
                        Vector wi;

                        if (bsdf->getType(comp) & BSDF::EBackSide)
                            wi = warp::squareToUniformSphere(sampler->next2D());
                        else
                            wi = warp::squareToCosineHemisphere(sampler->next2D());

                        BSDFAdapter adapter(bsdf, sampler, wi, comp);

                        ref<ChiSquare> chiSqr = new ChiSquare(thetaBins, 2*thetaBins, wiSamples);
                        chiSqr->setLogLevel(EDebug);

                        // Initialize the tables used by the chi-square test
                        chiSqr->fill(
                            boost::bind(&BSDFAdapter::generateSample, &adapter),
                            boost::bind(&BSDFAdapter::pdf, &adapter, _1, _2)
                        );

                        // (the following assumes that the distribution has 1 parameter, e.g. exponent value)
                        ChiSquare::ETestResult result = chiSqr->runTest(SIGNIFICANCE_LEVEL);
                        if (result == ChiSquare::EReject) {
                            std::string filename = formatString("failure_%i.m", failureCount++);
                            chiSqr->dumpTables(filename);
                            failAndContinue(formatString("Uh oh, the chi-square test indicates a potential "
                                "issue for wi=%s. Dumped the contingency tables to '%s' for user analysis",
                                wi.toString().c_str(), filename.c_str()));
                        } else {
                            succeed();
                        }
                        largestWeight = std::max(largestWeight, adapter.getLargestWeight());
                        ++testCount;
                        progress->update(j+1);
                    }
                    Log(EInfo, "The largest encountered importance weight was = %.2f", largestWeight);
                    largestWeight = 0;
                }
            }
        }
        Log(EInfo, "%i/%i BSDF checks succeeded", testCount-failureCount, testCount);
        delete progress;
    }

    void test02_PhaseFunction() {
        /* Load a set of phase function instances to be tested from the following XML file */
        FileResolver *resolver = Thread::getThread()->getFileResolver();
        const fs::path scenePath =
            resolver->resolveAbsolute("data/tests/test_phase.xml");
        ref<Scene> scene = loadScene(scenePath);

        const ref_vector<ConfigurableObject> &objects = scene->getReferencedObjects();
        int thetaBins = 10, wiSamples = 20, failureCount = 0, testCount = 0;
        ref<Sampler> sampler = static_cast<Sampler *> (PluginManager::getInstance()->
                createObject(MTS_CLASS(Sampler), Properties("independent")));

        ProgressReporter *progress = new ProgressReporter("Checking", wiSamples, NULL);

        Log(EInfo, "Verifying phase function sampling routines ..");
        for (size_t i=0; i<objects.size(); ++i) {
            if (!objects[i]->getClass()->derivesFrom(MTS_CLASS(PhaseFunction)))
                continue;

            const PhaseFunction *phase = static_cast<const PhaseFunction *>(objects[i].get());
            Float largestWeight = 0;

            Log(EInfo, "Processing phase function model %s", phase->toString().c_str());
            Log(EInfo, "Checking the model for %i incident directions", wiSamples);
            progress->reset();
            MediumSamplingRecord mRec;

            /* Sampler fiber/particle orientation */
            mRec.orientation = warp::squareToUniformSphere(sampler->next2D());

            /* Test for a number of different incident directions */
            for (int j=0; j<wiSamples; ++j) {
                Vector wi = warp::squareToUniformSphere(sampler->next2D());

                PhaseFunctionAdapter adapter(mRec, phase, sampler, wi);
                ref<ChiSquare> chiSqr = new ChiSquare(thetaBins, 2*thetaBins, wiSamples);
                chiSqr->setLogLevel(EDebug);

                // Initialize the tables used by the chi-square test
                chiSqr->fill(
                    boost::bind(&PhaseFunctionAdapter::generateSample, &adapter),
                    boost::bind(&PhaseFunctionAdapter::pdf, &adapter, _1, _2)
                );

                // (the following assumes that the distribution has 1 parameter, e.g. exponent value)
                ChiSquare::ETestResult result = chiSqr->runTest(SIGNIFICANCE_LEVEL);
                if (result == ChiSquare::EReject) {
                    std::string filename = formatString("failure_%i.m", failureCount++);
                    chiSqr->dumpTables(filename);
                    failAndContinue(formatString("Uh oh, the chi-square test indicates a potential "
                        "issue for wi=%s. Dumped the contingency tables to '%s' for user analysis",
                        wi.toString().c_str(), filename.c_str()));
                } else {
                    succeed();
                }
                largestWeight = std::max(largestWeight, adapter.getLargestWeight());
                ++testCount;
                progress->update(j+1);
            }

            Log(EInfo, "Done with this phase function. The largest encountered "
                    "importance weight was = %.2f", largestWeight);
        }
        Log(EInfo, "%i/%i phase function checks succeeded", testCount-failureCount, testCount);
        delete progress;
    }

    void test03_EmitterDirect() {
        /* Load a set of emitter instances to be tested from the following XML file */
        FileResolver *resolver = Thread::getThread()->getFileResolver();
        const fs::path scenePath =
            resolver->resolveAbsolute("data/tests/test_emitter.xml");
        ref<Scene> scene = loadScene(scenePath);
        scene->initialize();

        const ref_vector<Emitter> &emitters = scene->getEmitters();
        int thetaBins = 10, failureCount = 0, testCount = 0;
        ref<Sampler> sampler = static_cast<Sampler *> (PluginManager::getInstance()->
                createObject(MTS_CLASS(Sampler), Properties("independent")));

        Log(EInfo, "Verifying emitter sampling routines ..");
        for (size_t i=0; i<emitters.size(); ++i) {
            const Emitter *emitter = emitters[i].get();

            Log(EInfo, "Processing emitter function model %s", emitter->toString().c_str());

            EmitterAdapter adapter(emitter, sampler);
            ref<ChiSquare> chiSqr = new ChiSquare(thetaBins, 2*thetaBins, 1);
            chiSqr->setLogLevel(EDebug);

            // Initialize the tables used by the chi-square test
            chiSqr->fill(
                boost::bind(&EmitterAdapter::generateSample, &adapter),
                boost::bind(&EmitterAdapter::pdf, &adapter, _1, _2)
            );
            chiSqr->dumpTables("test.m");

            // (the following assumes that the distribution has 1 parameter, e.g. exponent value)
            ChiSquare::ETestResult result = chiSqr->runTest(SIGNIFICANCE_LEVEL);
            if (result == ChiSquare::EReject) {
                std::string filename = formatString("failure_%i.m", failureCount++);
                chiSqr->dumpTables(filename);
                failAndContinue(formatString("Uh oh, the chi-square test indicates a potential "
                    "issue. Dumped the contingency tables to '%s' for user analysis",
                    filename.c_str()));
            } else {
                succeed();
            }
            ++testCount;
        }
        Log(EInfo, "%i/%i emitter checks succeeded", testCount-failureCount, testCount);
    }
};

MTS_EXPORT_TESTCASE(TestChiSquare, "Chi-square test for various sampling functions")
MTS_NAMESPACE_END


/**
 * @file split_sum.cpp
 *
 */

#include "split_sum.h"
#include "forecast_time.h"
#include "level.h"
#include "logger.h"
#include "plugin_factory.h"
#include <boost/thread.hpp>
#include <iostream>
#include <map>

#include "neons.h"
#include "radon.h"
#include "writer.h"

using namespace std;
using namespace himan::plugin;

mutex mySingleFileWriteMutex;

const int SUB_THREAD_COUNT = 5;

map<string, himan::params> sourceParameters;

split_sum::split_sum()
{
	itsLogger = logger("split_sum");

	// Define source parameters for each output parameter

	// General precipitation (liquid + solid)
	sourceParameters["RR-1-MM"] = {param("RR-KGM2")};
	sourceParameters["RR-3-MM"] = {param("RR-KGM2")};
	sourceParameters["RR-6-MM"] = {param("RR-KGM2")};
	sourceParameters["RR-12-MM"] = {param("RR-KGM2")};
	sourceParameters["RR-24-MM"] = {param("RR-KGM2")};

	sourceParameters["RRC-3-MM"] = {param("RRC-KGM2")};

	sourceParameters["RRR-KGM2"] = {param("RR-KGM2")};  // So-called HHSade
	sourceParameters["RRRC-KGM2"] = {param("RRC-KGM2")};
	sourceParameters["RRRL-KGM2"] = {param("RRL-KGM2")};

	// Snow
	sourceParameters["SN-3-MM"] = {param("SNACC-KGM2")};
	sourceParameters["SN-6-MM"] = {param("SNACC-KGM2")};
	sourceParameters["SN-24-MM"] = {param("SNACC-KGM2")};

	sourceParameters["SNR-KGM2"] = {param("SNACC-KGM2")};
	sourceParameters["SNRC-KGM2"] = {param("SNC-KGM2")};
	sourceParameters["SNRL-KGM2"] = {param("SNL-KGM2")};

	// Graupel
	sourceParameters["GRR-MMH"] = {param("GR-KGM2")};

	// Solid (snow + graupel + hail)
	sourceParameters["RRRS-KGM2"] = {param("RRS-KGM2")};

	// Radiation
	sourceParameters["RADGLO-WM2"] = {param("RADGLOA-JM2"), param("RADGLO-WM2")};
	sourceParameters["RADLW-WM2"] = {param("RADLWA-JM2"), param("RADLW-WM2")};
	sourceParameters["RTOPLW-WM2"] = {param("RTOPLWA-JM2"), param("RTOPLW-WM2")};
	sourceParameters["RNETLW-WM2"] = {param("RNETLWA-JM2"), param("RNETLW-WM2")};
	sourceParameters["RADSW-WM2"] = {param("RADSWA-JM2")};
}

void split_sum::Process(std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	/*
	 * Set target parameter to split_sum.
	 *
	 * We need to specify grib and querydata parameter information
	 * since we don't know which one will be the output format.
	 *
	 */

	vector<param> params;

	if (itsConfiguration->Exists("rr1h") && itsConfiguration->GetValue("rr1h") == "true")
	{
		param parm("RR-1-MM", 353, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 1));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rr3h") && itsConfiguration->GetValue("rr3h") == "true")
	{
		param parm("RR-3-MM", 354, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 3));
		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rr6h") && itsConfiguration->GetValue("rr6h") == "true")
	{
		param parm("RR-6-MM", 355, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 6));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rr12h") && itsConfiguration->GetValue("rr12h") == "true")
	{
		param parm("RR-12-MM", 356, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 12));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rr24h") && itsConfiguration->GetValue("rr24h") == "true")
	{
		param parm("RR-24-MM", 53, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 24));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("sn3h") && itsConfiguration->GetValue("sn3h") == "true")
	{
		param parm("SN-3-MM");
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 3));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("sn6h") && itsConfiguration->GetValue("sn6h") == "true")
	{
		param parm("SN-6-MM");
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 6));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("sn24h") && itsConfiguration->GetValue("sn24h") == "true")
	{
		param parm("SN-24-MM", 766, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 24));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rrc3h") && itsConfiguration->GetValue("rrc3h") == "true")
	{
		param parm("RRC-3-MM", 1025, 0, 1, 8);
		parm.Unit(kMm);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 3));
		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rrr") && itsConfiguration->GetValue("rrr") == "true")
	{
		param parm("RRR-KGM2", 49, 0, 1, 52);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rrrc") && itsConfiguration->GetValue("rrrc") == "true")
	{
		param parm("RRRC-KGM2", 201, 0, 1, 196);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("rrrl") && itsConfiguration->GetValue("rrrl") == "true")
	{
		param parm("RRRL-KGM2", 200, 0, 1, 54);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	// Graupel

	if (itsConfiguration->Exists("grr") && itsConfiguration->GetValue("grr") == "true")
	{
		param parm("GRR-MMH", 1168);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	// Solid

	if (itsConfiguration->Exists("rrrs") && itsConfiguration->GetValue("rrrs") == "true")
	{
		param parm("RRRS-KGM2", 1170);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	// Snow

	if (itsConfiguration->Exists("snr") && itsConfiguration->GetValue("snr") == "true")
	{
		param parm("SNR-KGM2", 264, 0, 1, 53);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("snrc") && itsConfiguration->GetValue("snrc") == "true")
	{
		param parm("SNRC-KGM2", 269, 0, 1, 55);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("snrl") && itsConfiguration->GetValue("snrl") == "true")
	{
		param parm("SNRL-KGM2", 268, 0, 1, 56);
		parm.Unit(kKgm2);

		parm.Aggregation(aggregation(kAccumulation, kUnknownTimeResolution, kHPMissingInt));

		params.push_back(parm);
	}

	// Radiation
	// These are RATES not SUMS

	if (itsConfiguration->Exists("glob") && itsConfiguration->GetValue("glob") == "true")
	{
		param parm("RADGLO-WM2", 317, 0, 4, 3);

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("lw") && itsConfiguration->GetValue("lw") == "true")
	{
		param parm("RADLW-WM2", 315, 0, 5, 192);

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("toplw") && itsConfiguration->GetValue("toplw") == "true")
	{
		// Same grib2 parameter definition as with RADLW-WM2, this is just on
		// another surface

		param parm("RTOPLW-WM2", 314, 0, 5, 192);

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("netlw") && itsConfiguration->GetValue("netlw") == "true")
	{
		param parm("RNETLW-WM2", 312, 0, 5, 5);

		params.push_back(parm);
	}

	if (itsConfiguration->Exists("sw") && itsConfiguration->GetValue("sw") == "true")
	{
		param parm("RADSW-WM2", 316, 0, 4, 192);

		params.push_back(parm);
	}

	if (params.empty())
	{
		itsLogger.Trace("No parameter definition given, defaulting to rr6h");

		param parm("RR-6-MM", 355, 0, 1, 8);

		parm.Aggregation(aggregation(kAccumulation, kHourResolution, 6));

		params.push_back(parm);
	}

	SetParams(params);

	// Default max workers (db connections) is 16.
	// Later in this plugin we launch sub threads so that the total number of
	// threads will be over 16. Connection pool is not working 100% correctly
	// because at that point it will hang.
	// To prevent this, make the pool larger.

	auto n = GET_PLUGIN(neons);
	n->PoolMaxWorkers(SUB_THREAD_COUNT * 12);  // 12 is the max thread count from compiled_plugin_base

	auto r = GET_PLUGIN(radon);
	r->PoolMaxWorkers(SUB_THREAD_COUNT * 12);

	Start();
}

/*
 * Calculate()
 *
 * This function does the actual calculation.
 */

void split_sum::Calculate(shared_ptr<info> myTargetInfo, unsigned short threadIndex)
{
	boost::thread_group threads;
	vector<info_t> infos;

	int subThreadIndex = 0;

	for (myTargetInfo->ResetParam(); myTargetInfo->NextParam(); ++subThreadIndex)
	{
		auto newInfo = make_shared<info>(*myTargetInfo);

		infos.push_back(newInfo);  // extend lifetime over this loop

		threads.add_thread(new boost::thread(&split_sum::DoParam, this, newInfo, myTargetInfo->Param().Name(),
		                                     to_string(threadIndex) + "_" + to_string(subThreadIndex)));

		if (subThreadIndex % SUB_THREAD_COUNT == 0)
		{
			threads.join_all();

			infos.clear();
		}
	}

	threads.join_all();
}

void split_sum::DoParam(info_t myTargetInfo, std::string myParamName, string subThreadIndex) const
{
	assert(myTargetInfo);
	assert(myTargetInfo->Param().Name() == myParamName);

	forecast_time forecastTime = myTargetInfo->Time();
	level forecastLevel = myTargetInfo->Level();

	auto myThreadedLogger = logger("splitSumSubThread#" + subThreadIndex);

	myThreadedLogger.Info("Calculating parameter " + myParamName + " time " +
	                      static_cast<string>(myTargetInfo->Time().ValidDateTime()) + " level " +
	                      static_cast<string>(forecastLevel));

	const bool isRadiationCalculation =
	    (myParamName == "RADGLO-WM2" || myParamName == "RADLW-WM2" || myParamName == "RTOPLW-WM2" ||
	     myParamName == "RNETLW-WM2" || myParamName == "RADSW-WM2");

	const bool isRateCalculation =
	    (isRadiationCalculation || myParamName == "RRR-KGM2" || myParamName == "RRRL-KGM2" ||
	     myParamName == "RRRC-KGM2" || myParamName == "SNR-KGM2" || myParamName == "SNRC-KGM2" ||
	     myParamName == "SNRL-KGM2" || myParamName == "GRR-MMH" || myParamName == "RRRS-KGM2" ||
	     myParamName == "RRC-KGM2");

	// Have to re-fetch infos each time since we might have to change element
	// from liquid to snow to radiation so we need also different source parameters

	info_t curSumInfo;
	info_t prevSumInfo;

	if (myTargetInfo->Time().Step() == 0)
	{
		// This is the first time step, calculation can not be done

		myThreadedLogger.Info("This is the first time step -- not calculating " + myParamName + " for step " +
		                      to_string(forecastTime.Step()));
		return;
	}

	/*
	 * Two modes of operation:
	 *
	 * 1) When calculating split_sum sums, always get the previous
	 * step value from the current step and get both values (current and
	 * previous). If either can't be found, skip time step. The time step
	 * is determined from the parameter (for example RR-3-H --> time step
	 * is three hours).
	 *
	 * 2) When calculating split_sum rate, get the first data that's
	 * earlier or same than current time step and the next data that's
	 * later or same than the current time step. Then calculate rate
	 * based on those values.
	 */

	int step = itsConfiguration->ForecastStep();

	if (isRateCalculation)
	{
		auto infos = GetSourceDataForRate(myTargetInfo, step);

		prevSumInfo = infos.first;
		curSumInfo = infos.second;
	}
	else
	{
		// Fetch data for previous step

		int paramStep = myTargetInfo->Param().Aggregation().TimeResolutionValue();

		if (myTargetInfo->Time().StepResolution() != kHourResolution)
		{
			assert(myTargetInfo->Time().StepResolution() == kMinuteResolution);

			paramStep *= 60;
		}

		// Skip early steps if necessary

		if (myTargetInfo->Time().Step() >= paramStep)
		{
			forecast_time prevTimeStep = myTargetInfo->Time();

			prevTimeStep.ValidDateTime().Adjust(prevTimeStep.StepResolution(), -paramStep);

			prevSumInfo = FetchSourceData(myTargetInfo, prevTimeStep);
		}

		// Data from current time step, but only if we have data for previous
		// step

		if (prevSumInfo)
		{
			curSumInfo = FetchSourceData(myTargetInfo, myTargetInfo->Time());
		}
	}

	if (!prevSumInfo || !curSumInfo)
	{
		// Data was not found

		myThreadedLogger.Warning("Data not found: not calculating " + myTargetInfo->Param().Name() + " for step " +
		                         to_string(myTargetInfo->Time().Step()));
		return;
	}

	myThreadedLogger.Trace("Previous data step is " + to_string(prevSumInfo->Time().Step()));
	myThreadedLogger.Trace("Current/next data step is " + to_string(curSumInfo->Time().Step()));

	double scaleFactor = 1.;

	// EC gives precipitation in meters, we are calculating millimeters

	if (curSumInfo->Param().Unit() == kM ||
	    ((myTargetInfo->Producer().Id() == 240 || myTargetInfo->Producer().Id() == 243) &&
	     !isRadiationCalculation))  // HIMAN-98
	{
		scaleFactor = 1000.;
	}

	string deviceType = "CPU";

	step = static_cast<int>(curSumInfo->Time().Step() - prevSumInfo->Time().Step());

	if (isRadiationCalculation)
	{
		/*
		 * Radiation unit is W/m^2 which is J/m^2/s, so we need to convert
		 * time to seconds.
		 *
		 * Step is always one hour or more, even in the case of Harmonie.
		 */

		if (myTargetInfo->Time().StepResolution() == kMinuteResolution)
		{
			step = 3600;
		}
		else if (myTargetInfo->Time().StepResolution() == kHourResolution)
		{
			step *= 3600;
		}
		else
		{
			myThreadedLogger.Error("Unknown time resolution: " +
			                       string(HPTimeResolutionToString.at(myTargetInfo->Time().StepResolution())));
			return;
		}
	}
	else if (myTargetInfo->Time().StepResolution() == kMinuteResolution || !isRateCalculation)
	{
		/*
		 * If calculating for Harmonie, use hour as base time unit, or disable
		 * it if sum is calculated.
		 */

		step = 1;
	}

	const double invstep = 1. / step;

	auto& result = VEC(myTargetInfo);

	for (auto&& tup : zip_range(result, VEC(curSumInfo), VEC(prevSumInfo)))
	{
		double& result = tup.get<0>();
		double currentSum = tup.get<1>();
		double previousSum = tup.get<2>();

		if (currentSum == kFloatMissing || previousSum == kFloatMissing)
		{
			continue;
		}

		result = (currentSum - previousSum) * invstep * scaleFactor;

		if (result < 0 && myParamName != "RTOPLW-WM2")
		{
			result = 0;
		}

		assert(isRadiationCalculation || result >= 0);
	}

	myThreadedLogger.Info("[" + deviceType + "] Parameter " + myParamName + " missing values: " +
	                      to_string(myTargetInfo->Data().MissingCount()) + "/" +
	                      to_string(myTargetInfo->Data().Size()));
}

pair<shared_ptr<himan::info>, shared_ptr<himan::info>> split_sum::GetSourceDataForRate(
    shared_ptr<const info> myTargetInfo, int step) const
{
	shared_ptr<info> prevInfo, curInfo;

	HPTimeResolution timeResolution = myTargetInfo->Time().StepResolution();

	// 1. Assuming we *know* what the step is, fetch previous and current
	// based on that step.

	if (step != kHPMissingInt)
	{
		if (myTargetInfo->Producer().Id() == 210)
		{
			step = 60;  // Forecast step is 15 (Harmonie), but it has been agreed
			            // with AKS that we'll use one hour since editor displays
			            // only hourly data.
		}

		forecast_time wantedTimeStep(myTargetInfo->Time());
		wantedTimeStep.ValidDateTime().Adjust(timeResolution, -step);

		if (wantedTimeStep.Step() >= 0)
		{
			prevInfo = FetchSourceData(myTargetInfo, wantedTimeStep);
		}
	}

	curInfo = FetchSourceData(myTargetInfo, myTargetInfo->Time());

	if (curInfo && prevInfo)
	{
		return make_pair(prevInfo, curInfo);
	}

	// 2. Data was not found on the requested steps. Now we have to scan the database
	// for data which is slow.

	int maxSteps = 6;  // by default look for 6 hours forward or backward
	step = 1;          // by default the difference between time steps is one (ie. one hour))

	if (myTargetInfo->Producer().Id() == 210)
	{
		step = 60;
	}
	else if (timeResolution != kHourResolution)
	{
		itsLogger.Fatal("Invalid time resolution value: " + HPTimeResolutionToString.at(timeResolution));
		abort();
	}

	itsLogger.Trace("Target time is " + static_cast<string>(myTargetInfo->Time().ValidDateTime()));

	if (!prevInfo)
	{
		itsLogger.Trace("Searching for previous data");

		// start going backwards in time and search for the
		// first data that exists

		forecast_time wantedTimeStep(myTargetInfo->Time());

		for (int i = 0; !prevInfo && i <= maxSteps * step; i++)
		{
			wantedTimeStep.ValidDateTime().Adjust(timeResolution, -step);

			if (wantedTimeStep.Step() < 0)
			{
				continue;
			}

			itsLogger.Trace("Trying time " + static_cast<string>(wantedTimeStep.ValidDateTime()));
			prevInfo = FetchSourceData(myTargetInfo, wantedTimeStep);

			if (prevInfo)
			{
				itsLogger.Debug("Found previous data");
			}
		}

		if (!prevInfo)
		{
			itsLogger.Error("Previous data not found");
			return make_pair(prevInfo, curInfo);
		}
	}

	if (!curInfo)
	{
		itsLogger.Trace("Searching for next data");

		// start going forwards in time and search for the
		// first data that exists

		forecast_time wantedTimeStep(myTargetInfo->Time());

		for (int i = 0; !curInfo && i <= maxSteps * step; i++)
		{
			wantedTimeStep.ValidDateTime().Adjust(timeResolution, step);

			itsLogger.Trace("Trying time " + static_cast<string>(wantedTimeStep.ValidDateTime()));
			curInfo = FetchSourceData(myTargetInfo, wantedTimeStep);

			if (curInfo)
			{
				itsLogger.Debug("Found current data");
			}
		}
	}

	return make_pair(prevInfo, curInfo);
}

shared_ptr<himan::info> split_sum::FetchSourceData(shared_ptr<const info> myTargetInfo,
                                                   const forecast_time& wantedTime) const
{
	level wantedLevel(kHeight, 0, "HEIGHT");

	auto params = sourceParameters[myTargetInfo->Param().Name()];

	if (params.empty())
	{
		itsLogger.Fatal("Source parameter for " + myTargetInfo->Param().Name() + " not found");
		abort();
	}

	if (myTargetInfo->Param().Name() == "RTOPLW-WM2")
	{
		wantedLevel = level(kTopOfAtmosphere, 0, "TOP");
	}

	shared_ptr<info> SumInfo = Fetch(wantedTime, wantedLevel, params, myTargetInfo->ForecastType());

	// If model does not provide data for timestep 0, emulate it
	// by providing a zero-grid

	if (!SumInfo && wantedTime.Step() == 0)
	{
		SumInfo = make_shared<info>(*myTargetInfo);
		vector<forecast_time> times = {wantedTime};
		vector<level> levels = {wantedLevel};
		vector<param> params = {sourceParameters[myTargetInfo->Param().Name()][0]};

		SumInfo->Params(params);
		SumInfo->Levels(levels);
		SumInfo->Times(times);

		SumInfo->Create(myTargetInfo->Grid());
		SumInfo->Data().Fill(0);
	}

	return SumInfo;
}

void split_sum::WriteToFile(const info& targetInfo, write_options writeOptions)
{
	auto aWriter = GET_PLUGIN(writer);

	writeOptions.write_empty_grid = false;

	aWriter->WriteOptions(writeOptions);

	// writing might modify iterator positions --> create a copy

	auto tempInfo = targetInfo;

	tempInfo.ResetParam();

	while (tempInfo.NextParam())
	{
		if (itsConfiguration->FileWriteOption() == kDatabase || itsConfiguration->FileWriteOption() == kMultipleFiles)
		{
			aWriter->ToFile(tempInfo, itsConfiguration);
		}
		else
		{
			lock_guard<mutex> lock(mySingleFileWriteMutex);

			aWriter->ToFile(tempInfo, itsConfiguration, itsConfiguration->ConfigurationFile());
		}
	}

	if (itsConfiguration->UseDynamicMemoryAllocation())
	{
		DeallocateMemory(targetInfo);
	}
}


/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

/**********************************************************************************************/
/*  This program is part of the Barcelona OpenMP Tasks Suite */
/*  Copyright (C) 2009 Barcelona Supercomputing Center - Centro Nacional de
 * Supercomputacion  */
/*  Copyright (C) 2009 Universitat Politecnica de Catalunya */
/*                                                                                */
/*  This program is free software; you can redistribute it and/or modify */
/*  it under the terms of the GNU General Public License as published by */
/*  the Free Software Foundation; either version 2 of the License, or */
/*  (at your option) any later version. */
/*                                                                                */
/*  This program is distributed in the hope that it will be useful, */
/*  but WITHOUT ANY WARRANTY; without even the implied warranty of */
/*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the */
/*  GNU General Public License for more details. */
/*                                                                                */
/*  You should have received a copy of the GNU General Public License */
/*  along with this program; if not, write to the Free Software */
/*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
 * USA            */
/**********************************************************************************************/

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <libgen.h>
#include "bots.h"
#include "sparselu.h"

#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/LargeArray.h"
#include "galois/graphs/FileGraph.h"
#include "galois/runtime/KDGtwoPhase.h"
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

extern char bots_arg_file[256];

/***********************************************************************
 * checkmat:
 **********************************************************************/
int checkmat(float* M, float* N) {
  int i, j;
  float r_err;

  for (i = 0; i < bots_arg_size_1; i++) {
    for (j = 0; j < bots_arg_size_1; j++) {
      r_err = M[i * bots_arg_size_1 + j] - N[i * bots_arg_size_1 + j];
      if (r_err == 0.0)
        continue;

      if (r_err < 0.0)
        r_err = -r_err;

      if (M[i * bots_arg_size_1 + j] == 0) {
        bots_message("Checking failure: A[%d][%d]=%f  B[%d][%d]=%f; \n", i, j,
                     M[i * bots_arg_size_1 + j], i, j,
                     N[i * bots_arg_size_1 + j]);
        return FALSE;
      }
      r_err = r_err / M[i * bots_arg_size_1 + j];
      if (r_err > EPSILON) {
        bots_message(
            "Checking failure: A[%d][%d]=%f  B[%d][%d]=%f; Relative Error=%f\n",
            i, j, M[i * bots_arg_size_1 + j], i, j, N[i * bots_arg_size_1 + j],
            r_err);
        return FALSE;
      }
    }
  }
  return TRUE;
}

/***********************************************************************
 * genmat:
 **********************************************************************/
static void synthetic_genmat(float* M[]) {
  int null_entry, init_val, i, j, ii, jj;
  float* p;
  int a = 0, b = 0;

  init_val = 1325;

  /* generating the structure */
  for (ii = 0; ii < bots_arg_size; ii++) {
    for (jj = 0; jj < bots_arg_size; jj++) {
      /* computing null entries */
      null_entry = FALSE;
      if ((ii < jj) && (ii % 3 != 0))
        null_entry = TRUE;
      if ((ii > jj) && (jj % 3 != 0))
        null_entry = TRUE;
      if (ii % 2 == 1)
        null_entry = TRUE;
      if (jj % 2 == 1)
        null_entry = TRUE;
      if (ii == jj)
        null_entry = FALSE;
      if (ii == jj - 1)
        null_entry = FALSE;
      if (ii - 1 == jj)
        null_entry = FALSE;
      /* allocating matrix */
      if (null_entry == FALSE) {
        a++;
        M[ii * bots_arg_size + jj] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        if (M[ii * bots_arg_size + jj] == NULL) {
          bots_message("Error: Out of memory\n");
          exit(101);
        }
        /* initializing matrix */
        p = M[ii * bots_arg_size + jj];
        for (i = 0; i < bots_arg_size_1; i++) {
          for (j = 0; j < bots_arg_size_1; j++) {
            init_val = (3125 * init_val) % 65536;
            (*p)     = (float)((init_val - 32768.0) / 16384.0);
            p++;
          }
        }
      } else {
        b++;
        M[ii * bots_arg_size + jj] = NULL;
      }
    }
  }
  bots_debug("allo = %d, no = %d, total = %d, factor = %f\n", a, b, a + b,
             (float)((float)a / (float)(a + b)));
}

// From symmetric matrix
static void structure_from_file_genmat(float* M[]) {
  int ii, jj;
  int a = 0, b;
  galois::graphs::FileGraph g;
  int num_blocks;
  unsigned max_id;

  g.fromFile(bots_arg_file);
  memset(M, 0, bots_arg_size * bots_arg_size * sizeof(*M));

  num_blocks = (g.size() + bots_arg_size_1 - 1) / bots_arg_size_1;
  max_id     = bots_arg_size_1 * bots_arg_size;

  printf("full size: %d\n", num_blocks);

  /* generating the structure */
  for (auto ii : g) {
    if (ii >= max_id)
      break;
    int bii = ii / bots_arg_size_1;
    for (auto edge : g.out_edges(ii)) {
      /* computing null entries */
      int jj = g.getEdgeDst(edge);
      if (jj >= max_id)
        continue;
      int bjj = jj / bots_arg_size_1;
      if (M[bii * bots_arg_size + bjj] == NULL) {
        a++;
        M[bii * bots_arg_size + bjj] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        memset(M[bii * bots_arg_size + bjj], 0,
               bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      }
      if (M[bii * bots_arg_size + bjj] == NULL) {
        bots_message("Error: Out of memory\n");
        exit(101);
      }
      if (M[bjj * bots_arg_size + bii] == NULL) {
        a++;
        M[bjj * bots_arg_size + bii] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        memset(M[bjj * bots_arg_size + bii], 0,
               bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      }
      if (M[bjj * bots_arg_size + bii] == NULL) {
        bots_message("Error: Out of memory\n");
        exit(101);
      }
      M[bii * bots_arg_size + bjj]
       [(ii % bots_arg_size_1) * bots_arg_size_1 + (jj % bots_arg_size_1)] =
           g.getEdgeData<float>(edge);
      M[bjj * bots_arg_size + bii]
       [(jj % bots_arg_size_1) * bots_arg_size_1 + (ii % bots_arg_size_1)] =
           g.getEdgeData<float>(edge);
    }
  }
  // Add identity diagonal as necessary
  for (ii = 0; ii < bots_arg_size; ++ii) {
    if (M[ii * bots_arg_size + ii] == NULL) {
      a++;
      M[ii * bots_arg_size + ii] =
          (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      memset(M[ii * bots_arg_size + ii], 0,
             bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
    }
    for (jj = 0; jj < bots_arg_size_1; ++jj) {
      if (M[ii * bots_arg_size + ii][jj * bots_arg_size_1 + jj] == 0.0)
        M[ii * bots_arg_size + ii][jj * bots_arg_size_1 + jj] = 1.0;
    }
  }
  b = num_blocks * num_blocks - a;
  bots_debug("allo = %d, no = %d, total = %d, factor = %f\n", a, b, a + b,
             (float)((float)a / (float)(a + b)));
}

void genmat(float* M[]) {
  if (strlen(bots_arg_file) == 0)
    synthetic_genmat(M);
  else
    structure_from_file_genmat(M);
}

/***********************************************************************
 * print_structure:
 **********************************************************************/
void print_structure(char* name, float* M[]) {
  int ii, jj;
  bots_message("Structure for matrix %s @ 0x%p\n", name, M);
  for (ii = 0; ii < bots_arg_size; ii++) {
    for (jj = 0; jj < bots_arg_size; jj++) {
      if (M[ii * bots_arg_size + jj] != NULL) {
        bots_message("x");
      } else
        bots_message(" ");
    }
    bots_message("\n");
  }
  bots_message("\n");
}
/***********************************************************************
 * allocate_clean_block:
 **********************************************************************/
float* allocate_clean_block() {
  int i, j;
  float *p, *q;

  p = (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
  q = p;
  if (p != NULL) {
    for (i = 0; i < bots_arg_size_1; i++)
      for (j = 0; j < bots_arg_size_1; j++) {
        (*p) = 0.0;
        p++;
      }

  } else {
    bots_message("Error: Out of memory\n");
    exit(101);
  }
  return (q);
}

/***********************************************************************
 * lu0:
 **********************************************************************/
void lu0(float* diag) {
  int i, j, k;

  for (k = 0; k < bots_arg_size_1; k++)
    for (i = k + 1; i < bots_arg_size_1; i++) {
      diag[i * bots_arg_size_1 + k] =
          diag[i * bots_arg_size_1 + k] / diag[k * bots_arg_size_1 + k];
      for (j = k + 1; j < bots_arg_size_1; j++)
        diag[i * bots_arg_size_1 + j] =
            diag[i * bots_arg_size_1 + j] -
            diag[i * bots_arg_size_1 + k] * diag[k * bots_arg_size_1 + j];
    }
}

/***********************************************************************
 * bdiv:
 **********************************************************************/
void bdiv(float* diag, float* row) {
  int i, j, k;
  for (i = 0; i < bots_arg_size_1; i++)
    for (k = 0; k < bots_arg_size_1; k++) {
      row[i * bots_arg_size_1 + k] =
          row[i * bots_arg_size_1 + k] / diag[k * bots_arg_size_1 + k];
      for (j = k + 1; j < bots_arg_size_1; j++)
        row[i * bots_arg_size_1 + j] =
            row[i * bots_arg_size_1 + j] -
            row[i * bots_arg_size_1 + k] * diag[k * bots_arg_size_1 + j];
    }
}
/***********************************************************************
 * bmod:
 **********************************************************************/
void bmod(float* row, float* col, float* inner) {
  int i, j, k;
  for (i = 0; i < bots_arg_size_1; i++)
    for (j = 0; j < bots_arg_size_1; j++)
      for (k = 0; k < bots_arg_size_1; k++)
        inner[i * bots_arg_size_1 + j] =
            inner[i * bots_arg_size_1 + j] -
            row[i * bots_arg_size_1 + k] * col[k * bots_arg_size_1 + j];
}
/***********************************************************************
 * fwd:
 **********************************************************************/
void fwd(float* diag, float* col) {
  int i, j, k;
  for (j = 0; j < bots_arg_size_1; j++)
    for (k = 0; k < bots_arg_size_1; k++)
      for (i = k + 1; i < bots_arg_size_1; i++)
        col[i * bots_arg_size_1 + j] =
            col[i * bots_arg_size_1 + j] -
            diag[i * bots_arg_size_1 + k] * col[k * bots_arg_size_1 + j];
}

static galois::LargeArray<galois::runtime::Lockable> locks;

void sparselu_init(float*** pBENCH, char* pass) {
  galois::setActiveThreads(bots_arg_size_2);
  galois::substrate::getThreadPool().burnPower(bots_arg_size_2);
  galois::preAlloc(5 * bots_arg_size_2);
  galois::reportPageAlloc("MeminfoPre");
  *pBENCH = (float**)malloc(bots_arg_size * bots_arg_size * sizeof(float*));
  genmat(*pBENCH);
  print_structure(pass, *pBENCH);
  // locks.allocateInterleaved(bots_arg_size*bots_arg_size); XXX
  locks.allocateLocal(bots_arg_size * bots_arg_size);
  locks.construct();
}

void sparselu_seq_call(float** BENCH) {
  int ii, jj, kk;

  for (kk = 0; kk < bots_arg_size; kk++) {
    lu0(BENCH[kk * bots_arg_size + kk]);
    for (jj = kk + 1; jj < bots_arg_size; jj++)
      if (BENCH[kk * bots_arg_size + jj] != NULL) {
        fwd(BENCH[kk * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj]);
      }
    for (ii = kk + 1; ii < bots_arg_size; ii++)
      if (BENCH[ii * bots_arg_size + kk] != NULL) {
        bdiv(BENCH[kk * bots_arg_size + kk], BENCH[ii * bots_arg_size + kk]);
      }
    for (ii = kk + 1; ii < bots_arg_size; ii++)
      if (BENCH[ii * bots_arg_size + kk] != NULL)
        for (jj = kk + 1; jj < bots_arg_size; jj++)
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            if (BENCH[ii * bots_arg_size + jj] == NULL)
              BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
            bmod(BENCH[ii * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj],
                 BENCH[ii * bots_arg_size + jj]);
          }
  }
}

struct FwdBdiv {
  typedef int tt_does_not_need_aborts;

  float** BENCH;
  int kk;

  enum { SEEK, FWD, BDIV };

  struct Task {
    int type;
    int arg;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int arg) const { return {SEEK, arg}; }
  };

  void doSeek(const Task& t, galois::UserContext<Task>& ctx) {
    int jj = t.arg;
    if (BENCH[kk * bots_arg_size + jj] != NULL)
      ctx.push(Task{FWD, jj});
    int ii = t.arg;
    if (BENCH[ii * bots_arg_size + kk] != NULL)
      ctx.push(Task{BDIV, ii});
  }

  void doFwd(const Task& t, galois::UserContext<Task>& ctx) {
    int jj = t.arg;
    fwd(BENCH[kk * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj]);
  }

  void doBdiv(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg;
    bdiv(BENCH[kk * bots_arg_size + kk], BENCH[ii * bots_arg_size + kk]);
  }

  void operator()(const Task& t, galois::UserContext<Task>& ctx) {
    switch (t.type) {
    case SEEK:
      return doSeek(t, ctx);
    case FWD:
      return doFwd(t, ctx);
    case BDIV:
      return doBdiv(t, ctx);
    default:
      abort();
    }
  }
};

struct Bmod {
  typedef int tt_does_not_need_aborts;

  float** BENCH;
  int kk;

  enum { SEEK, BMOD };

  struct Task {
    int type;
    int arg1;
    int arg2;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int arg) const { return {SEEK, arg, arg}; }
  };

  void doSeek(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg1;
    if (BENCH[ii * bots_arg_size + kk] != NULL)
      for (int jj = kk + 1; jj < bots_arg_size; jj++)
        if (BENCH[kk * bots_arg_size + jj] != NULL)
          ctx.push(Task{BMOD, ii, jj});
  }

  void doBmod(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg1;
    int jj = t.arg2;
    if (BENCH[ii * bots_arg_size + jj] == NULL)
      BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
    bmod(BENCH[ii * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj],
         BENCH[ii * bots_arg_size + jj]);
  }

  void operator()(const Task& t, galois::UserContext<Task>& ctx) {
    switch (t.type) {
    case SEEK:
      return doSeek(t, ctx);
    case BMOD:
      return doBmod(t, ctx);
    default:
      abort();
    }
  }
};

struct FwdBdivBmod {
  enum { LU0 = 0, FWD = 1, BDIV = 2, BMOD = 3 };

  struct Task {
    int type;
    int kk;
    int arg0;
    int arg1;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int kk) const { return {LU0, kk, 0, 0}; }
  };

  struct Comparator {
    bool operator()(const Task& t1, const Task& t2) const {
      // Lexicographic (kk, type, arg0, arg1)
      if (t1.kk == t2.kk)
        if (t1.type == t2.type)
          if (t1.arg0 == t2.arg0)
            return t1.arg1 < t2.arg1;
          else
            return t1.arg0 < t2.arg0;
        else
          return t1.type < t2.type;
      else
        return t1.kk < t2.kk;
    }
  };

  struct NeighborhoodVisitor {
    static const unsigned CHUNK_SIZE = 4;
    float** BENCH;

    typedef int tt_does_not_need_push;

    void acquire(int ii, int jj) {
      galois::runtime::acquire(&locks[ii * bots_arg_size + jj],
                               galois::MethodFlag::WRITE);
    }

    void operator()(const Task& t, galois::UserContext<Task>&) {
      int ii, jj, kk = t.kk;
      switch (t.type) {
      case LU0:
        acquire(kk, kk);
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            acquire(kk, jj);
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, kk);
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, kk);
            for (jj = kk + 1; jj < bots_arg_size; jj++) {
              if (BENCH[kk * bots_arg_size + jj] != NULL) {
                acquire(kk, jj);
                acquire(ii, jj);
              }
            }
          }
        }
        return;
      case FWD:
        acquire(kk, t.arg0);
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, t.arg0);
          }
        }
        return;
      case BDIV:
        acquire(t.arg0, kk);
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            acquire(t.arg0, jj);
          }
        }
        return;
      case BMOD:
        acquire(t.arg0, t.arg1);
        return;
      }
    }
  };

  struct Process {
    static const int CHUNK_SIZE = 1;

    typedef int tt_does_not_need_aborts;

    float** BENCH;

    void operator()(const Task& t, galois::UserContext<Task>& ctx) {
      int ii, jj, kk = t.kk;

      switch (t.type) {
      case LU0:
        lu0(BENCH[kk * bots_arg_size + kk]);
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            ctx.push(Task{BDIV, kk, ii, 0});
          }
        }
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            ctx.push(Task{FWD, kk, jj, 0});
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            for (jj = kk + 1; jj < bots_arg_size; jj++) {
              if (BENCH[kk * bots_arg_size + jj] != NULL) {
                if (BENCH[ii * bots_arg_size + jj] == NULL)
                  BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
                ctx.push(Task{BMOD, kk, ii, jj});
              }
            }
          }
        }
        return;
      case FWD:
        return fwd(BENCH[kk * bots_arg_size + kk],
                   BENCH[kk * bots_arg_size + t.arg0]);
      case BDIV:
        return bdiv(BENCH[kk * bots_arg_size + kk],
                    BENCH[t.arg0 * bots_arg_size + kk]);
      case BMOD:
        return bmod(BENCH[t.arg0 * bots_arg_size + kk],
                    BENCH[kk * bots_arg_size + t.arg1],
                    BENCH[t.arg0 * bots_arg_size + t.arg1]);
      }
    }
  };
};

static void bs_algo(float** BENCH) {
  int ii, jj, kk;

  namespace ww = galois::worklists;
  typedef ww::StableIterator<>::with_container<ww::PerSocketChunkLIFO<1>>::type
      WL;

  for (kk = 0; kk < bots_arg_size; kk++) {
    lu0(BENCH[kk * bots_arg_size + kk]);

    galois::for_each(
        boost::transform_iterator<FwdBdiv::Initializer,
                                  boost::counting_iterator<int>>(kk + 1),
        boost::transform_iterator<FwdBdiv::Initializer,
                                  boost::counting_iterator<int>>(bots_arg_size),
        FwdBdiv{BENCH, kk}, galois::wl<WL>());

    galois::for_each(
        boost::transform_iterator<Bmod::Initializer,
                                  boost::counting_iterator<int>>(kk + 1),
        boost::transform_iterator<Bmod::Initializer,
                                  boost::counting_iterator<int>>(bots_arg_size),
        Bmod{BENCH, kk}, galois::wl<WL>());
  }
}

static void ikdg_algo(float** BENCH) {
  int kk;
  typedef boost::transform_iterator<FwdBdivBmod::Initializer,
                                    boost::counting_iterator<int>>
      TI;

  galois::setDoAllImpl(galois::DOALL_COUPLED);
  if (galois::getDoAllImpl() != galois::DOALL_COUPLED) {
    std::abort();
  }

  galois::runtime::for_each_ordered_ikdg(
      galois::runtime::makeStandardRange(TI(0), TI(bots_arg_size)),
      FwdBdivBmod::Comparator{}, FwdBdivBmod::NeighborhoodVisitor{BENCH},
      FwdBdivBmod::Process{BENCH},
      std::make_tuple(galois::loopname("sparselu-ikdg")));
}

void sparselu_par_call(float** BENCH) {
  galois::StatManager manager;
  galois::StatTimer T;

  T.start();
  bots_message(
      "Computing SparseLU Factorization (%dx%d matrix with %dx%d blocks) ",
      bots_arg_size, bots_arg_size, bots_arg_size_1, bots_arg_size_1);
  if (bots_app_cutoff_value_1 == 0)
    bs_algo(BENCH);
  else
    ikdg_algo(BENCH);
  bots_message(" completed!\n");
  galois::reportPageAlloc("MeminfoPost");
  T.stop();
}

void sparselu_fini(float** BENCH, char* pass) {
  galois::substrate::getThreadPool().beKind();
  print_structure(pass, BENCH);
  locks.destroy();
  locks.deallocate();
}

int sparselu_check(float** SEQ, float** BENCH) {
  int ii, jj, ok = 1;

  for (ii = 0; ((ii < bots_arg_size) && ok); ii++) {
    for (jj = 0; ((jj < bots_arg_size) && ok); jj++) {
      if (SEQ[ii * bots_arg_size + jj] == NULL &&
          BENCH[ii * bots_arg_size + jj] != NULL)
        ok = FALSE;
      if (SEQ[ii * bots_arg_size + jj] != NULL &&
          BENCH[ii * bots_arg_size + jj] == NULL)
        ok = FALSE;
      if (SEQ[ii * bots_arg_size + jj] != NULL &&
          BENCH[ii * bots_arg_size + jj] != NULL)
        ok = checkmat(SEQ[ii * bots_arg_size + jj],
                      BENCH[ii * bots_arg_size + jj]);
    }
  }
  if (ok)
    return BOTS_RESULT_SUCCESSFUL;
  else
    return BOTS_RESULT_UNSUCCESSFUL;
}


/*=============================================================================
    Copyright (c) 2006-2007 Tobias Schwinger

    Use modification and distribution are 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).
==============================================================================*/

#include <boost/fusion/functional/adapter/unfused_rvalue_args.hpp>
#include <boost/detail/lightweight_test.hpp>

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

#include <boost/mpl/identity.hpp>

#include <boost/utility/result_of.hpp>

#include <boost/fusion/algorithm/iteration/fold.hpp>

namespace fusion = boost::fusion;
namespace mpl = boost::mpl;

using boost::noncopyable;
typedef mpl::true_ no_nullary_call;

template <class Base = boost::blank>
struct test_func
    : Base
{
    template <typename Sig>
    struct result;

    template <class Self, class Seq>
    struct result< Self(Seq) >
        : mpl::identity<long>
    { };

    template <typename Seq>
    long operator()(Seq const & seq) const
    {
        long state = 0;
        return fusion::fold(seq, state, fold_op());
    }

    template < typename Seq >
    long operator()(Seq const & seq)
    {
        long state = 100;
        return fusion::fold(seq, state, fold_op());
    }

  private:

    struct fold_op
    {
        typedef long result_type;

        template <typename T>
        long operator()(T const & elem, long value) const
        {
          return value + sizeof(T) * elem;
        }
    };
};

void result_type_tests()
{
    using boost::is_same;

    typedef fusion::unfused_rvalue_args< test_func<> > t;
    BOOST_TEST(( is_same< boost::result_of< t () >::type, long >::value ));
    BOOST_TEST(( is_same< boost::result_of< t (int) >::type, long >::value ));
}

int main()
{
    result_type_tests();

    test_func<noncopyable> f;
    fusion::unfused_rvalue_args< test_func<> > unfused_func;
    fusion::unfused_rvalue_args< test_func<noncopyable> & > unfused_func_ref(f);
    fusion::unfused_rvalue_args< test_func<> const > unfused_func_c;
    fusion::unfused_rvalue_args< test_func<> > const unfused_func_c2;
    fusion::unfused_rvalue_args< test_func<noncopyable> const & > unfused_func_c_ref(f);

    BOOST_TEST(unfused_func() == 100);
    BOOST_TEST(unfused_func_ref() == 100);
    BOOST_TEST(unfused_func_c() == 0);
    BOOST_TEST(unfused_func_c2() == 0);
    BOOST_TEST(unfused_func_c_ref() == 0);

    static const long expected = 1*sizeof(int) + 2*sizeof(long) + 7*sizeof(char);
    BOOST_TEST(unfused_func(1,2l,'\007') == 100 + expected);
    BOOST_TEST(unfused_func_ref(1,2l,'\007') == 100 + expected);
    BOOST_TEST(unfused_func_c(1,2l,'\007') == 0 + expected);
    BOOST_TEST(unfused_func_c2(1,2l,'\007') == 0 + expected);
    BOOST_TEST(unfused_func_c_ref(1,2l,'\007') == 0 + expected);

    return boost::report_errors();
}