#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 "pidcontroller.h"
#include<rosgraph_msgs/Clock.h>

#include "std_msgs/String.h"
#include<gazebo_msgs/ApplyBodyWrench.h>

#include<gazebo_msgs/ApplyBodyWrenchRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include<gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/orientationEstimate.h"

using namespace  std;
using namespace  Eigen;

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

//ros::Subscriber quaternions;
double teta_L=0;
double phi_L=0;

double teta_R=0;
double phi_R=0;

double teta_center=0;
double phi_center=0;

double quaternion2ankle_pitch(double q0,double q1,double q2,double q3){
    double R11,R32,R33,R31,theta;
    //    R11=q0*q0+q1*q1-q2*q2-q3*q3;
    R31=2*(q1*q3-q0*q2);
    //    theta=atan2(-R31,R11);
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    theta=atan2(-R31,sqrt(R32*R32+R33*R33));

    return theta;
}
double quaternion2ankle_roll(double q0,double q1,double q2,double q3){
    double R23,R22,phi,R33,R32;
    //R23=2*(q2*q3-q0*q1);
    //R22=q0*q0-q1*q1+q2*q2-q3*q3;
    //phi=atan2(-R23,R22);
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    phi=atan2(R32,R33);
    return phi;
}

PIDController teta_pid_L;
PIDController phi_pid_L;
PIDController teta_pid_R;
PIDController phi_pid_R;

PIDController teta_pid_center;
PIDController phi_pid_center;

double p_teta,i_teta,d_teta,p_phi,i_phi,d_phi,dt,rate;
double p_teta_center,i_teta_center,d_teta_center,p_phi_center,i_phi_center,d_phi_center;

double teta_motor_L=0;
double teta_motor_R=0;
double phi_motor_L=0;
double phi_motor_R=0;
double teta_motor_center=0;
double phi_motor_center=0;
double timestep=.01;
double time_=0;

geometry_msgs::Pose GetLinkPosition(QString linkName ,const gazebo_msgs::LinkStates::ConstPtr linkStates)
{
    geometry_msgs::Pose result;
    const std::vector<std::string> &names = linkStates->name;
    const std::vector<geometry_msgs::Pose> positions = linkStates->pose;

    for(int i=0;i<names.size();i++)
    {

        if(QString::fromStdString( names[i]).contains(linkName))
        {

            return  positions[i];

        }
    }

    return result;
}
void chatterCallback(const gazebo_msgs::LinkStates::ConstPtr& msg)
{
    const std::vector<std::string> &names = msg->name;
    //const std::vector<geometry_msgs::Pose> psitions = msg->pose;
    //  double distance=0;

    geometry_msgs::Pose parentPosition_L=GetLinkPosition("robot::LLeg_Foot_Link",msg);
//    qDebug()<<"we are here:"<< parentPosition.orientation.x<<parentPosition.orientation.y<<parentPosition.orientation.z<<parentPosition.orientation.w;
   teta_L= quaternion2ankle_pitch( parentPosition_L.orientation.w,parentPosition_L.orientation.x,parentPosition_L.orientation.y,parentPosition_L.orientation.z);
   phi_L=quaternion2ankle_roll( parentPosition_L.orientation.w,parentPosition_L.orientation.x,parentPosition_L.orientation.y,parentPosition_L.orientation.z);

   geometry_msgs::Pose parentPosition_R=GetLinkPosition("robot::LLeg_Foot_Link",msg);
//    qDebug()<<"we are here:"<< parentPosition_R.orientation.x<<parentPosition_R.orientation.y<<parentPosition_R.orientation.z<<parentPosition_R.orientation.w;
  teta_R= quaternion2ankle_pitch( parentPosition_R.orientation.w,parentPosition_R.orientation.x,parentPosition_R.orientation.y,parentPosition_R.orientation.z);
  phi_R=quaternion2ankle_roll( parentPosition_R.orientation.w,parentPosition_R.orientation.x,parentPosition_R.orientation.y,parentPosition_R.orientation.z);

  geometry_msgs::Pose parentPosition_center=GetLinkPosition("robot::Pelvis",msg);
//    qDebug()<<"we are here:"<< parentPosition_center.orientation.x<<parentPosition_center.orientation.y<<parentPosition_center.orientation.z<<parentPosition_center.orientation.w;
 teta_center= quaternion2ankle_pitch( parentPosition_center.orientation.w,parentPosition_center.orientation.x,parentPosition_center.orientation.y,parentPosition_center.orientation.z);
 phi_center=quaternion2ankle_roll( parentPosition_center.orientation.w,parentPosition_center.orientation.x,parentPosition_center.orientation.y,parentPosition_center.orientation.z);

   qDebug()<<"teta_L:"<< teta_L*180/M_PI<<", Phi_L="<<phi_L*180/M_PI<<"teta_R:"<< teta_R*180/M_PI<<", Phi_R="<<phi_R*180/M_PI<<"teta_center:"<< teta_center*180/M_PI<<", Phi_center="<<phi_center*180/M_PI;
    //qDebug()<<QString::fromStdString( names[9])<<psitions[9].position.x<<psitions[9].position.y<<psitions[9].position.z;
    //ROS_INFO("I heard");

}

//teta_pid.Init(0,0,0,0,0,0);
//teta_pid.Init(dt,1,-1,p_teta,i_teta,d_teta);
//phi_pid.Init(dt,1,-1,p_teta,i_teta,d_teta);

//void  SendGazebo(double t){

//    std_msgs::Float64 data;

//    //ros::Time time = ros::Time::now();

//    data.data=0;
//    pub1.publish(data);
//    pub2.publish(data);
//    pub3.publish(data);
//    pub4.publish(data);
//    pub5.publish(data);
//    pub6.publish(data);
//    pub7.publish(data);
//    pub8.publish(data);

//    pub13.publish(data);
//    pub14.publish(data);
//    pub15.publish(data);
//    pub16.publish(data);
//    pub17.publish(data);
//    pub18.publish(data);
//    pub19.publish(data);
//    pub20.publish(data);
//    pub21.publish(data);
//    pub22.publish(data);
//    pub23.publish(data);
//    pub24.publish(data);
//    pub25.publish(data);
//    pub26.publish(data);
//    pub27.publish(data);
//    pub28.publish(data);

//    data.data=min(0.8*sin(t)*sin(t),1.0);
//    pub10.publish(data);

//    data.data=-data.data/2;
//    pub9.publish(data);

//    double maximum=.5;
//    teta_motor=teta_motor+teta_pid.Calculate(0,teta);
//    data.data=teta_motor;
//    if (data.data<-maximum){data.data=-maximum;}
//    if (data.data>maximum){data.data=maximum;}
//    pub11.publish(data);
//    phi_motor=phi_motor+phi_pid.Calculate(0,phi);
//    data.data=phi_motor;
//    if (data.data<-maximum){data.data=-maximum;}
//    if (data.data>maximum){data.data=maximum;}
//    pub12.publish(data);

//    //    double maximum=.5;
//    //        data.data=teta;
//    //        if (data.data<-maximum){data.data=-maximum;}
//    //        if (data.data>maximum){data.data=maximum;}
//    //        pub11.publish(data);
//    //        data.data=phi;
//    //        if (data.data<-maximum){data.data=-maximum;}
//    //        if (data.data>maximum){data.data=maximum;}
//    //        pub12.publish(daos::spinOnce();ta);
//}

void RecievIMULeft(const sensor_msgs::Imu & msg)
{

    teta_L= msg.orientation.y;
    phi_L=msg.orientation.x;
//ROS_INFO("Theta_L:[%f] Phi_L:[%f]",  teta_L*180/3.141592,phi_L*180/3.141592);

}

void RecievTime(const rosgraph_msgs::Clock & msg)
{
    time_ =double(msg.clock.nsec)*1e-9 + double(msg.clock.sec);
}

void RecievIMURight(const sensor_msgs::Imu & msg)
{
    teta_R= msg.orientation.y;
    phi_R=msg.orientation.x;
//ROS_INFO("Theta_R:[%f] Phi_R:[%f]",  teta_R*180/3.141592,phi_R*180/3.141592);
}

void RecievIMUCenter(const xsens_msgs::orientationEstimate & msg)
{
    teta_center= msg.pitch;
   phi_center=msg.roll;
    //ROS_INFO("Theta_R:[%f] Phi_R:[%f]",  teta_R*180/3.141592,phi_R*180/3.141592);
}

int main(int argc, char **argv)
{
    //check _timesteps

    std_srvs::Empty emptyCall;
    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "footControllerNode");

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

    ros::Subscriber  IMULeft = nh.subscribe("/yei2000154", 1, RecievIMULeft);
    ros::Subscriber  IMURight = nh.subscribe("/yei200015B", 1, RecievIMURight);
    ros::Subscriber  IMULCenter = nh.subscribe("/mti/filter/orientation", 1, RecievIMUCenter);

    ros::Subscriber  time_sub = nh.subscribe("/clock", 1, RecievTime);
    ros::Subscriber sub = nh.subscribe("/gazebo/link_states", 1, &chatterCallback);
    ros::ServiceClient tareLeft= nh.serviceClient<std_srvs::Empty>("/Tareyei2000154");
    ros::ServiceClient tareRight= nh.serviceClient<std_srvs::Empty>("/Tareyei200015B");
    //ros::Subscriber  IMURight = nh.subscribe("/yei200015B", 100, RecievIMURight);
    ros::Subscriber  IMUCenter = nh.subscribe("/mti/sensor/imu", 100, RecievIMUCenter);

    //double theta,phi;
    //theta=quaternion2ankle_pitch(quaternions(0),quaternions(1),quaternions(2),quaternions(3));
    //phi=quaternion2ankle_roll(quaternions(0),quaternions(1),quaternions(2),quaternions(3));

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

    rate=100.0;
    ros::Rate loop_rate(rate);
    dt=1/rate;
    p_teta=0.05;
    p_phi=0.05;
    i_teta=0;i_phi=0;
    d_teta=0;d_phi=0;
    teta_pid_L.Init(dt,.15,-.15,p_teta,i_teta,d_teta);
    phi_pid_L.Init(dt,.15,-.15,p_phi,i_phi,d_phi);
    teta_pid_R.Init(dt,.15,-.15,p_teta,i_teta,d_teta);
    phi_pid_R.Init(dt,.15,-.15,p_phi,i_phi,d_phi);

    p_teta_center=0.00009;
    p_phi_center=0.00009;
    i_teta_center=0;i_phi_center=0;
    d_teta_center=0;d_phi_center=0;
    teta_pid_center.Init(dt,0.15,-.15,p_teta_center,i_teta_center,d_teta_center);
    phi_pid_center.Init(dt,.15,-.15,p_phi_center,i_phi_center,d_phi_center);

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

    //sensor imu----------------------------------------------sensor imu
    //sensor imu----------------------------------------------sensor imu
    //  timer.start();
    //  ros::Subscriber sub = nh.subscribe("/mti/sensor/imu", 1000, chatterCallback);
    //sensor imu----------------------------------------------sensor imu
    //sensor imu----------------------------------------------sensor imu

    //SendGazebo(0);
    std_msgs::Float64 data0;

    data0.data=0;
    //pub11.publish(data0);
    //pub12.publish(data0);
    //ROS_INFO("Worked!!! %i",IMULeft.getNumPublishers());

    tareLeft.call(emptyCall);
    tareRight.call(emptyCall);
    while (ros::ok())
    {//time=timestep+time;
        //if (IMULeft.getTopic()=="\0"){ time=0; }
        //ROS_INFO("In loop %i",IMULeft.getNumPublishers());
        //SendGazebo(time_);

        teta_motor_L=teta_motor_L+teta_pid_L.Calculate(0,teta_L);
        phi_motor_L=phi_motor_L+phi_pid_L.Calculate(0,phi_L);

        teta_motor_R=teta_motor_R+teta_pid_R.Calculate(0,teta_R);
        phi_motor_R=phi_motor_R+phi_pid_R.Calculate(0,phi_R);

        teta_motor_center=teta_motor_center+teta_pid_center.Calculate(0,teta_center);
        phi_motor_center=phi_motor_center+phi_pid_center.Calculate(0,phi_center);

        double maximum_d=.15;

        if (teta_motor_L<-maximum_d){teta_motor_L=-maximum_d;}
        if (teta_motor_L>maximum_d){teta_motor_L=maximum_d;}
        if (phi_motor_L<-maximum_d){phi_motor_L=-maximum_d;}
        if (phi_motor_L>maximum_d){phi_motor_L=maximum_d;}

        if (teta_motor_R<-maximum_d){teta_motor_R=-maximum_d;}
        if (teta_motor_R>maximum_d){teta_motor_R=maximum_d;}
        if (phi_motor_R<-maximum_d){phi_motor_R=-maximum_d;}
        if (phi_motor_R>maximum_d){phi_motor_R=maximum_d;}

       //int qref(12);
//        for (int i = 0; i < 12; ++i) {
//            qref[i]=0;
//        }

//        qref[4]=int(teta_motor_L*2304*100/2/3.141592);
//       // qref[4]=int(phi_motor_L*2304*100/2/3.141592);
//        qref[5]=int(phi_motor_L*2304*100/2/3.141592);
//        qref[1]=int(-teta_motor_R*2304*100/2/3.141592);
//        qref[0]=int(phi_motor_R*2304*100/2/3.141592);
//        int maximum=8000;

//        if (qref[4]<-maximum){qref[4]=-maximum;}
//        if (qref[4]>maximum){qref[4]=maximum;}
//        if (qref[5]<-maximum){qref[5]=-maximum;}
//        if (qref[5]>maximum){qref[5]=maximum;}

//        if (qref[0]<-maximum){qref[0]=-maximum;}
//        if (qref[0]>maximum){qref[0]=maximum;}
//        if (qref[1]<-maximum){qref[1]=-maximum;}
//        if (qref[1]>maximum){qref[1]=maximum;}

        vector<double> cntrl(13);
        cntrl[0]=0.0;
        cntrl[1]=0;
        cntrl[2]=-phi_motor_center;
        cntrl[3]=0;
        cntrl[4]=0;
        cntrl[5]=teta_motor_R;
        cntrl[6]=phi_motor_R;
        cntrl[7]=0;
        cntrl[8]=-phi_motor_center;
        cntrl[9]=0;
        cntrl[10]=0;
        cntrl[11]=teta_motor_L;
        cntrl[12]=phi_motor_L;

msg.data.clear();
vector<int> qref(12);
        QCgenerator QC;
        qref=QC.ctrldata2qc(cntrl);
// ROS_INFO("teta_motor_L_QC:[%d] phi_motor_L_QC:[%d] teta_motor_R_QC:[%d] phi_motor_R_QC:[%d] ", qref[4],qref[5], qref[1],qref[0]);
       // ROS_INFO("phi_motor_L_QC:[%d] phi_IMU : [%f]",qref[5],phi_L);
       // ROS_INFO("teta_l_IMU={%f} phi_l_IMU={%f} teta_l={%f} phi_l={%f}",teta_L,phi_L,teta_motor_L,phi_motor_L);
        ROS_INFO(" phi_c_IMU={%f}}",phi_center);

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

          //  cout << qref[i-1] <<" , "<<flush;
          msg.data.push_back(qref[i]);

        }
          chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <iostream>
#include <cmath>

#include "../include/KF.h"
#include "../include/dsho.h"
#include "../include/matern32.h"
#include "../include/ndsho.h"

using namespace Eigen;
using namespace std;

#define TWOPI 6.283185307179586

// function to read a space separated file

std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main() {

	// read file with two component damped simple harmonic oscillator simulated data
	//std::vector<double> values = load_csv("two_comp_dsho.txt");
	//cout << values.size() << endl;

	// map data to VectorXds
	//VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
	//VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);

  int vecsize = 10000;
	// map data to VectorXds
	VectorXd yi(vecsize);
  VectorXd times = Eigen::VectorXd::Random(vecsize);
  times.array() += 1.0;
  times.array() *= (100*0.5);
  std::sort(times.data(), times.data() + times.size());

	// this is the observational error vector
	VectorXd yerr = VectorXd::Ones(yi.size());
	yerr.array() *= 0.05;

	VectorXd y_sim, y_sim_one;

	for(int j=0; j < 24; j++) {
		//auto const seed = std::random_device()();
		int seed = 123;
		std::mt19937 rng(seed);
		cout << seed << endl;
		// set parameters for a 2 component DSHO model
		Eigen::Vector2d omegas,Qpars,varfs;
		Eigen::Matrix<double,1,1> omega_one, Qpar_one, varf_one;
		double omega0 = 8; // this component stays fixed
		double period_list[6] = {1.0, 2.0, 4.0, 8.0, 16.0, 32.0};
		std::cout << j/6 << j%6 << std::endl;
		double omega1 = TWOPI / period_list[j%6];
		double Q0 = 10.0;
		double Q1 = 10.0;
		double varf0 = 1.0;
		double varf1 = 1.0;
		if ((j/6 == 1) || (j/6 ==3))
		{
			varf0 = 0.01;
			varf1 = 0.01;
		}
		if (j/6 > 1)
		{
			Q1 = 1.0;
		}
		omegas << omega0, omega1;
		Qpars << Q0, Q1;
		varfs << varf0, varf1;

		omega_one << omega1;
		Qpar_one << Q1;
		varf_one << varf1;

		// Simulate a 2 component DSHO model
		gpstate::n_dsho::N_DSHOSolver ndsho(times,yi,yerr,omegas, Qpars, varfs);
		ndsho.simulate_N_DSHO(y_sim, rng);

		// For efficiency, just output one of the components as well (the varying one)
		gpstate::n_dsho::N_DSHOSolver ndsho_one(times,yi,yerr,omega_one, Qpar_one, varf_one);
		ndsho_one.simulate_N_DSHO(y_sim_one, rng);

		// write simulated vector to file GPtest.txt
		std::ofstream file("GPtest" + std::to_string(j+1) + "_ndsho.txt");
		assert(file.is_open());
		file << "# N_DSHO simulated light curve" << endl;
		file << "# input parameters: " << endl;
		file << "# omega: " << omega0 << ", " << omega1 << endl;
		file << "# Q: " << Q0 << ", " << Q1 << endl;
		file << "# varf: " << varf0 << ", " << varf1 << endl;
		file << "# time y_sim" << endl;
		for(int i=0; i<times.rows(); i++)
			file <<  times(i) <<" "<< y_sim(i) << endl;

		std::ofstream file_one("GPtest" + std::to_string(j+1) + "_dsho.txt");
		file_one << "# DSHO simulated light curve" << endl;
		file_one << "# input parameters: " << endl;
		file_one << "# omega: " << omega_one << endl;
		file_one << "# Q: " << Qpar_one << endl;
		file_one << "# varf: " << varf_one << endl;
		file_one << "# time y_sim" << endl;
		for(int i=0; i<times.rows(); i++)
			file_one <<  times(i) <<" "<< y_sim_one(i) << endl;

	}
}


/* -*- c++ -*- */
/*
 * Copyright 2013 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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 3, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "selective_fading_model_impl.h"
#include <gnuradio/io_signature.h>
#include <iostream>

#include <boost/format.hpp>
#include <boost/random.hpp>

#include <gnuradio/fxpt.h>
#include <sincostable.h>

// FASTSINCOS:  0 = slow native,  1 = gr::fxpt impl,  2 = sincostable.h
#define FASTSINCOS  2

namespace gr {
  namespace channels {

    selective_fading_model::sptr
    selective_fading_model::make( unsigned int N, float fDTs, bool LOS, float K, int seed, std::vector<float> delays, std::vector<float> mags, int ntaps)
    {
      return gnuradio::get_initial_sptr
	(new selective_fading_model_impl( N, fDTs, LOS, K, seed, delays, mags, ntaps));
    }

    // Block constructor
    selective_fading_model_impl::selective_fading_model_impl( unsigned int N, float fDTs, bool LOS, float K, int seed, std::vector<float> delays, std::vector<float> mags, int ntaps )
      : sync_block("selective_fading_model",
		       io_signature::make(1, 1, sizeof(gr_complex)),
		       io_signature::make(1, 1, sizeof(gr_complex))),
        d_delays(delays),
        d_mags(mags),
        d_sintable(1024)
    {
        if(mags.size() != delays.size())
            throw std::runtime_error("magnitude and delay vectors must be the same length!");

        for(size_t i=0; i<mags.size(); i++){
            d_faders.push_back(new gr::channels::flat_fader_impl(N, fDTs, (i==0)&&(LOS), K, seed+i));
        }

        // set up tap history
        if(ntaps < 1){ throw std::runtime_error("ntaps must be >= 1"); }
        set_history(1+ntaps);
        d_taps.resize(ntaps, gr_complex(0,0));
    }

    selective_fading_model_impl::~selective_fading_model_impl()
    {
        for(size_t i=0; i<d_faders.size(); i++){
            delete d_faders[i];
        }
    }

    int
    selective_fading_model_impl::work (int noutput_items,
        gr_vector_const_void_star &input_items,
        gr_vector_void_star &output_items)
    {
        const gr_complex* in = (const gr_complex*) input_items[0];
        gr_complex* out = (gr_complex*) output_items[0];

        // loop over each output sample
        for(int i=0; i<noutput_items; i++){

            // clear the current values in each tap
            for(size_t j=0; j<d_taps.size(); j++){
                d_taps[j] = gr_complex(0,0);
            }

            // add each flat fading component to the taps
            for(size_t j=0; j<d_faders.size(); j++){
                gr_complex ff_H(d_faders[j]->next_sample());
                for(size_t k=0; k<d_taps.size(); k++){
                    float dist = k-d_delays[j];
                    float interpmag = d_sintable.sinc(M_PI*dist);
                    d_taps[k] += ff_H * interpmag * d_mags[j];
                }
            }

            // apply the taps and generate output
            gr_complex sum(0,0);
            for(size_t j=0; j<d_taps.size(); j++){
                sum += in[i+j] * d_taps[d_taps.size()-j-1];
            }

            // assign output
            out[i] = sum;
        }

        // return all outputs
        return noutput_items;
    }

    void
    selective_fading_model_impl::setup_rpc()
    {
#ifdef GR_CTRLPORT
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_get<selective_fading_model, float >(
            alias(), "fDTs",
            &selective_fading_model::fDTs,
            pmt::mp(0), pmt::mp(1), pmt::mp(0.01),
            "Hz*Sec", "normalized maximum doppler frequency (fD*Ts)",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_set<selective_fading_model, float >(
            alias(), "fDTs",
            &selective_fading_model::set_fDTs,
            pmt::mp(0), pmt::mp(1), pmt::mp(0.01),
            "Hz*Sec", "normalized maximum doppler frequency (fD*Ts)",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));

    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_get<selective_fading_model, float >(
            alias(), "K",
            &selective_fading_model::K,
            pmt::mp(0), pmt::mp(8), pmt::mp(4),
            "Ratio", "Rician factor (ratio of the specular power to the scattered power)",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_set<selective_fading_model, float >(
            alias(), "K",
            &selective_fading_model::set_K,
            pmt::mp(0), pmt::mp(8), pmt::mp(4),
            "Ratio", "Rician factor (ratio of the specular power to the scattered power)",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));

    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_get<selective_fading_model, float >(
            alias(), "step",
            &selective_fading_model::step,
            pmt::mp(0), pmt::mp(8), pmt::mp(4),
            "radians", "Maximum step size for random walk angle per sample",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));
    add_rpc_variable(
        rpcbasic_sptr(new rpcbasic_register_set<selective_fading_model, float >(
            alias(), "step",
            &selective_fading_model::set_step,
            pmt::mp(0), pmt::mp(1), pmt::mp(0.00001),
            "radians", "Maximum step size for random walk angle per sample",
            RPC_PRIVLVL_MIN, DISPTIME | DISPOPTSTRIP)));
#endif /* GR_CTRLPORT */
    }

  } /* namespace channels */
} /* namespace gr */


/*
==============================================================================
KratosStructuralApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
janosch.stascheit@rub.de
nagel@sd.rub.de
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
- Ruhr-University Bochum, Institute for Structural Mechanics, Germany

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/
/* **************************************************************************************
*
*   Last Modified by:    $Author: hbui $
*   Date:                $Date: 24 Oct 2014 $
*   Revision:            $Revision: 1.1 $
*
* ***************************************************************************************/

// System includes
// External includes
#include <boost/timer.hpp>
// Project includes
#include "includes/define.h"
#include "line_pressure_isogeometric_2d.h"
#include "utilities/math_utils.h"
#include "structural_application/custom_utilities/sd_math_utils.h"
#include "isogeometric_application/isogeometric_application.h"

//#define DEBUG_LEVEL1

namespace Kratos
{
//***********************************************************************************
//***********************************************************************************
// -------- //
//  PUBLIC  //
// -------- //

// Constructor
LinePressureIsogeometric2D::LinePressureIsogeometric2D()
{
}

// Constructor
LinePressureIsogeometric2D::LinePressureIsogeometric2D( IndexType NewId, GeometryType::Pointer pGeometry )
    : LineForceIsogeometric( NewId, pGeometry )
{
}

// Constructor
LinePressureIsogeometric2D::LinePressureIsogeometric2D( IndexType NewId, GeometryType::Pointer pGeometry,
                          PropertiesType::Pointer pProperties )
    : LineForceIsogeometric( NewId, pGeometry, pProperties )
{
}

//***********************************************************************************
//***********************************************************************************
Condition::Pointer LinePressureIsogeometric2D::Create( IndexType NewId,
                                        NodesArrayType const& ThisNodes,
                                        PropertiesType::Pointer pProperties ) const
{
    return Condition::Pointer( new LinePressureIsogeometric2D( NewId, GetGeometry().Create( ThisNodes ), pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
Condition::Pointer LinePressureIsogeometric2D::Create( IndexType NewId,
                                        GeometryType::Pointer pGeom,
                                        PropertiesType::Pointer pProperties ) const
{
    return Condition::Pointer( new LinePressureIsogeometric2D( NewId, pGeom, pProperties ) );
}

//***********************************************************************************
//***********************************************************************************
// Destructor
LinePressureIsogeometric2D::~LinePressureIsogeometric2D()
{
}

//***********************************************************************************
//***********************************************************************************
void LinePressureIsogeometric2D::GetDofList( DofsVectorType& ConditionalDofList,
                              const ProcessInfo& rCurrentProcessInfo ) const
{
    ConditionalDofList.resize( 0 );

    for ( unsigned int i = 0; i < GetGeometry().size(); ++i )
    {
        ConditionalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_X ) );
        ConditionalDofList.push_back( GetGeometry()[i].pGetDof( DISPLACEMENT_Y ) );
    }
}

//***********************************************************************************
void LinePressureIsogeometric2D::CalculateDampingMatrix(MatrixType& rDampMatrix, const ProcessInfo& rCurrentProcessInfo)
{
    KRATOS_TRY
    rDampMatrix.resize(0,0,false);
    KRATOS_CATCH("")
}
//***********************************************************************************
//***********************************************************************************
//***********************************************************************************
//***********************************************************************************
void LinePressureIsogeometric2D::CalculateAll( MatrixType& rLeftHandSideMatrix,
                                VectorType& rRightHandSideVector,
                                const ProcessInfo& rCurrentProcessInfo,
                                bool CalculateStiffnessMatrixFlag,
                                bool CalculateResidualVectorFlag )
{
    KRATOS_TRY

    const unsigned int number_of_nodes = GetGeometry().size();
    const unsigned int dim = 2;
    unsigned int MatSize = number_of_nodes * dim;
    //resizing as needed the LHS

    if ( CalculateStiffnessMatrixFlag == true ) //calculation of the matrix is required
    {
        if ( rLeftHandSideMatrix.size1() != MatSize )
            rLeftHandSideMatrix.resize( MatSize, MatSize, false );

        noalias( rLeftHandSideMatrix ) = ZeroMatrix( MatSize, MatSize ); //resetting LHS
    }

    //resizing as needed the RHS
    if ( CalculateResidualVectorFlag == true ) //calculation of the matrix is required
    {
        if ( rRightHandSideVector.size() != MatSize )
            rRightHandSideVector.resize( MatSize, false );

        rRightHandSideVector = ZeroVector( MatSize ); //resetting RHS
    }

    //reading integration points
    const GeometryType::IntegrationPointsArrayType& integration_points =
                mpIsogeometricGeometry->IntegrationPoints(mThisIntegrationMethod);

    //calculating shape function values and local gradients
    GeometryType::ShapeFunctionsGradientsType DN_De;
    Matrix Ncontainer;

    mpIsogeometricGeometry->CalculateShapeFunctionsIntegrationPointsValuesAndLocalGradients(
        Ncontainer,
        DN_De,
        integration_points
    );

    #ifdef DEBUG_LEVEL1
    KRATOS_WATCH(mThisIntegrationMethod)
    KRATOS_WATCH(integration_points.size())
    KRATOS_WATCH(typeid(*mpIsogeometricGeometry).name())
    KRATOS_WATCH(mpIsogeometricGeometry->size())
    std::cout << "At condition " << Id() << ":";
    for (int i = 0; i < mpIsogeometricGeometry->size(); ++i)
        std::cout << " " << (*mpIsogeometricGeometry)[i].Id();
    std::cout << std::endl;
    KRATOS_WATCH(Ncontainer)
    #endif

    const double& P = this->GetValue(PRESSURE);
//    KRATOS_WATCH(P)

    // loop over integration points
    Vector Load( dim );
    Vector t( dim );
    Vector G( dim );
    double dL;
    for ( unsigned int PointNumber = 0; PointNumber < integration_points.size(); ++PointNumber )
    {
        // compute integration weight
        double IntegrationWeight = integration_points[PointNumber].Weight();

        IntegrationWeight *= GetProperties()[THICKNESS];

        // compute length
        noalias( t ) = ZeroVector( dim );//tangential vector
        noalias( G ) = ZeroVector( dim );
        for ( unsigned int n = 0; n < GetGeometry().size(); ++n )
        {
            t[0] += GetGeometry().GetPoint( n ).X0() * DN_De[PointNumber]( n, 0 );
            t[1] += GetGeometry().GetPoint( n ).Y0() * DN_De[PointNumber]( n, 0 );
            G[0] += GetGeometry().GetPoint( n ).X0() * Ncontainer( PointNumber, n );
            G[1] += GetGeometry().GetPoint( n ).Y0() * Ncontainer( PointNumber, n );
        }
        dL = norm_2(t);

        //calculating load
        Load[0] = -P*t[1];
        Load[1] = P*t[0];

        // contribute to RIGHT HAND SIDE VECTOR
        if ( CalculateResidualVectorFlag == true ) //calculation of the matrix is required
        {
            for ( unsigned int prim = 0; prim < GetGeometry().size(); ++prim )
                for ( unsigned int i = 0; i < dim; ++i )
                    rRightHandSideVector( prim * dim + i ) +=
                        Ncontainer( PointNumber, prim ) * Load( i ) * IntegrationWeight;
        }
    }

    #ifdef DEBUG_LEVEL1
    KRATOS_WATCH(rRightHandSideVector)
    #endif

    KRATOS_CATCH( "" )
}

} // Namespace Kratos.

#undef DEBUG_LEVEL1


#include <boost/geometry/formulas/vincenty_inverse.hpp>


#include <vector>
#include <string>
#include <iostream>
#include <unistd.h>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <std_msgs/ColorRGBA.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float64MultiArray.h"
// #include "/usr/include/eigen3/Eigen/Dense"
#include <Eigen/Dense>

using namespace cv;
using namespace std;
using namespace Eigen;

class FindNRT
{
private:
  // node handle, image, and video variables
  ros::NodeHandle nh_;
  int cam_num, img_width_, img_hight_;
  std::string window_name_;
  VideoCapture cap_;
  Mat frame_;

  // variables for board parameters
  double sqr_size_;
  int row_corners_, col_corners_;

  // variables for camera parameters
  int cam_mat_row_, cam_mat_col_;
  int dist_coef_row_, dist_coef_col_;

  // variables for solvePnP
  std::vector<cv::Point3f> object_points_;
  std::vector<cv::Point2f> image_points_;
  cv::Mat cam_mat_;
  cv::Mat dist_coef_;
  cv::Mat rvec_, tvec_;

  // variables for resutls
  cv::Mat R_cvmat_;
  bool Refreshed_;

  // Subscribe and Publish topics
  ros::Publisher pos_pub_;
  ros::Subscriber locs_sub_;

  // variables for subscribing to image points and publishing world points
  std_msgs::Float64MultiArray locs_ordered_, pos_world_;

public:
  FindNRT()
  {
    // initializing video object
    nh_.getParam("/findNRT/cam_num", cam_num);
    nh_.getParam("/findNRT/cam_params/image_width", img_width_);
    nh_.getParam("/findNRT/cam_params/image_hight", img_hight_);
    window_name_ = "checkerboard_img";
    cap_ = VideoCapture(cam_num);
    cv::namedWindow(window_name_, cv::WINDOW_NORMAL);
    cv::resizeWindow(window_name_, img_width_, img_hight_);

    // initializing board parameters
    nh_.getParam("/findNRT/board_params/size/row_corners",row_corners_);
    nh_.getParam("/findNRT/board_params/size/col_corners",col_corners_);
    nh_.getParam("/findNRT/board_params/sqr_size",sqr_size_);

    //initializing object points
    object_points_ = std::vector<cv::Point3f> (row_corners_*col_corners_,cv::Point3f());
    // initializing camera matrix
    nh_.getParam("/findNRT/cam_params/camera_matrix/rows", cam_mat_row_);
    nh_.getParam("/findNRT/cam_params/camera_matrix/cols", cam_mat_col_);
    cam_mat_ = cv::Mat(cam_mat_row_,cam_mat_col_,cv::DataType<double>::type);
    // initializing distortion coefficients
    nh_.getParam("/findNRT/cam_params/distortion_coefficients/rows", dist_coef_row_);
    nh_.getParam("/findNRT/cam_params/distortion_coefficients/cols", dist_coef_col_);
    dist_coef_ = cv::Mat(dist_coef_row_,dist_coef_col_,cv::DataType<double>::type);
    // initializing rotation and translation vectors
    rvec_ = cv::Mat(1,3,cv::DataType<double>::type);//CV_32F
    tvec_ = cv::Mat(1,3,cv::DataType<double>::type);
    // initializing Rotation matrix
    R_cvmat_ = cv::Mat(3,3,cv::DataType<double>::type);

    // initializing subscriber and publisher
    locs_sub_ = nh_.subscribe("/locs/ordered", 1, &FindNRT::callback, this);
    pos_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/pos/world", 10);

    // initializing locs_ordered and pos_world
    locs_ordered_.data.clear();
    pos_world_.data.clear();

  }

  ~FindNRT()
  {
    cv::destroyAllWindows();
  }

  void find_nrt() // finds rotation matrix and translation vector using checkerboard
  {
    get_img();
    get_cam_param();

    bool patternfound = false;
    cv::Size patternsize (row_corners_, col_corners_);
    patternfound = cv::findChessboardCorners(frame_,
                                             patternsize,
                                             image_points_,
                                             cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK );
    while (!patternfound) // if pattern not detected, repeat process with new image
    {
      std::cout<<"No pattern found. Modify board postition, or camera postion/parameters"<<endl;
      get_img();
      show_imag();
      patternfound = cv::findChessboardCorners(frame_,
                                               patternsize,
                                               image_points_,
                                               cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK );
    }
    // checkerboard pattern should be defined now
    cv::drawChessboardCorners(frame_, patternsize, image_points_, patternfound);
    show_imag();
    // at this point we have data in image_points

    // define object_points with data corresponding to the corners of the board:
    // the data points are defined based on the image points generated. y decreases
    // (going from bottom to top of image), then x increases (going from left
    // to right of image) to define the data points.
    for (int i = 0; i < col_corners_; ++i)
    {
      for (int j = 0; j < row_corners_; ++j)
      {
        object_points_.at( row_corners_*i+j ) = cv::Point3f((i+1)*sqr_size_, row_corners_*sqr_size_ - (j+1)*sqr_size_, 0);
      }
    }

    bool solved;
    bool use_extrinsic_guess;
    solved = cv::solvePnP (object_points_, image_points_,
                           cam_mat_, dist_coef_,
                           rvec_, tvec_);

    cv::Rodrigues(rvec_, R_cvmat_);

  // // uncomment debugging section to print out R, T, image points, object popints, and draw x-y axis on image
  // ///////// DEBUGGING /////////////////
  //
  // cout<<"R_cvmat = " <<endl << R_cvmat << endl;
  // cout<<"tvec = " <<endl << tvec << endl;
  //
  // for (int i = 0; i < col_corners_; ++i)
  // {
  //   for (int j = 0; j < row_corners_; ++j)
  //   {
  //     cout << object_points_.at( row_corners_*i+j ) << "    " << image_points_.at( row_corners_*i+j ) << endl;
  //   }
  // }
  //
  //  int radius = 5;
  //  int thickness = 3;
  //
  //  std::vector<cv::Point3d> points;
  //  points.push_back(cv::Point3d(0,0,0));
  //  points.push_back(cv::Point3d(0.5,0,0));
  //  points.push_back(cv::Point3d(0,0.5,0));
  //
  //  std::vector<cv::Point2d> projected_points;
  //  cv::projectPoints(points, rvec_, tvec_, cam_mat_, dist_coef_, projected_points);
  //
  //  line(frame_, projected_points[0], projected_points[1], Scalar(255, 255, 255), thickness); // x-axis in white
  //  line(frame_, projected_points[0], projected_points[2], Scalar(0, 0, 0), thickness); // y-axis in black
  //
  //  circle(frame_,  projected_points[0], radius, Scalar(0, 0, 0), thickness);
  //  circle(frame_,  projected_points[1], radius, Scalar(0, 0, 0), thickness);
  //  circle(frame_,  projected_points[2], radius, Scalar(0, 0, 0), thickness);
  //
  //  show_imag();
  //  ////////// DEBUGGING END ///////////////

    stop_cap();
  }

  void pub_recon_data() // finds x,y,z world position of detected objects in the image
  {
    if (locs_ordered_.data.empty())
    {
      return;
    }

    int num_rob = locs_ordered_.data.size()/2;

    MatrixXd imgPts; imgPts.resize(3, num_rob);
    for (int i = 0; i < num_rob; ++i)
    {
      imgPts(0,i) = locs_ordered_.data[2*i];
      imgPts(1,i) = locs_ordered_.data[2*i+1];
      imgPts(2,i) = 1;
    }

    MatrixXd camMat; camMat.resize(3,3);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
      {
        camMat(i,j) = cam_mat_.at<double>(i,j);
      }
    }

    MatrixXd invCamMat; invCamMat.resize(3,3);
    invCamMat = camMat.inverse();

    MatrixXd p; p.resize(3,num_rob);
    p = invCamMat * imgPts;

    Vector3d nc;
    Vector3d temp001;
    temp001 << 0,0,1;
    MatrixXd R; R.resize(3,3);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
      {
        R(i,j) = R_cvmat_.at<double>(i,j);
      }
    }
    nc = R * temp001;

    double d = 0;

    Vector3d T;
    for (int i = 0; i < 3; ++i)
    {
      T(i) = tvec_.at<double>(i);
    }
    d = nc.dot(T) ;

    MatrixXd nxp; nxp.resize(1,num_rob);
    nxp = nc.transpose() * p;

    MatrixXd P; P.resize(3, num_rob);
    for (int i = 0; i < num_rob; ++i)
    {
      for (int j = 0; j < 3; ++j)
      {
        P(j,i) = d * p(j,i) / nxp(i);
      }
    }

    MatrixXd Pw; Pw.resize(3,num_rob);
    MatrixXd rtp; rtp.resize(3,num_rob);
    MatrixXd rtt; rtt.resize(3,1);
    rtp = R.transpose() * P;
    rtt = - R.transpose() * T;

    for (int i = 0; i < num_rob; ++i)
    {
      for (int j = 0; j < 3; ++j)
      {
        Pw(j,i) = rtp(j,i) + rtt(j);
      }
    }

    double pos[2][num_rob];
    for (int i = 0; i < num_rob; ++i)
    {
      pos[0][i] = + Pw(0,i);
      pos[1][i] = - Pw(1,i); // flip y to get the data represented in the conventional x-y-z frame with z pointing up (necessary for form ctrl)
      this->pos_world_.data.push_back(pos[0][i]);
      this->pos_world_.data.push_back(pos[1][i]);
    }

    pos_pub_.publish(this->pos_world_);

    pos_world_.data.clear();

  }

private:

  void get_img() // gets one image from camera
  {
    if (!this->cap_.isOpened())
    {
      return;
    }
    cv::Mat raw_img;
    this->cap_ >> raw_img;
    cv::cvtColor(raw_img, this->frame_, CV_BGR2GRAY);
    return;
  }

  void show_imag() // display image
  {
    if(this->frame_.empty())
    {
      return;
    }
    cv::imshow(window_name_, this->frame_);
    int key = cv::waitKey(1);
    return;
  }

  void stop_cap() //destroys video object and closes windows if destroyAllWindows is uncommented
  {
    cv::destroyAllWindows();
    this->cap_.release();
    return;
  }

  void get_cam_param() // gets camera parameters from yaml file
  {
    std::vector<double> a;
    std::vector<double> d;
    this->nh_.getParam("/findNRT/cam_params/camera_matrix/data",a);
    this->nh_.getParam("/findNRT/cam_params/distortion_coefficients/data",d);
    for (int i = 0; i < this->cam_mat_row_; ++i)
    {
      for (int j = 0; j < this->cam_mat_col_ ; ++j)
      {
        this->cam_mat_.at<double>(i,j) = a[cam_mat_col_*i + j];
      }
    }
    for (int i = 0; i < dist_coef_row_; ++i)
    {
      for (int j = 0; j < dist_coef_col_; ++j)
      {
        this->dist_coef_.at<double>(i,j) = d[cam_mat_col_*i + j];
      }
    }
    return;
  }

  void callback(const std_msgs::Float64MultiArray& locs) // callback to get ordered locations
  {
    this->locs_ordered_ = locs;
    pub_recon_data();
    return;
  }

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "findNRT");
  ros::NodeHandle nh;

  ros::Rate r(30);

  FindNRT f;
  f.find_nrt();
  f.pub_recon_data();
  ros::spin();
  r.sleep();

  if(!ros::ok())
  {
    return 0;
  }

  ros::shutdown();

}


#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "CALPHADConcSolverBinary.h"
#include "CALPHADEqConcSolverBinary.h"
#include "CALPHADFunctions.h"
#include "PhysicalConstants.h"
#include "functions.h"
#include "well_functions.h"

#include <boost/property_tree/json_parser.hpp>

#include <iomanip>
#include <string>

namespace pt = boost::property_tree;

namespace Thermo4PFM
{

CALPHADFreeEnergyFunctionsBinary::CALPHADFreeEnergyFunctionsBinary(
    pt::ptree& calphad_db, boost::optional<pt::ptree&> newton_db,
    const EnergyInterpolationType energy_interp_func_type,
    const ConcInterpolationType conc_interp_func_type)
    : energy_interp_func_type_(energy_interp_func_type),
      conc_interp_func_type_(conc_interp_func_type),
      newton_tol_(1.e-8),
      newton_alpha_(1.),
      newton_maxits_(20),
      newton_verbose_(false)
{
    std::string fenergy_diag_filename("energy.vtk");
    fenergy_diag_filename_ = new char[fenergy_diag_filename.length() + 1];
    strcpy(fenergy_diag_filename_, fenergy_diag_filename.c_str());

    readParameters(calphad_db);

    if (newton_db) readNewtonparameters(newton_db.get());
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::readNewtonparameters(
    pt::ptree& newton_db)
{
    newton_tol_     = newton_db.get<double>("tol", newton_tol_);
    newton_alpha_   = newton_db.get<double>("alpha", newton_alpha_);
    newton_maxits_  = newton_db.get<int>("max_its", newton_maxits_);
    newton_verbose_ = newton_db.get<bool>("verbose", newton_verbose_);
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::readParameters(pt::ptree& calphad_db)
{
    pt::ptree& species0_db = calphad_db.get_child("SpeciesA");
    std::string dbnameL("PhaseL");
    g_species_phaseL_[0].initialize("L0", species0_db.get_child(dbnameL));
    std::string dbnameA("PhaseA");
    g_species_phaseA_[0].initialize("A0", species0_db.get_child(dbnameA));

    pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
    g_species_phaseL_[1].initialize("L1", speciesB_db.get_child(dbnameL));
    g_species_phaseA_[1].initialize("A1", speciesB_db.get_child(dbnameA));

    // read Lmix coefficients
    std::string dbnamemixL("LmixPhaseL");
    pt::ptree Lmix0_db = calphad_db.get_child(dbnamemixL);
    readLmixBinary(Lmix0_db, LmixPhaseL_);

    std::string dbnamemixA("LmixPhaseA");
    pt::ptree Lmix1_db = calphad_db.get_child(dbnamemixA);
    readLmixBinary(Lmix1_db, LmixPhaseA_);

    // print database just read
    // std::clog << "CALPHAD database..." << std::endl;
    // pt::write_json(std::clog, calphad_db);
}

//-----------------------------------------------------------------------
#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp declare target
#endif

double CALPHADFreeEnergyFunctionsBinary::computeFreeEnergy(
    const double temperature, const double* const conc, const PhaseIndex pi,
    const bool gp)
{
    const CalphadDataType l0 = lmixPhase(0, pi, temperature);
    const CalphadDataType l1 = lmixPhase(1, pi, temperature);
    const CalphadDataType l2 = lmixPhase(2, pi, temperature);
    const CalphadDataType l3 = lmixPhase(3, pi, temperature);

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        default:
            //            std::cerr << "CALPHADFreeEnergyFunctionsBinary::"
            //                         "computeFreeEnergy(), undefined phase"
            //                      << "!!!" << std::endl;
            // abort();
            return 0.;
    }

    double fe = conc[0] * g_species[0].fenergy(temperature)
                + (1. - conc[0]) * g_species[1].fenergy(temperature)
                + CALPHADcomputeFMixBinary(l0, l1, l2, l3, conc[0])
                + CALPHADcomputeFIdealMixBinary(
                      gas_constant_R_JpKpmol * temperature, conc[0]);

    // subtract -mu*c to get grand potential
    if (gp)
    {
        double deriv;
        computeDerivFreeEnergy(temperature, conc, pi, &deriv);
        fe -= deriv * conc[0];
    }

    return fe;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::computeDerivFreeEnergy(
    const double temperature, const double* const conc, const PhaseIndex pi,
    double* deriv)
{
    const CalphadDataType l0 = lmixPhase(0, pi, temperature);
    const CalphadDataType l1 = lmixPhase(1, pi, temperature);
    const CalphadDataType l2 = lmixPhase(2, pi, temperature);
    const CalphadDataType l3 = lmixPhase(3, pi, temperature);

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        default:
            //            std::cerr << "CALPHADFreeEnergyFunctionsBinary::"
            //                         "computeFreeEnergy(), undefined phase!!!"
            //                      << std::endl;
            // abort();
            return;
    }

    double mu = (g_species[0].fenergy(temperature)
                    - g_species[1].fenergy(temperature))
                + CALPHADcomputeFMix_derivBinary(l0, l1, l2, l3, conc[0])
                + CALPHADcomputeFIdealMix_derivBinary(
                      gas_constant_R_JpKpmol * temperature, conc[0]);

    deriv[0] = mu;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::computeSecondDerivativeFreeEnergy(
    const double temp, const double* const conc, const PhaseIndex pi,
    double* d2fdc2)
{
    // assert(conc[0] >= 0.);
    // assert(conc[0] <= 1.);

    const CalphadDataType l0 = lmixPhase(0, pi, temp);
    const CalphadDataType l1 = lmixPhase(1, pi, temp);
    const CalphadDataType l2 = lmixPhase(2, pi, temp);
    const CalphadDataType l3 = lmixPhase(3, pi, temp);
    const double rt          = gas_constant_R_JpKpmol * temp;

    d2fdc2[0] = (CALPHADcomputeFMix_deriv2Binary(l0, l1, l2, l3, conc[0])
                 + CALPHADcomputeFIdealMix_deriv2Binary(rt, conc[0]));
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::computeTdependentParameters(
    const double temperature, CalphadDataType* Lmix_L, CalphadDataType* Lmix_A,
    CalphadDataType* fA, CalphadDataType* fB)
{
    fA[0]     = g_species_phaseL_[0].fenergy(temperature);
    fB[0]     = g_species_phaseL_[1].fenergy(temperature);
    Lmix_L[0] = lmixPhase(0, PhaseIndex::phaseL, temperature);
    Lmix_L[1] = lmixPhase(1, PhaseIndex::phaseL, temperature);
    Lmix_L[2] = lmixPhase(2, PhaseIndex::phaseL, temperature);
    Lmix_L[3] = lmixPhase(3, PhaseIndex::phaseL, temperature);

    fA[1]     = g_species_phaseA_[0].fenergy(temperature);
    fB[1]     = g_species_phaseA_[1].fenergy(temperature);
    Lmix_A[0] = lmixPhase(0, PhaseIndex::phaseA, temperature);
    Lmix_A[1] = lmixPhase(1, PhaseIndex::phaseA, temperature);
    Lmix_A[2] = lmixPhase(2, PhaseIndex::phaseA, temperature);
    Lmix_A[3] = lmixPhase(3, PhaseIndex::phaseA, temperature);
}

//=======================================================================

// compute equilibrium concentrations in various phases for given temperature
bool CALPHADFreeEnergyFunctionsBinary::computeCeqT(
    const double temperature, double* ceq, const int maxits, const bool verbose)
{
#ifndef HAVE_OPENMP_OFFLOAD
    if (verbose)
        std::cout << "CALPHADFreeEnergyFunctionsBinary::computeCeqT()"
                  << std::endl;
#endif
    // assert(temperature > 0.);

    // evaluate temperature dependent parameters
    CalphadDataType fA[3];
    CalphadDataType fB[3];

    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];

    computeTdependentParameters(temperature, Lmix_L, Lmix_A, fA, fB);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CALPHADEqConcSolverBinary eq_solver;

    eq_solver.setup(RTinv, Lmix_L, Lmix_A, fA, fB);
    int ret = eq_solver.ComputeConcentration(ceq, newton_tol_, maxits);

#ifndef HAVE_OPENMP_OFFLOAD
    if (ret >= 0)
    {
        if (verbose)
        {
            std::cout << "CALPHAD, c_eq phase0=" << ceq[0] << std::endl;
            std::cout << "CALPHAD, c_eq phase1=" << ceq[1] << std::endl;
        }
    }
    else
    {
        std::cout << "CALPHADFreeEnergyFunctionsBinary, WARNING: ceq "
                     "computation did not converge"
                  << std::endl;
    }
#endif

    return (ret >= 0);
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::computePhasesFreeEnergies(
    const double temperature, const double* const hphi, const double conc,
    double& fl, double& fa)
{
    // std::cout<<"CALPHADFreeEnergyFunctionsBinary::computePhasesFreeEnergies()"<<endl;

    double c[2] = { conc, conc };

    // evaluate temperature dependent parameters
    CalphadDataType fA[2];
    CalphadDataType fB[2];

    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    computeTdependentParameters(temperature, Lmix_L, Lmix_A, fA, fB);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CALPHADConcSolverBinary solver;
    solver.setup(conc, hphi[0], RTinv, Lmix_L, Lmix_A, fA, fB);
    int ret = solver.ComputeConcentration(
        c, newton_tol_, newton_maxits_, newton_alpha_);
    if (ret < 0)
    {
#if 0
        std::cerr << "ERROR in "
                     "CALPHADFreeEnergyFunctionsBinary::"
                     "computePhasesFreeEnergies()"
                     " ---"
                  << "conc=" << conc << ", hphi=" << hphi << std::endl;
        abort();
#endif
    }

    // assert(c[0] >= 0.);
    fl = computeFreeEnergy(temperature, &c[0], PhaseIndex::phaseL, false);

    // assert(c[1] >= 0.);
    fa = computeFreeEnergy(temperature, &c[1], PhaseIndex::phaseA, false);
}

//-----------------------------------------------------------------------

int CALPHADFreeEnergyFunctionsBinary::computePhaseConcentrations(
    const double temperature, const double* const conc, const double* const phi,
    double* x)
{
    // assert(x[0] >= 0.);
    // assert(x[1] >= 0.);
    // assert(x[0] <= 1.);
    // assert(x[1] <= 1.);

    const double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];

    computeTdependentParameters(temperature, Lmix_L, Lmix_A, fA, fB);

    const double hphi = interp_func(conc_interp_func_type_, phi[0]);

    // conc could be outside of [0.,1.] in a trial step
    double c0 = conc[0] >= 0. ? conc[0] : 0.;
    c0        = c0 <= 1. ? c0 : 1.;
    // solve system of equations to find (cl,cs) given c0 and hphi
    // x: initial guess and solution
    CALPHADConcSolverBinary solver;
    solver.setup(c0, hphi, RTinv, Lmix_L, Lmix_A, fA, fB);
    int ret = solver.ComputeConcentration(
        x, newton_tol_, newton_maxits_, newton_alpha_);
#if 0
    if (ret == -1)
    {
        std::cerr << "ERROR, "
                     "CALPHADFreeEnergyFunctionsBinary::"
                     "computePhaseConcentrations() "
                     "failed for conc="
                  << conc[0] << ", hphi=" << hphi[0] << std::endl;
        abort();
    }
#endif

    return ret;
}
#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp end declare target
#endif

//-----------------------------------------------------------------------

void CALPHADFreeEnergyFunctionsBinary::energyVsPhiAndC(const double temperature,
    const double* const ceq, const bool found_ceq, const double phi_well_scale,
    const int npts_phi, const int npts_c)
{
    std::cout << "CALPHADFreeEnergyFunctionsBinary::energyVsPhiAndC()..."
              << std::endl;

    double slopec = 0.;
    double fc0    = 0.;
    double fc1    = 0.;
    if (found_ceq)
    {
        // compute slope of f between equilibrium concentrations
        // to add slopec*conc to energy later on

        fc0    = computeFreeEnergy(temperature, &ceq[0], PhaseIndex::phaseL);
        fc1    = computeFreeEnergy(temperature, &ceq[1], PhaseIndex::phaseA);
        slopec = -(fc1 - fc0) / (ceq[1] - ceq[0]);
    }
    std::cout << std::setprecision(8) << "fc0: " << fc0 << "..."
              << ", fc1: " << fc1 << "..." << std::endl;
    std::cout << "CALPHADFreeEnergyFunctionsBinary: Use slope: " << slopec
              << "..." << std::endl;

    // reset cmin, cmax, deltac
    double cmin   = std::min(ceq[0], ceq[1]);
    double cmax   = std::max(ceq[0], ceq[1]);
    double dc     = cmax - cmin;
    cmin          = std::max(0.25 * cmin, cmin - 0.25 * dc);
    cmax          = std::min(1. - 0.25 * (1. - cmax), cmax + 0.25 * dc);
    cmax          = std::max(cmax, cmin + dc);
    double deltac = (cmax - cmin) / (npts_c - 1);

    std::ofstream tfile(fenergy_diag_filename_, std::ios::out);

    printEnergyVsPhiHeader(
        temperature, npts_phi, npts_c, cmin, cmax, slopec, tfile);

    for (int i = 0; i < npts_c; i++)
    {
        double conc = cmin + deltac * i;
        printEnergyVsPhi(
            &conc, temperature, phi_well_scale, npts_phi, slopec, tfile);
    }
}

// Print out free energy as a function of phase
// for given composition and temperature
// File format: ASCII VTK, readble with Visit
void CALPHADFreeEnergyFunctionsBinary::printEnergyVsPhiHeader(
    const double temperature, const int nphi, const int nc, const double cmin,
    const double cmax, const double slopec, std::ostream& os) const
{
    os << "# vtk DataFile Version 2.0" << std::endl;
    os << "Free energy + " << slopec << "*c [J/mol] at T=" << temperature
       << std::endl;
    os << "ASCII" << std::endl;
    os << "DATASET STRUCTURED_POINTS" << std::endl;

    os << "DIMENSIONS   " << nphi << " " << nc << " 1" << std::endl;
    double asp_ratio_c = (nc > 1) ? (cmax - cmin) / (nc - 1) : 1.;
    os << "ASPECT_RATIO " << 1. / (nphi - 1) << " " << asp_ratio_c << " 1."
       << std::endl;
    os << "ORIGIN        0. " << cmin << " 0." << std::endl;
    os << "POINT_DATA   " << nphi * nc << std::endl;
    os << "SCALARS energy float 1" << std::endl;
    os << "LOOKUP_TABLE default" << std::endl;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::printEnergyVsPhi(
    const double* const conc, const double temperature,
    const double phi_well_scale, const int npts, const double slopec,
    std::ostream& os)
{
    // std::cout << "CALPHADFreeEnergyFunctionsBinary::printEnergyVsPhi()..." <<
    // std::endl;
    const double dphi = 1.0 / (double)(npts - 1);

    // os << "# phi     f(phi)     for c=" << conc
    //           << " and T=" << temperature << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double phi = i * dphi;

        double e       = fchem(&phi, conc, temperature);
        const double w = phi_well_scale * well_func(phi);

        os << e + w + slopec * conc[0] << std::endl;
    }
    // os << std::endl;
}

//=======================================================================
// compute free energy in [J/mol]
double CALPHADFreeEnergyFunctionsBinary::fchem(
    const double* const phi, const double* const conc, const double temperature)
{
    const double hcphi = interp_func(conc_interp_func_type_, phi[0]);

    const double tol = 1.e-8;
    double fl        = 0.;
    double fa        = 0.;
    double fb        = 0.;
    if ((phi[0] > tol) & (phi[0] < (1. - tol)))
    {
        computePhasesFreeEnergies(temperature, &hcphi, conc[0], fl, fa);
    }
    else
    {
        if (phi[0] <= tol)
        {
            fl = computeFreeEnergy(temperature, conc, PhaseIndex::phaseL);
        }
        else
        {
            fa = computeFreeEnergy(temperature, conc, PhaseIndex::phaseA);
        }
    }

    const double hfphi = interp_func(energy_interp_func_type_, phi[0]);

    double e = (1.0 - hfphi) * fl + hfphi * fa;

    return e;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::printEnergyVsComposition(
    const double temperature, std::ostream& os, const int npts)
{
    const double dc = 1.0 / (double)(npts - 1);

    os << "#phi=0" << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double conc = i * dc;

        const double phi = 0.0;

        double e = fchem(&phi, &conc, temperature);
        os << conc << "\t" << e << std::endl;
    }
    os << std::endl << std::endl;

    os << "#phi=1" << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double conc = i * dc;

        const double phi = 1.0;

        double e = fchem(&phi, &conc, temperature);
        os << conc << "\t" << e << std::endl;
    }
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinary::preRunDiagnostics(
    const double T0, const double T1)
{
    std::ofstream os1("FlC0vsT.dat", std::ios::out);
    os1 << "#Species 0, Phase L" << std::endl;
    g_species_phaseL_[0].plotFofT(os1, T0, T1);

    std::ofstream os2("FlC1vsT.dat", std::ios::out);
    os2 << "#Species 1, Phase L" << std::endl;
    g_species_phaseL_[1].plotFofT(os2, T0, T1);

    std::ofstream os3("FsC0vsT.dat", std::ios::out);
    os3 << "#Species 0, Phase A" << std::endl;
    g_species_phaseA_[0].plotFofT(os3, T0, T1);

    std::ofstream os4("FsC1vsT.dat", std::ios::out);
    os4 << "#Species 1, Phase A" << std::endl;
    g_species_phaseA_[1].plotFofT(os4, T0, T1);
}
}


#include <iostream>                    // std::flush
#include <fstream>                     // std::ofstream
#include <string>                      // std::string
#include <tuple>                       // std::tuple
#include <vector>                      // std::vector
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // WAE_Encoder, WAE_Decoder, GAN_Discriminator
#include "dataloader.hpp"              // DataLoader::ImageFolderWithPaths
#include "visualizer.hpp"              // visualizer::graph

// Define Namespace
namespace po = boost::program_options;

// -------------------
// Validation Function
// -------------------
void valid(po::variables_map &vm, DataLoader::ImageFolderWithPaths &valid_dataloader, torch::Device &device, Loss &criterion, torch::nn::BCEWithLogitsLoss &criterion_GAN, WAE_Encoder &enc, WAE_Decoder &dec, GAN_Discriminator &dis, const size_t epoch, visualizer::graph &writer_rec, visualizer::graph &writer_gan, visualizer::graph &writer_dis){

    // (0) Initialization and Declaration
    size_t iteration;
    size_t mini_batch_size;
    float ave_rec_loss, total_rec_loss;
    float ave_enc_loss, total_enc_loss;
    float ave_dis_real_loss, total_dis_real_loss;
    float ave_dis_fake_loss, total_dis_fake_loss;
    std::ofstream ofs;
    std::tuple<torch::Tensor, std::vector<std::string>> mini_batch;
    torch::Tensor image, output, z_real, z_fake;
    torch::Tensor dis_real_out, dis_fake_out;
    torch::Tensor rec_loss, enc_loss, dis_real_loss, dis_fake_loss;
    torch::Tensor label_real, label_fake;

    // (1) Tensor Forward per Mini Batch
    torch::NoGradGuard no_grad;
    enc->eval();
    dec->eval();
    iteration = 0;
    total_rec_loss = 0.0; total_enc_loss = 0.0; total_dis_real_loss = 0.0; total_dis_fake_loss = 0.0;
    while (valid_dataloader(mini_batch)){

        image = std::get<0>(mini_batch).to(device);
        mini_batch_size = image.size(0);

        // (1.1) Set Target Label
        label_real = torch::full({(long int)mini_batch_size}, /*value=*/1.0, torch::TensorOptions().dtype(torch::kFloat)).to(device);
        label_fake = torch::full({(long int)mini_batch_size}, /*value=*/0.0, torch::TensorOptions().dtype(torch::kFloat)).to(device);

        // (1.2) Discriminator Loss
        z_real = torch::randn({(long int)mini_batch_size, (long int)vm["nz"].as<size_t>()}).to(device);
        z_fake = enc->forward(image);
        dis_real_out = dis->forward(z_real).view({-1});
        dis_fake_out = dis->forward(z_fake).view({-1});
        dis_real_loss = criterion_GAN(dis_real_out, label_real) * vm["Lambda"].as<float>();
        dis_fake_loss = criterion_GAN(dis_fake_out, label_fake) * vm["Lambda"].as<float>();

        // (1.3) Auto Encoder Loss
        output = dec->forward(z_fake);
        rec_loss = criterion(output, image);
        dis_fake_out = dis->forward(z_fake).view({-1});
        enc_loss = criterion_GAN(dis_fake_out, label_real) * vm["Lambda"].as<float>();

        // (1.4) Update Loss
        total_rec_loss += rec_loss.item<float>();
        total_enc_loss += enc_loss.item<float>();
        total_dis_real_loss += dis_real_loss.item<float>();
        total_dis_fake_loss += dis_fake_loss.item<float>();

        iteration++;
    }

    // (2) Calculate Average Loss
    ave_rec_loss = total_rec_loss / (float)iteration;
    ave_enc_loss = total_enc_loss / (float)iteration;
    ave_dis_real_loss = total_dis_real_loss / (float)iteration;
    ave_dis_fake_loss = total_dis_fake_loss / (float)iteration;

    // (3.1) Record Loss (Log)
    ofs.open("checkpoints/" + vm["dataset"].as<std::string>() + "/log/valid.txt", std::ios::app);
    ofs << "epoch:" << epoch << '/' << vm["epochs"].as<size_t>() << ' ' << std::flush;
    ofs << "rec:" << ave_rec_loss << ' ' << std::flush;
    ofs << "enc:" << ave_enc_loss << ' ' << std::flush;
    ofs << "D_Real:" << ave_dis_real_loss << ' ' << std::flush;
    ofs << "D_Fake:" << ave_dis_fake_loss << std::endl;
    ofs.close();

    // (3.2) Record Loss (Graph)
    writer_rec.plot(/*base=*/epoch, /*value=*/{ave_rec_loss});
    writer_gan.plot(/*base=*/epoch, /*value=*/{ave_enc_loss, ave_dis_real_loss + ave_dis_fake_loss});
    writer_dis.plot(/*base=*/epoch, /*value=*/{ave_dis_real_loss + ave_dis_fake_loss, ave_dis_real_loss, ave_dis_fake_loss});

    // End Processing
    return;

}


// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved.
//
// 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.

// Note: Do not print/log ondemand object.

#include "base/common.h"
#include "base/flags.h"
#include "base/log.h"
#include "kaldi/matrix/kaldi-matrix.h"
#include "kaldi/util/kaldi-io.h"
#include "utils/file_utils.h"
// #include "boost/json.hpp"
#include <boost/json/src.hpp>

DEFINE_string(json_file, "", "cmvn json file");
DEFINE_string(cmvn_write_path, "./cmvn.ark", "write cmvn");
DEFINE_bool(binary, true, "write cmvn in binary (true) or text(false)");

using namespace boost::json;  // from <boost/json.hpp>

int main(int argc, char* argv[]) {
    gflags::ParseCommandLineFlags(&argc, &argv, false);
    google::InitGoogleLogging(argv[0]);

    LOG(INFO) << "cmvn josn path: " << FLAGS_json_file;

    auto ifs = std::ifstream(FLAGS_json_file);
    std::string json_str = ppspeech::ReadFile2String(FLAGS_json_file);
    auto value = boost::json::parse(json_str);
    if (!value.is_object()) {
        LOG(ERROR) << "Input json file format error.";
    }

    for (auto obj : value.as_object()) {
        if (obj.key() == "mean_stat") {
            LOG(INFO) << "mean_stat:" << obj.value();
        }
        if (obj.key() == "var_stat") {
            LOG(INFO) << "var_stat: " << obj.value();
        }
        if (obj.key() == "frame_num") {
            LOG(INFO) << "frame_num: " << obj.value();
        }
    }

    boost::json::array mean_stat = value.at("mean_stat").as_array();
    std::vector<kaldi::BaseFloat> mean_stat_vec;
    for (auto it = mean_stat.begin(); it != mean_stat.end(); it++) {
        mean_stat_vec.push_back(it->as_double());
    }

    boost::json::array var_stat = value.at("var_stat").as_array();
    std::vector<kaldi::BaseFloat> var_stat_vec;
    for (auto it = var_stat.begin(); it != var_stat.end(); it++) {
        var_stat_vec.push_back(it->as_double());
    }

    kaldi::int32 frame_num = uint64_t(value.at("frame_num").as_int64());
    LOG(INFO) << "nframe: " << frame_num;

    size_t mean_size = mean_stat_vec.size();
    kaldi::Matrix<double> cmvn_stats(2, mean_size + 1);
    for (size_t idx = 0; idx < mean_size; ++idx) {
        cmvn_stats(0, idx) = mean_stat_vec[idx];
        cmvn_stats(1, idx) = var_stat_vec[idx];
    }
    cmvn_stats(0, mean_size) = frame_num;
    LOG(INFO) << cmvn_stats;

    kaldi::WriteKaldiObject(cmvn_stats, FLAGS_cmvn_write_path, FLAGS_binary);
    LOG(INFO) << "cmvn stats have write into: " << FLAGS_cmvn_write_path;
    LOG(INFO) << "Binary: " << FLAGS_binary;
    return 0;
}


#pragma once

/* Classical variable propagation domain */

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/support/stats.hpp>

#include <boost/optional.hpp>

namespace crab {
namespace domains {
/**
 *  Each variable is mapped to an element in this lattice:
 *
 *                 top
 *                  |
 * ...,Var_1,Var_2,...,Var_{n-1},Var_n,...
 *                  |
 *                bottom
 *
 * top means that it might not be a variable
 **/
template <typename VariableName> class variable_domain {
  boost::optional<VariableName> m_variable;
  bool m_is_bottom;
  using variable_t = variable_domain<VariableName>;
  variable_domain(bool is_bottom)
      : m_variable(boost::none), m_is_bottom(is_bottom) {}

public:
  variable_domain(VariableName c) : m_variable(c), m_is_bottom(false) {}

  static variable_t bottom() { return variable_domain(true); }

  static variable_t top() { return variable_domain(false); }

  // static variable_t singleton() { return variable_domain(VariableName(0)); }

  bool is_bottom() const { return m_is_bottom; }

  bool is_top() const { return (!is_bottom() && !m_variable); }

  bool is_variable() const { return m_variable != boost::none; }

  VariableName get_variable() const {
    assert(is_variable());
    return *m_variable;
  }

  /** Begin Lattice Operations **/

  /**
    Indicates either this <= v property based on lattice.

    @param v a variable object has the same type as current.
    @return true if <= holds; else false.
  */
  bool operator<=(const variable_t &v) const {
    if (is_bottom() || v.is_top()) {
      return true;
    } else if (v.is_bottom() || is_top()) {
      return false;
    } else {
      assert(is_variable());
      assert(v.is_variable());
      return get_variable() == v.get_variable();
    }
  }

  /**
    Indicates either this == v property based on lattice.
    Note top == top?

    @param v a variable object has the same type as current.
    @return true if == holds; else false.
  */
  bool operator==(const variable_t &v) const {
    return (m_is_bottom == v.m_is_bottom && m_variable == v.m_variable);
  }

  /**
    Indicates either this | v property based on lattice.
    bot | v = v; this | bot = this
    if this == v then this | v = this
    otherwise return top

    @param v a variable object has the same type as current.
    @return what we described above.
  */
  variable_t operator|(const variable_t &v) const {
    if (is_bottom() || v.is_top())
      return v;
    else if (is_top() || v.is_bottom())
      return *this;
    else {
      assert(is_variable());
      assert(v.is_variable());
      if (get_variable() == v.get_variable()) {
        return *this;
      } else {
        return variable_t::top();
      }
    }
  }

  variable_t operator||(const variable_t &v) const { return *this | v; }

  template <typename Thresholds>
  variable_t widening_thresholds(const variable_t &v,
                                 const Thresholds &ts /*unused*/) const {
    return *this | v;
    // Q: how we handle widening with threshold
  }

  /**
    Indicates either this & v property based on lattice.
    this & top = this; top & v = v
    if this == v then this & v = this
    otherwise return bot

    @param v a variable object has the same type as current.
    @return what we described above.
  */
  variable_t operator&(const variable_t &v) const {
    if (is_bottom() || v.is_top())
      return *this;
    else if (is_top() || v.is_bottom()) {
      return v;
    } else {
      assert(is_variable());
      assert(v.is_variable());
      if (get_variable() == v.get_variable()) {
        return *this;
      } else {
        return variable_t::bottom();
      }
    }
  }

  variable_t operator&&(const variable_t &v) const { return *this & v; }
  /** End  Lattice Operations **/

  /** Begin arithmetic operations **/
  // The following operations -- op
  // Var_i op Var_j = top
  variable_t Add(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t Sub(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t Mul(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t SDiv(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t SRem(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t UDiv(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t URem(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t Arith(crab::domains::arith_operation_t op,
                   const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  /** End arithmetic operations **/

  /** Begin bitwise operations **/
  // These operations depend on the type of Number
  variable_t BitwiseAnd(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseOr(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseXor(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseShl(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseLShr(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseAShr(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  variable_t BitwiseOp(const variable_t &v) const {
    if (is_bottom() || v.is_bottom()) {
      return variable_t::bottom();
    } else {
      return variable_t::top();
    }
  }
  /** End bitwise operations **/

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
    } else if (is_top()) {
      o << "top";
    } else {
      assert(is_variable());
      o << get_variable();
    }
  }

  // void rename(const variable_vector_t &from,
  //             const variable_vector_t &to) {
  //   if (!is_variable()) {
  //     return;
  //   }
  //   for (int i = 0; i < from.size(); ++i) {
  //     auto tmp = from[i].name();
  //     if (tmp == m_variable) {
  //       m_variable = to[i].name();
  //       break;
  //     }
  //   }
  // }

  // void forget(const variable_vector_t &variables) {
  //   if (is_bottom() || is_top()) {
  //     return;
  //   }
  //   for (auto const &var : variables) {
  //     auto tmp = from[i].name();
  //     if (m_variable == tmp) {
  //       m_variable = boost::none;
  //       m_is_bottom = true;
  //       break;
  //     }
  //   }
  // }

  friend inline crab_os &operator<<(crab_os &o, const variable_t &v) {
    v.write(o);
    return o;
  }
};

// template <typename VariableName>
// class variable_domain final
//     : public
//     crab::domains::abstract_domain_api<variable_domain<VariableName>> {
// public:
//   using variable_domain_t = variable_domain<VariableName>;
//   using abstract_domain_t =
//       crab::domains::abstract_domain_api<variable_domain_t>;
//   using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
//   using typename abstract_domain_t::interval_t;
//   using typename abstract_domain_t::linear_constraint_system_t;
//   using typename abstract_domain_t::linear_constraint_t;
//   using typename abstract_domain_t::linear_expression_t;
//   using typename abstract_domain_t::reference_constraint_t;
//   using typename abstract_domain_t::variable_or_constant_t;
//   using typename abstract_domain_t::variable_or_constant_vector_t;
//   using typename abstract_domain_t::variable_t;
//   using typename abstract_domain_t::variable_vector_t;
//   using variable_value_t = variable<VariableName>;
//   using varname_t = VariableName;

// private:
//   using separate_domain_t = ikos::separate_domain<variable_t,
//   variable_value_t>;

// private:
//   separate_domain_t m_env;

//   variable_domain(separate_domain_t &&env) : m_env(std::move(env)) {}

//   variable_value_t eval(const linear_expression_t &expr) const {
//     assert(!is_bottom());
//     variable_value_t r(expr.constant());
//     for (auto const &kv : expr) {
//       variable_value_t c(kv.first);
//       r = r.Add(c.Mul(m_env[kv.second]));
//       if (r.is_top()) {
//         break;
//       }
//     }
//     return r;
//   }

//   constant_t compute_residual(const linear_constraint_t &cst,
//                               const variable_value_t &pivot) const {
//     constant_t residual(cst.constant());
//     for (auto const &kv : cst) {
//       constant_t c(kv.first);
//       const variable_value_t &v = kv.second;
//       if (!(v == pivot)) {
//         residual = residual.Sub(c.Mul(m_env[v]));
//         if (residual.is_top()) {
//           break;
//         }
//       }
//     }
//     return residual;
//   }

//   void propagate(const linear_constraint_t &cst) {
//     if (is_bottom()) {
//       return;
//     }

//     if (cst.is_inequality() || cst.is_strict_inequality() ||
//         cst.is_disequation()) {
//       constant_t e = eval(cst.expression());
//       if (e.is_constant()) {
//         if (cst.is_inequality()) {
//           if (!(e.get_constant() <= number_t(0))) {
//             set_to_bottom();
//             return;
//           }
//         } else if (cst.is_disequation()) {
//           if (!(e.get_constant() != number_t(0))) {
//             set_to_bottom();
//             return;
//           }
//         } else if (cst.is_strict_inequality()) {
//           if (!(e.get_constant() < number_t(0))) {
//             set_to_bottom();
//             return;
//           }
//         }
//       }
//     } else if (cst.is_equality()) {
//       for (auto kv : cst) {
//         number_t c = kv.first;
//         const variable_t &pivot = kv.second;
//         constant_t new_c = compute_residual(cst, pivot).SDiv(c);
//         if (!new_c.is_top()) {
//           m_env.set(pivot, m_env[pivot] & new_c);
//         }
//       }
//     }
//   }

//   void solve_constraints(const linear_constraint_system_t &csts) {
//     for (auto const &c : csts) {
//       if (is_bottom()) {
//         return;
//       }
//       if (c.is_inequality() && c.is_unsigned()) {
//         // we don't handle unsigned constraints
//         continue;
//       }
//       if (c.is_tautology()) {
//         continue;
//       }
//       if (c.is_contradiction()) {
//         set_to_bottom();
//         return;
//       }
//       propagate(c);
//     }
//   }

// public:
//   variable_domain_t make_top() const override {
//     return variable_domain_t(separate_domain_t::top());
//   }

//   variable_domain_t make_bottom() const override {
//     return variable_domain_t(separate_domain_t::bottom());
//   }

//   void set_to_top() override {
//     variable_domain abs(separate_domain_t::top());
//     std::swap(*this, abs);
//   }

//   void set_to_bottom() override {
//     variable_domain abs(separate_domain_t::bottom());
//     std::swap(*this, abs);
//   }

//   variable_domain() : m_env(separate_domain_t::top()) {}

//   variable_domain(const variable_domain_t &e) : m_env(e.m_env) {
//     crab::CrabStats::count(domain_name() + ".count.copy");
//     crab::ScopedCrabStats __st__(domain_name() + ".copy");
//   }

//   variable_domain(variable_domain_t &&e) : m_env(std::move(e.m_env)) {}

//   variable_domain_t &operator=(const variable_domain_t &v) {
//     crab::CrabStats::count(domain_name() + ".count.copy");
//     crab::ScopedCrabStats __st__(domain_name() + ".copy");
//     if (this != &v) {
//       m_env = v.m_env;
//     }
//     return *this;
//   }

//   variable_domain_t &operator=(variable_domain_t &&v) {
//     if (this != &v) {
//       m_env = std::move(v.m_env);
//     }
//     return *this;
//   }

//   variable_value_t get_variable_value(const variable_t &v) const {
//     return m_env[v];
//   }

//   void set_variable_value(const variable_t &v, variable_value_t v_val) {
//     m_env.set(v, v_val);
//   }

//   bool is_bottom() const override { return m_env.is_bottom(); }

//   bool is_top() const override { return m_env.is_top(); }

//   bool operator<=(const variable_domain_t &v) const override {
//     crab::CrabStats::count(domain_name() + ".count.leq");
//     crab::ScopedCrabStats __st__(domain_name() + ".leq");
//     return (m_env <= v.m_env);
//   }

//   void operator|=(const variable_domain_t &v) override {
//     crab::CrabStats::count(domain_name() + ".count.join");
//     crab::ScopedCrabStats __st__(domain_name() + ".join");
//     CRAB_LOG("constant-domain",
//              crab::outs() << "Join " << m_env << " and " << v.m_env <<
//              "\n";);
//     m_env = m_env | v.m_env;
//     CRAB_LOG("constant-domain", crab::outs() << "Res=" << m_env << "\n";);
//   }

//   variable_domain_t operator|(const variable_domain_t &v) const override {
//     crab::CrabStats::count(domain_name() + ".count.join");
//     crab::ScopedCrabStats __st__(domain_name() + ".join");
//     return (m_env | v.m_env);
//   }

//   variable_domain_t operator&(const variable_domain_t &v) const override {
//     crab::CrabStats::count(domain_name() + ".count.meet");
//     crab::ScopedCrabStats __st__(domain_name() + ".meet");
//     return (m_env & v.m_env);
//   }

//   variable_domain_t operator||(const variable_domain_t &v) const override {
//     crab::CrabStats::count(domain_name() + ".count.widening");
//     crab::ScopedCrabStats __st__(domain_name() + ".widening");
//     return (m_env || v.m_env);
//   }

//   variable_domain_t widening_thresholds(
//       const variable_domain_t &v,
//       const crab::iterators::thresholds<number_t> &ts) const override {
//     crab::CrabStats::count(domain_name() + ".count.widening");
//     crab::ScopedCrabStats __st__(domain_name() + ".widening");
//     return m_env.widening_thresholds(v.m_env, ts);
//   }

//   variable_domain_t operator&&(const variable_domain_t &v) const override {
//     crab::CrabStats::count(domain_name() + ".count.narrowing");
//     crab::ScopedCrabStats __st__(domain_name() + ".narrowing");
//     return (m_env && v.m_env);
//   }

//   void operator-=(const variable_t &v) override {
//     crab::CrabStats::count(domain_name() + ".count.forget");
//     crab::ScopedCrabStats __st__(domain_name() + ".forget");
//     m_env -= v;
//   }

//   interval_t operator[](const variable_t &v) override {
//     variable_value_t val = m_env[v];
//     if (val.is_bottom()) {
//       return interval_t::bottom();
//     } else if (val.is_top()) {
//       return interval_t::top();
//     } else {
//       assert(val.is_variable());
//       return interval_t(val.get_variable());
//     }
//   }

//   // // TODO: update this
//   // void operator+=(const linear_constraint_system_t &csts) override {
//   //   crab::CrabStats::count(domain_name() + ".count.add_constraints");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");
//   //   solve_constraints(csts);
//   // }

//   // // TODO: update this
//   // void assign(const variable_t &x, const linear_expression_t &e) override
//   {
//   //   crab::CrabStats::count(domain_name() + ".count.assign");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".assign");
//   //   if (boost::optional<variable_t> v = e.get_variable()) {
//   //     m_env.set(x, m_env[(*v)]);
//   //   } else {
//   //     m_env.set(x, eval(e));
//   //   }
//   // }

//   // void apply(crab::domains::arith_operation_t op, const variable_t &x,
//   //            const variable_t &y, const variable_t &z) override {
//   //   crab::CrabStats::count(domain_name() + ".count.apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".apply");

//   //   if (!is_bottom()) {
//   //     variable_value_t yc = m_env[y];
//   //     variable_value_t zc = m_env[z];
//   //     variable_value_t xc = variable_value_t::top();
//   //     xc = yc.Arith(op, zc);
//   //     m_env.set(x, xc);
//   //   }
//   // }

//   // void apply(crab::domains::arith_operation_t op, const variable_t &x,
//   //            const variable_t &y, number_t k) override {
//   //   crab::CrabStats::count(domain_name() + ".count.apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".apply");

//   //   // Var_i op num(k) should not be applied
//   //   CRAB_ERROR("Operation ", op, " not supported for variable domain");

//   //   if (!is_bottom()) {
//   //     variable_value_t xc = variable_value_t::top();
//   //     m_env.set(x, xc);
//   //   }
//   // }

//   // intrinsics operations
//   void intrinsic(std::string name, const variable_or_constant_vector_t
//   &inputs,
//                  const variable_vector_t &outputs) override {
//     CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
//   }

//   // void backward_intrinsic(std::string name,
//   //                         const variable_or_constant_vector_t &inputs,
//   //                         const variable_vector_t &outputs,
//   //                         const constant_domain_t &invariant) override {
//   //   CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
//   // }

//   // // backward arithmetic operations
//   // void backward_assign(const variable_t &x, const linear_expression_t &e,
//   //                      const constant_domain_t &inv) override {
//   //   crab::CrabStats::count(domain_name() + ".count.backward_assign");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".backward_assign");
//   //   // TODO
//   // }

//   // void backward_apply(crab::domains::arith_operation_t op, const
//   variable_t &x,
//   //                     const variable_t &y, number_t z,
//   //                     const constant_domain_t &inv) override {
//   //   crab::CrabStats::count(domain_name() + ".count.backward_apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
//   //   // TODO
//   // }

//   // void backward_apply(crab::domains::arith_operation_t op, const
//   variable_t &x,
//   //                     const variable_t &y, const variable_t &z,
//   //                     const constant_domain_t &inv) override {
//   //   crab::CrabStats::count(domain_name() + ".count.backward_apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
//   //   // TODO
//   // }

//   // cast operations
//   // void apply(crab::domains::int_conv_operation_t /*op*/, const variable_t
//   &dst,
//   //            const variable_t &src) override {
//   //   // ignore the widths
//   //   assign(dst, src);
//   // }

//   // bitwise operations
//   // void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
//   //            const variable_t &y, const variable_t &z) override {
//   //   crab::CrabStats::count(domain_name() + ".count.apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".apply");

//   //   if (!is_bottom()) {
//   //     variable_value_t yc = m_env[y];
//   //     variable_value_t zc = m_env[z];
//   //     variable_value_t xc = variable_value_t::top();
//   //     xc = yc.BitwiseOp(zc);
//   //     m_env.set(x, xc);
//   //   }
//   // }

//   // void apply(crab::domains::bitwise_operation_t op, const variable_t &x,
//   //            const variable_t &y, number_t k) override {
//   //   crab::CrabStats::count(domain_name() + ".count.apply");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".apply");

//   //   // Var_i op num(k) should not be applied
//   //   CRAB_ERROR("Operation ", op, " not supported for variable domain");

//   //   if (!is_bottom()) {
//   //     variable_value_t xc = variable_value_t::top();
//   //     m_env.set(x, xc);
//   //   }
//   // }

//   // virtual void select(const variable_t &lhs, const linear_constraint_t
//   &cond,
//   //                     const linear_expression_t &e1,
//   //                     const linear_expression_t &e2) override {
//   //   crab::CrabStats::count(domain_name() + ".count.select");
//   //   crab::ScopedCrabStats __st__(domain_name() + ".select");

//   //   if (!is_bottom()) {
//   //     constant_domain_t inv1(*this);
//   //     inv1 += cond;
//   //     if (inv1.is_bottom()) {
//   //       assign(lhs, e2);
//   //       return;
//   //     }
//   //     constant_domain_t inv2(*this);
//   //     inv2 += cond.negate();
//   //     if (inv2.is_bottom()) {
//   //       assign(lhs, e1);
//   //       return;
//   //     }
//   //     m_env.set(lhs, eval(e1) | eval(e2));
//   //   }
//   // }

//   /// variable_domain_t implements only standard abstract operations of
//   /// a equality domain so it is intended to be used as a leaf domain
//   /// in the hierarchy of domains.
//   NUMERICAL_OPERATIONS_NOT_IMPLEMENTED(variable_domain_t)
//   BOOL_OPERATIONS_NOT_IMPLEMENTED(variable_domain_t)
//   ARRAY_OPERATIONS_NOT_IMPLEMENTED(variable_domain_t)
//   REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(variable_domain_t)

//   void forget(const variable_vector_t &variables) override {
//     if (is_bottom() || is_top()) {
//       return;
//     }
//     for (auto const &var : variables) {
//       this->operator-=(var);
//     }
//   }

//   void project(const variable_vector_t &variables) override {
//     crab::CrabStats::count(domain_name() + ".count.project");
//     crab::ScopedCrabStats __st__(domain_name() + ".project");

//     m_env.project(variables);
//   }

//   void rename(const variable_vector_t &from,
//               const variable_vector_t &to) override {
//     crab::CrabStats::count(domain_name() + ".count.rename");
//     crab::ScopedCrabStats __st__(domain_name() + ".rename");

//     m_env.rename(from, to);
//   }

//   void expand(const variable_t &x, const variable_t &new_x) override {
//     crab::CrabStats::count(domain_name() + ".count.expand");
//     crab::ScopedCrabStats __st__(domain_name() + ".expand");

//     if (is_bottom() || is_top()) {
//       return;
//     }

//     m_env.set(new_x, m_env[x]);
//   }

//   void normalize() override {}

//   void minimize() override {}

//   void write(crab::crab_os &o) const override {
//     crab::CrabStats::count(domain_name() + ".count.write");
//     crab::ScopedCrabStats __st__(domain_name() + ".write");

//     m_env.write(o);
//   }

//   linear_constraint_system_t to_linear_constraint_system() const override {
//     crab::CrabStats::count(domain_name() +
//                            ".count.to_linear_constraint_system");
//     crab::ScopedCrabStats __st__(domain_name() +
//                                  ".to_linear_constraint_system");

//     linear_constraint_system_t csts;

//     if (this->is_bottom()) {
//       csts += linear_constraint_t::get_false();
//       return csts;
//     }

//     for (auto it = m_env.begin(); it != m_env.end(); ++it) {
//       const variable_t &v = it->first;
//       const variable_value_t &val = it->second;
//       if (val.is_variable()) {
//         csts += linear_constraint_t(v == val.get_variable());
//       }
//     }
//     return csts;
//   }

//   disjunctive_linear_constraint_system_t
//   to_disjunctive_linear_constraint_system() const override {
//     auto lin_csts = to_linear_constraint_system();
//     if (lin_csts.is_false()) {
//       return disjunctive_linear_constraint_system_t(true /*is_false*/);
//     } else if (lin_csts.is_true()) {
//       return disjunctive_linear_constraint_system_t(false /*is_false*/);
//     } else {
//       return disjunctive_linear_constraint_system_t(lin_csts);
//     }
//   }

//   std::string domain_name() const override { return "VariableDomain"; }

// }; // class variable_domain
} // namespace domains
} // namespace crab

// namespace crab {
// namespace domains {
// template <typename VariableName>
// struct abstract_domain_traits<variable_domain<VariableName>> {
//   using number_t = Number;
//   using varname_t = VariableName;
// };

// } // namespace domains
// } // namespace crab