#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <ctime>
#include <algorithm>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <ros/ros.h>
#include <ros/package.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"

#include <object_slam/Object_landmark.h>
#include <object_slam/Frame.h>
#include <object_slam/g2o_Object.h>

#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/detect_3d_cuboid.h"

#include "line_lbd/line_lbd_allclass.h"

using namespace std;
using namespace Eigen;

typedef pcl::PointCloud<pcl::PointXYZRGB> CloudXYZRGB;

#define CV_LOAD_IMAGE_ANYDEPTH cv::IMREAD_ANYDEPTH

// global variable
std::string base_folder;
bool online_detect_mode;
bool save_results_to_txt;
cv::Mat_<float> matx_to3d_, maty_to3d_;

// for converting depth image to point cloud.
void set_up_calibration(const Eigen::Matrix3f& calibration_mat,const int im_height,const int im_width)
{
    matx_to3d_.create(im_height, im_width);
    maty_to3d_.create(im_height, im_width);
    float center_x=calibration_mat(0,2);  //cx
    float center_y=calibration_mat(1,2);  //cy
    float fx_inv=1.0/calibration_mat(0,0);  // 1/fx
    float fy_inv=1.0/calibration_mat(1,1);  // 1/fy
    for (int v = 0; v < im_height; v++) {
	for (int u = 0; u < im_width; u++) {
	  matx_to3d_(v,u) = (u - center_x) * fx_inv;
	  maty_to3d_(v,u) = (v - center_y) * fy_inv;
	}
    }
}

// depth img is already in m unit.
void depth_to_cloud(const cv::Mat& rgb_img, const cv::Mat& depth_img,const Eigen::Matrix4f transToWorld, CloudXYZRGB::Ptr& point_cloud,bool downsample=false)
{
    pcl::PointXYZRGB pt;
    pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> vox_grid_;
    float close_depth_thre = 0.1;
    float far_depth_thre = 3.0;
      far_depth_thre = 3;
    int im_width = rgb_img.cols; int im_height= rgb_img.rows;
    for (int32_t i=0; i<im_width*im_height; i++) {      // row by row
	int ux=i % im_width; int uy=i / im_width;
	float pix_depth= depth_img.at<float>(uy,ux);
	if (pix_depth>close_depth_thre && pix_depth<far_depth_thre){
	      pt.z=pix_depth; pt.x=matx_to3d_(uy,ux)*pix_depth; pt.y=maty_to3d_(uy,ux)*pix_depth;
	      Eigen::VectorXf global_pt=homo_to_real_coord_vec<float>(transToWorld*Eigen::Vector4f(pt.x,pt.y,pt.z,1));  // change to global position
	      pt.x=global_pt(0); pt.y=global_pt(1); pt.z=global_pt(2);
	      pt.r = rgb_img.at<cv::Vec3b>(uy,ux)[2]; pt.g = rgb_img.at<cv::Vec3b>(uy,ux)[1]; pt.b = rgb_img.at<cv::Vec3b>(uy,ux)[0];
	      point_cloud->points.push_back(pt);
	}
    }
    if (downsample)
    {
	vox_grid_.setLeafSize(0.02,0.02,0.02);
	vox_grid_.setDownsampleAllData(true);
	vox_grid_.setInputCloud(point_cloud);
	vox_grid_.filter(*point_cloud);
    }
}

// one cuboid need front and back markers...
void cuboid_corner_to_marker(const Matrix38d& cube_corners,visualization_msgs::Marker& marker, int bodyOrfront)
{
    Eigen::VectorXd edge_pt_ids;
    if (bodyOrfront==0) { // body edges
	edge_pt_ids.resize(16); edge_pt_ids<<1,2,3,4,1,5,6,7,8,5,6,2,3,7,8,4;edge_pt_ids.array()-=1;
    }else { // front face edges
	edge_pt_ids.resize(5); edge_pt_ids<<1,2,6,5,1;edge_pt_ids.array()-=1;
    }
    marker.points.resize(edge_pt_ids.rows());
    for (int pt_id=0; pt_id<edge_pt_ids.rows(); pt_id++)
    {
	marker.points[pt_id].x = cube_corners(0, edge_pt_ids(pt_id));
	marker.points[pt_id].y = cube_corners(1, edge_pt_ids(pt_id));
	marker.points[pt_id].z = cube_corners(2, edge_pt_ids(pt_id));
    }
}

// one cuboid need front and back markers...  rgbcolor is 0-1 based
visualization_msgs::MarkerArray cuboids_to_marker(object_landmark* obj_landmark, Vector3d rgbcolor)
{
    visualization_msgs::MarkerArray plane_markers;  visualization_msgs::Marker marker;
    if (obj_landmark==nullptr)
	return plane_markers;

    marker.header.frame_id="/world";  marker.header.stamp=ros::Time::now();
    marker.id = 0; //0
    marker.type = visualization_msgs::Marker::LINE_STRIP;   marker.action = visualization_msgs::Marker::ADD;
    marker.color.r = rgbcolor(0); marker.color.g = rgbcolor(1); marker.color.b = rgbcolor(2); marker.color.a = 1.0;
    marker.scale.x = 0.02;

    g2o::cuboid cube_opti = obj_landmark->cube_vertex->estimate();
    Eigen::MatrixXd cube_corners = cube_opti.compute3D_BoxCorner();

    for (int ii=0;ii<2;ii++) // each cuboid needs two markers!!! one for all edges, one for front facing edge, could with different color.
    {
	marker.id++;
	cuboid_corner_to_marker(cube_corners,marker, ii);
	plane_markers.markers.push_back(marker);
    }
    return plane_markers;
}

geometry_msgs::Pose posenode_to_geomsgs(const g2o::SE3Quat &pose_Twc)
{
    geometry_msgs::Pose pose_msg;
    Eigen::Vector3d pose_trans = pose_Twc.translation();
    pose_msg.position.x=pose_trans(0);
    pose_msg.position.y=pose_trans(1);
    pose_msg.position.z=pose_trans(2);
    Eigen::Quaterniond pose_quat = pose_Twc.rotation();
    pose_msg.orientation.x = pose_quat.x();
    pose_msg.orientation.y = pose_quat.y();
    pose_msg.orientation.z = pose_quat.z();
    pose_msg.orientation.w = pose_quat.w();
    return pose_msg;
}

nav_msgs::Odometry posenode_to_odommsgs(const g2o::SE3Quat &pose_Twc,const std_msgs::Header &img_header)
{
    nav_msgs::Odometry odom_msg;
    odom_msg.pose.pose=posenode_to_geomsgs(pose_Twc);
    odom_msg.header=img_header;
    return odom_msg;
}

// publish every frame's raw and optimized results
void publish_all_poses(std::vector<tracking_frame*> all_frames,std::vector<object_landmark*> cube_landmarks_history,
		       std::vector<object_landmark*> all_frame_rawcubes, Eigen::MatrixXd& truth_frame_poses)
{
    ros::NodeHandle n;
    ros::Publisher pub_slam_all_poses = n.advertise<geometry_msgs::PoseArray>("/slam_pose_array", 10);
    ros::Publisher pub_slam_odompose = n.advertise<nav_msgs::Odometry>("/slam_odom_pose", 10);
    ros::Publisher pub_truth_all_poses = n.advertise<geometry_msgs::PoseArray>("/truth_pose_array", 10);
    ros::Publisher pub_truth_odompose = n.advertise<nav_msgs::Odometry>("/truth_odom_pose", 10);
    ros::Publisher pub_slam_path = n.advertise<nav_msgs::Path>( "/slam_pose_paths", 10 );
    ros::Publisher pub_truth_path = n.advertise<nav_msgs::Path>( "/truth_pose_paths", 10 );
    ros::Publisher pub_final_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti", 10); //final unique cube pose
    ros::Publisher pub_history_opti_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_opti_hist", 10); // landmark cube pose after each optimization
    ros::Publisher pub_frame_raw_cube = n.advertise<visualization_msgs::MarkerArray>("/cubes_raw_frame", 10);
    ros::Publisher pub_2d_cuboid_project = n.advertise<sensor_msgs::Image>("/cuboid_project_img", 10);
    ros::Publisher raw_cloud_pub = n.advertise<CloudXYZRGB> ("/raw_point_cloud", 50);

    int total_frame_number = all_frames.size();

    // prepare all paths messages.
    geometry_msgs::PoseArray all_pred_pose_array;    std::vector<nav_msgs::Odometry> all_pred_pose_odoms;
    geometry_msgs::PoseArray all_truth_pose_array;    std::vector<nav_msgs::Odometry> all_truth_pose_odoms;
    nav_msgs::Path path_truths,path_preds;
    std_msgs::Header pose_header;    pose_header.frame_id = "/world";    pose_header.stamp = ros::Time::now();
    path_preds.header = pose_header;    path_truths.header = pose_header;
    for (int i = 0; i < total_frame_number; i++)
    {
	all_pred_pose_array.poses.push_back(posenode_to_geomsgs(all_frames[i]->cam_pose_Twc));
	all_pred_pose_odoms.push_back(posenode_to_odommsgs(all_frames[i]->cam_pose_Twc,pose_header) );

	geometry_msgs::PoseStamped postamp;
	postamp.pose = posenode_to_geomsgs(all_frames[i]->cam_pose_Twc);
	postamp.header = pose_header;
	path_preds.poses.push_back(postamp);
    }
    if (truth_frame_poses.rows()>0)
    {
	for (int i=0; i < total_frame_number;i++)
	{
	    geometry_msgs::Pose pose_msg;
	    pose_msg.position.x=truth_frame_poses(i,1);    pose_msg.position.y=truth_frame_poses(i,2);    pose_msg.position.z=truth_frame_poses(i,3);
	    pose_msg.orientation.x = truth_frame_poses(i,4);	pose_msg.orientation.y = truth_frame_poses(i,5);
	    pose_msg.orientation.z = truth_frame_poses(i,6);	pose_msg.orientation.w = truth_frame_poses(i,7);
	    all_truth_pose_array.poses.push_back(pose_msg);
	    nav_msgs::Odometry odom_msg;odom_msg.pose.pose=pose_msg;
	    odom_msg.header = pose_header;
	    all_truth_pose_odoms.push_back(odom_msg);

	    geometry_msgs::PoseStamped postamp;
	    postamp.pose = pose_msg;
	    postamp.header = pose_header;
	    path_truths.poses.push_back(postamp);
	}
    }
    all_pred_pose_array.header.stamp=ros::Time::now();    all_pred_pose_array.header.frame_id="/world";
    all_truth_pose_array.header.stamp=ros::Time::now();    all_truth_pose_array.header.frame_id="/world";

    if (save_results_to_txt)  // record cam pose and object pose
    {
	ofstream resultsFile;
	string resultsPath = base_folder + "output_cam_poses.txt";
	cout<<"resultsPath  "<<resultsPath<<endl;
	resultsFile.open(resultsPath.c_str());
	resultsFile << "# timestamp tx ty tz qx qy qz qw"<<"\n";
	for (int i=0;i<total_frame_number;i++)
	{
	    double time_string=truth_frame_poses(i,0);
	    ros::Time time_img(time_string);
	    resultsFile << time_img<<"  ";
	    resultsFile << all_frames[i]->cam_pose_Twc.toVector().transpose()<<"\n";
	}
	resultsFile.close();

	ofstream objresultsFile;
	string objresultsPath = base_folder + "output_obj_poses.txt";
	objresultsFile.open(objresultsPath.c_str());
	for (size_t j=0;j<cube_landmarks_history.size();j++)
	{
	    g2o::cuboid cube_opti = cube_landmarks_history[j]->cube_vertex->estimate();
	    // transform it to local ground plane.... suitable for matlab processing.
	    objresultsFile << cube_opti.toMinimalVector().transpose()<<" "<<"\n";
	}
	objresultsFile.close();
    }

    // sensor parameter for TUM cabinet data!
    Eigen::Matrix3f calib;
    float depth_map_scaling = 5000;
    calib<<535.4,  0, 320.1,
	    0,  539.2, 247.6,
	    0,      0,     1;
    set_up_calibration(calib,480,640);

    visualization_msgs::MarkerArray finalcube_markers = cuboids_to_marker(cube_landmarks_history.back(),Vector3d(0,1,0));

    bool show_truth_cloud = true;  // show point cloud using camera pose. for visualization purpose

    pcl::PCLPointCloud2 pcd_cloud2;

    ros::Rate loop_rate(5);  //5
    int frame_number = -1;
    while ( n.ok() )
    {
	frame_number++;

	if (0) // directly show final results
	{
	    pub_slam_all_poses.publish(all_pred_pose_array);	pub_truth_all_poses.publish(all_truth_pose_array);
	    pub_slam_path.publish(path_preds);	pub_truth_path.publish(path_truths);
	}
	pub_final_opti_cube.publish(finalcube_markers);

	if (frame_number<total_frame_number)
	{
	    // publish cuboid landmarks, after each frame's g2o optimization
	    if (cube_landmarks_history[frame_number]!=nullptr)
		pub_history_opti_cube.publish(cuboids_to_marker(cube_landmarks_history[frame_number],Vector3d(1,0,0)));

	    // publish raw detected cube in each frame, before optimization
	    if (all_frame_rawcubes.size()>0 && all_frame_rawcubes[frame_number]!=nullptr)
		pub_frame_raw_cube.publish(cuboids_to_marker(all_frame_rawcubes[frame_number],Vector3d(0,0,1)));

	    // publish camera pose estimation of this frame
	    pub_slam_odompose.publish(all_pred_pose_odoms[frame_number]);
	    pub_truth_odompose.publish(all_truth_pose_odoms[frame_number]);

// 	    std::cout<<"Frame position x/y:   "<<frame_number<<"        "<<all_pred_pose_odoms[frame_number].pose.pose.position.x<<"  "<<
// 			  all_pred_pose_odoms[frame_number].pose.pose.position.y <<std::endl;

	    char frame_index_c[256];	sprintf(frame_index_c,"%04d",frame_number);  // format into 4 digit

	    cv::Mat cuboid_2d_proj_img = all_frames[frame_number]->cuboids_2d_img;

	    std::string raw_rgb_img_name = base_folder+"raw_imgs/" + std::string(frame_index_c) + "_rgb_raw.jpg";
	    cv::Mat raw_rgb_img = cv::imread(raw_rgb_img_name, 1);

	    if (show_truth_cloud && (truth_frame_poses.rows()>0))
		if (frame_number%2==0) // show point cloud every N frames
		{
		    std::string raw_depth_img_name = base_folder+"depth_imgs/" + std::string(frame_index_c) + "_depth_raw.png";
		    cv::Mat raw_depth_img = cv::imread(raw_depth_img_name, CV_LOAD_IMAGE_ANYDEPTH);
		    raw_depth_img.convertTo(raw_depth_img, CV_32FC1, 1.0/depth_map_scaling,0);

		    CloudXYZRGB::Ptr point_cloud(new CloudXYZRGB());
		    Eigen::Matrix4f truth_pose_matrix=g2o::SE3Quat(truth_frame_poses.row(frame_number).segment<7>(1)).to_homogeneous_matrix().cast<float>();

		    depth_to_cloud(raw_rgb_img, raw_depth_img, truth_pose_matrix, point_cloud, true); // need to downsample cloud, otherwise too many
		    ros::Time curr_time=ros::Time::now();

		    point_cloud->header.frame_id = "/world";
		    point_cloud->header.stamp = (curr_time.toNSec() / 1000ull);
		    raw_cloud_pub.publish(point_cloud);
		}

	    cv_bridge::CvImage out_image;
	    out_image.header.stamp=ros::Time::now();
	    out_image.image=cuboid_2d_proj_img;
	    out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC3;
	    pub_2d_cuboid_project.publish(out_image.toImageMsg());
	}

	if (frame_number==int(all_pred_pose_odoms.size()))
	{
	    cout<<"Finish all visulialization!"<<endl;
	}

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

//NOTE offline_pred_objects and init_frame_poses are not used in online_detect_mode! truth cam pose of first frame is used.
void incremental_build_graph(Eigen::MatrixXd& offline_pred_frame_objects, Eigen::MatrixXd& init_frame_poses, Eigen::MatrixXd& truth_frame_poses)
{
    Eigen::Matrix3d calib;
    calib<<535.4,  0,  320.1,   // for TUM cabinet data.
	    0,  539.2, 247.6,
	    0,      0,     1;

    int total_frame_number = truth_frame_poses.rows();

    // detect all frames' cuboids.
    detect_3d_cuboid detect_cuboid_obj;
    detect_cuboid_obj.whether_plot_detail_images = false;
    detect_cuboid_obj.whether_plot_final_images = false;
    detect_cuboid_obj.print_details = false;  // false  true
    detect_cuboid_obj.set_calibration(calib);
    detect_cuboid_obj.whether_sample_bbox_height = false;
    detect_cuboid_obj.nominal_skew_ratio = 2;
    detect_cuboid_obj.whether_save_final_images = true;

    line_lbd_detect line_lbd_obj;
    line_lbd_obj.use_LSD = true;
    line_lbd_obj.line_length_thres = 15;  // remove short edges

    // graph optimization.
    //NOTE in this example, there is only one object!!! perfect association
    g2o::SparseOptimizer graph;
    g2o::BlockSolverX::LinearSolverType* linearSolver;
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    graph.setAlgorithm(solver);    graph.setVerbose(false);

    // only first truth pose is used. to directly visually compare with truth pose. also provide good roll/pitch
    g2o::SE3Quat fixed_init_cam_pose_Twc(truth_frame_poses.row(0).tail<7>());

    // save optimization results of each frame
    std::vector<object_landmark*> cube_pose_opti_history(total_frame_number, nullptr);  //landmark pose after each frame's optimization
    std::vector<object_landmark*> cube_pose_raw_detected_history(total_frame_number, nullptr); //raw detected cuboid frame each frame. before optimization

    int offline_cube_obs_row_id = 0;

    std::vector<tracking_frame*> all_frames(total_frame_number);
    g2o::VertexCuboid* vCube;

    // process each frame online and incrementally
    for (int frame_index=0;frame_index<total_frame_number;frame_index++)
    {
	  g2o::SE3Quat curr_cam_pose_Twc;
	  g2o::SE3Quat odom_val; // from previous frame to current frame

	  if (frame_index==0)
		curr_cam_pose_Twc = fixed_init_cam_pose_Twc;
	  else
	  {
		g2o::SE3Quat prev_pose_Tcw = all_frames[frame_index-1]->cam_pose_Tcw;
		if (frame_index>1)  // from third frame, use constant motion model to initialize camera.
		{
		    g2o::SE3Quat prev_prev_pose_Tcw = all_frames[frame_index-2]->cam_pose_Tcw;
		    odom_val = prev_pose_Tcw*prev_prev_pose_Tcw.inverse();
		}
		curr_cam_pose_Twc = (odom_val*prev_pose_Tcw).inverse();
	  }

	  tracking_frame* currframe = new tracking_frame();
	  currframe->frame_seq_id = frame_index;
	  all_frames[frame_index] = currframe;

	  bool has_detected_cuboid = false;
	  g2o::cuboid cube_local_meas; double proposal_error;
	  char frame_index_c[256];	sprintf(frame_index_c,"%04d",frame_index);  // format into 4 digit

	  // read or detect cuboid
	  if (online_detect_mode)
	  {
	      //start detect cuboid
	      cv::Mat raw_rgb_img = cv::imread(base_folder+"raw_imgs/"+frame_index_c+"_rgb_raw.jpg", 1);

	      //edge detection
	      cv::Mat all_lines_mat;
	      line_lbd_obj.detect_filter_lines(raw_rgb_img, all_lines_mat);
	      Eigen::MatrixXd all_lines_raw(all_lines_mat.rows,4);
	      for (int rr=0;rr<all_lines_mat.rows;rr++)
		for (int cc=0;cc<4;cc++)
		  all_lines_raw(rr,cc) = all_lines_mat.at<float>(rr,cc);

	      //read cleaned yolo 2d object detection
	      Eigen::MatrixXd raw_2d_objs(10,5);  // 2d rect [x1 y1 width height], and prob
	      if (!read_all_number_txt(base_folder+"/filter_2d_obj_txts/"+frame_index_c+"_yolo2_0.15.txt", raw_2d_objs))
		  return;
	      raw_2d_objs.leftCols<2>().array() -=1;   // change matlab coordinate to c++, minus 1

	      Matrix4d transToWolrd;
	      detect_cuboid_obj.whether_sample_cam_roll_pitch = (frame_index!=0); // first frame doesn't need to sample cam pose. could also sample. doesn't matter much
	      if (detect_cuboid_obj.whether_sample_cam_roll_pitch) //sample around first frame's pose
		  transToWolrd = fixed_init_cam_pose_Twc.to_homogeneous_matrix();
	      else
		  transToWolrd = curr_cam_pose_Twc.to_homogeneous_matrix();

	      std::vector<ObjectSet> frames_cuboids; // each 2d bbox generates an ObjectSet, which is vector of sorted proposals
	      detect_cuboid_obj.detect_cuboid(raw_rgb_img,transToWolrd,raw_2d_objs,all_lines_raw, frames_cuboids);
	      currframe->cuboids_2d_img = detect_cuboid_obj.cuboids_2d_img;

	      has_detected_cuboid = frames_cuboids.size()>0 && frames_cuboids[0].size()>0;
	      if (has_detected_cuboid)  // prepare object measurement
	      {
		  cuboid* detected_cube = frames_cuboids[0][0];  // NOTE this is a simple dataset, only one landmark

		  g2o::cuboid cube_ground_value; //cuboid in the local ground frame.
		  Vector9d cube_pose;cube_pose<<detected_cube->pos(0),detected_cube->pos(1),detected_cube->pos(2),
			  0,0,detected_cube->rotY,detected_cube->scale(0),detected_cube->scale(1),detected_cube->scale(2);  // xyz roll pitch yaw scale
		  cube_ground_value.fromMinimalVector(cube_pose);
		  cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc); // measurement is in local camera frame

		  if (detect_cuboid_obj.whether_sample_cam_roll_pitch)  //if camera roll/pitch is sampled, transform to the correct camera frame.
		  {
		      Vector3d new_camera_eulers =  detect_cuboid_obj.cam_pose_raw.euler_angle;
		      new_camera_eulers(0) += detected_cube->camera_roll_delta; new_camera_eulers(1) += detected_cube->camera_pitch_delta;
		      Matrix3d rotation_new = euler_zyx_to_rot<double>(new_camera_eulers(0),new_camera_eulers(1),new_camera_eulers(2));
		      Vector3d trans = transToWolrd.col(3).head<3>();
		      g2o::SE3Quat curr_cam_pose_Twc_new(rotation_new,trans);
		      cube_local_meas = cube_ground_value.transform_to(curr_cam_pose_Twc_new);
		  }
		  proposal_error = detected_cube->normalized_error;
	      }
	  }
	  else
	  {
	      int cube_obs_frame_id = offline_pred_frame_objects(offline_cube_obs_row_id,0);
	      has_detected_cuboid = cube_obs_frame_id==frame_index;
	      if (has_detected_cuboid)  // prepare object measurement   not all frame has observation!!
	      {
		  VectorXd measure_data = offline_pred_frame_objects.row(offline_cube_obs_row_id);
		  g2o::cuboid cube_ground_value;
		  Vector9d cube_pose;cube_pose<<measure_data(1),measure_data(2),measure_data(3),0,0,measure_data(4),
						measure_data(5),measure_data(6),measure_data(7);  // xyz roll pitch yaw scale
		  cube_ground_value.fromMinimalVector(cube_pose);
		  Eigen::VectorXd cam_pose_vec = init_frame_poses.row(frame_index);
		  g2o::SE3Quat cam_val_Twc(cam_pose_vec.segment<7>(1)); // time x y z qx qy qz qw
		  cube_local_meas = cube_ground_value.transform_to(cam_val_Twc); // measurement is in local camera frame
		  proposal_error = measure_data(8);

		  // read offline saved 2d image
		  std::string detected_cube_2d_img_name = base_folder+"pred_3d_obj_overview/" + std::string(frame_index_c) + "_best_objects.jpg";
		  currframe->cuboids_2d_img = cv::imread(detected_cube_2d_img_name, 1);

		  offline_cube_obs_row_id++; // switch to next row  NOTE at most one object one frame in this data
	      }
	  }

	  if (has_detected_cuboid)
	  {
	      object_landmark* localcuboid = new object_landmark();

	      localcuboid->cube_meas = cube_local_meas;
	      localcuboid->meas_quality = (1-proposal_error+0.5)/2;  // initial error 0-1, higher worse,  now change to [0.5,1] higher, better
	      currframe->observed_cuboids.push_back(localcuboid);
	  }

	  // set up g2o cube vertex. only one in this dataset
	  if (frame_index==0)
	  {
	      g2o::cuboid init_cuboid_global_pose = cube_local_meas.transform_from(curr_cam_pose_Twc);
	      vCube = new g2o::VertexCuboid();
	      vCube->setEstimate(init_cuboid_global_pose);
	      vCube->setId(0);
	      vCube->setFixed(false);
	      graph.addVertex(vCube);
	  }

	  // set up g2o camera vertex
	  g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
	  currframe->pose_vertex = vSE3;
	  vSE3->setId(frame_index+1);
	  graph.addVertex(vSE3);

	  vSE3->setEstimate(curr_cam_pose_Twc.inverse()); //g2o vertex usually stores world to camera pose.
	  vSE3->setFixed(frame_index==0);

	  // add g2o camera-object measurement edges, if there is
	  if (currframe->observed_cuboids.size()>0)
	  {
		object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];
		g2o::EdgeSE3Cuboid* e = new g2o::EdgeSE3Cuboid();
		e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vSE3 ));
		e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( vCube ));
		e->setMeasurement(cube_landmark_meas->cube_meas);
		e->setId(frame_index);
		Vector9d inv_sigma;inv_sigma<<1,1,1,1,1,1,1,1,1;
		inv_sigma = inv_sigma*2.0*cube_landmark_meas->meas_quality;
		Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
		e->setInformation(info);
		graph.addEdge(e);
	  }

	  // camera vertex, add cam-cam odometry edges
	  if (frame_index>0)
	  {
		g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();
		e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index-1]->pose_vertex ));
		e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>( all_frames[frame_index]->pose_vertex ));
		e->setMeasurement(odom_val);

		e->setId(total_frame_number+frame_index);
		Vector6d inv_sigma;inv_sigma<<1,1,1,1,1,1;
		inv_sigma = inv_sigma*1.0;
		Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
		e->setInformation(info);
		graph.addEdge(e);
	  }
	  graph.initializeOptimization();
	  graph.optimize(5); // do optimization!

	  // retrieve the optimization result, for debug visualization
	  for (int j=0;j<=frame_index;j++)
	  {
		all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
		all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
	  }
	  object_landmark* current_landmark = new object_landmark();  current_landmark->cube_vertex = new g2o::VertexCuboid();
	  current_landmark->cube_vertex->setEstimate(vCube->estimate());
	  cube_pose_opti_history[frame_index] = current_landmark;

	  if (all_frames[frame_index]->observed_cuboids.size()>0)
	  {
	      object_landmark* cube_landmark_meas = all_frames[frame_index]->observed_cuboids[0];
	      g2o::cuboid local_cube = cube_landmark_meas->cube_meas;

	      g2o::cuboid global_cube = local_cube.transform_from(all_frames[frame_index]->cam_pose_Twc);
	      object_landmark* tempcuboids2 = new object_landmark();	  tempcuboids2->cube_vertex = new g2o::VertexCuboid();
	      tempcuboids2->cube_vertex->setEstimate(global_cube);
	      cube_pose_raw_detected_history[frame_index] = tempcuboids2;
	  }
	  else
	      cube_pose_raw_detected_history[frame_index] = nullptr;
    }

    cout<<"Finish all optimization! Begin visualization."<<endl;

    publish_all_poses(all_frames, cube_pose_opti_history,cube_pose_raw_detected_history,truth_frame_poses);
}

int main(int argc,char* argv[])
{
    ros::init(argc, argv, "object_slam");

    ros::NodeHandle nh;

    nh.param ("/base_folder", base_folder, ros::package::getPath("object_slam")+"/data/");
    nh.param ("/online_detect_mode", online_detect_mode, true);
    nh.param ("/save_results_to_txt", save_results_to_txt, false);

    cout<<""<<endl;
    cout<<"base_folder   "<<base_folder<<endl;
    if (online_detect_mode)
	ROS_WARN_STREAM("Online detect object mode !!\n");
    else
	ROS_WARN_STREAM("Offline read object mode !!\n");

    //NOTE important
    // in online mode, pred_frame_objects and init_frame_poses are not used. only first frame of truth_frame_poses is used.

    std::string pred_objs_txt = base_folder+"detect_cuboids_saved.txt";  // saved cuboids in local ground frame.
    std::string init_camera_pose = base_folder+"pop_cam_poses_saved.txt"; // offline camera pose for cuboids detection (x y yaw=0, truth roll/pitch/height)
    std::string truth_camera_pose = base_folder+"truth_cam_poses.txt";
    Eigen::MatrixXd pred_frame_objects(100,10);  // 100 is some large row number, each row in txt has 10 numbers
    Eigen::MatrixXd init_frame_poses(100,8);
    Eigen::MatrixXd truth_frame_poses(100,8);

    if (!read_all_number_txt(pred_objs_txt,pred_frame_objects))
	return -1;
    if (!read_all_number_txt(init_camera_pose,init_frame_poses))
	return -1;
    if (!read_all_number_txt(truth_camera_pose,truth_frame_poses))
	return -1;

    std::cout<<"read data size:  "<<pred_frame_objects.rows()<<"  "<<init_frame_poses.rows()<<"  "<<truth_frame_poses.rows()<<std::endl;

    incremental_build_graph(pred_frame_objects,init_frame_poses,truth_frame_poses);

    return 0;
}


/*
    Copyright (c) 2011 Alin Marin Elena <alinm.elena@gmail.com>

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

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

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

#include <cmath>
#include <iostream>

#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_01.hpp>

#include "solution.h"
#include "utils.h"
#include "reading.h"
#include "colours.h"
#include "gutenberg.h"
#include "collectivevariable.h"

using namespace std;
using namespace colours;

void solution::InitializeSimulation ( std::vector<atom>& a, const double& temp, const double& box, const int& seed, string const& filename , double const& gamma )
{
     InitializeSimulation ( a,temp,box,seed,filename );
     int k=0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).setGamma ( gamma );
          ( *i ).setID ( k );
          k++;
     }
}

void solution::InitializeSimulation ( std::vector<atom>& a, const double& temp, const double& box, const int& seed, string const& filename )
{
// equal spaced in the box
     boost::mt19937 igen ( seed );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >    rgauss ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     double x,y,z,fact;

     int k=reading::readXYZ ( filename.c_str(),a );
     for ( int i=0; i<a.size(); i++ ) {
          a[i].setMass ( 1.0 );
          a[i].setID ( i );
          fact=sqrt ( utils::Kb*temp/a[i].getMass() );
          a[i].setVelocities ( fact*rgauss(),fact*rgauss(),fact*rgauss() );
          a[i].setForces ( 0.0,0.0,0.0 );
     }

//remove the movement of the centre of mass
     centerOfMassV ( a );
     scaleVelocities ( a,temp );
}

void solution::RandomVelocities ( std::vector<atom>& a, const double& temp, const double& box, const int& seed )
{
// equal spaced in the box
     boost::mt19937 igen ( seed );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >    rgauss ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     double x,y,z,fact;

     for ( int i=0; i<a.size(); i++ ) {
          fact=sqrt ( utils::Kb*temp/a[i].getMass() );
          a[i].setVelocities ( fact*rgauss(),fact*rgauss(),fact*rgauss() );
          a[i].setForces ( 0.0,0.0,0.0 );
     }
//remove the movement of the centre of mass
     centerOfMassV ( a );
     scaleVelocities ( a,temp );
}

void solution::InitializeSimulation ( std::vector<atom>& a, string const& filename )
{
     int k=reading::readFull ( filename.c_str(),a );
     if ( k!=0 ) {
          utils::Print ( "Error!!!",cout,colours::red );
     }
}

void solution::GenerateLattice ( vector< atom >&a, const double&rho,
                                 const int&mass, const double&nAtoms, double& box, const string& el )
{

     double volume=nAtoms*mass/rho;
     box=pow ( volume,1.0/3.0 );
     int n=int ( pow ( nAtoms,1.0/3.0 ) );
     if ( n*n*n!=nAtoms ) n++;
     double h=box/ ( n );
     double x;
     double y;
     double z;
     int m=0;
     for ( int i=0; i<n; i++ ) {
          x=i*h;
          for ( int j=0; j<n; j++ ) {
               y=j*h;
               for ( int k=0; k<n; k++ ) {
                    z=k*h;
                    if ( m<nAtoms ) {
                         a.push_back ( atom() );
                         a[m].setElement ( el );
                         a[m].setPositions ( x,y,z );
                         a[m].setMass ( mass );
                         a[m].setID ( m );
                    }
                    m++;
               }
          }
     }

}

void solution::centerOfMassV ( vector< atom >& a )
{
     double vmx=0.0, vmy=0.0, vmz=0.0,mt=0.0;
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          vmx+= ( *it ).px();
          vmy+= ( *it ).py();
          vmz+= ( *it ).pz();
          mt+= ( *it ).getMass();
     }
     vmx=-vmx/mt;
     vmy=-vmy/mt;
     vmz=-vmz/mt;
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).addtoVelocities ( vmx,vmy,vmz );
     }
}

void solution::scaleVelocities ( vector< atom >& a, const double& T )
{
     double tx=0.0,ty=0.0, tz=0.0;
     if ( T>0.0 ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               tx+= ( *it ).getMass() * ( *it ).vx2();
               ty+= ( *it ).getMass() * ( *it ).vy2();
               tz+= ( *it ).getMass() * ( *it ).vz2();
          }
          tx=tx/ ( a.size() *utils::Kb ) ;
          ty=ty/ ( a.size() *utils::Kb ) ;
          tz=tz/ ( a.size() *utils::Kb ) ;
          tx=sqrt ( T/tx );
          ty=sqrt ( T/ty );
          tz=sqrt ( T/tz );
     }
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).scaleVelocity ( tx,ty,tz );
     }
}

double solution::computeForces ( vector< atom >& a, const double & rc, const double& box, double& vir )
{
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).setForces ( 0.0,0.0,0.0 );
     }

     double R,fR,fx,fy,fz,energy;
     vector<double> dR ( 3 );
     double ec=utils::LennardJones ( rc );
     energy=0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end()-1; i++ ) {
          for ( vector<atom>::iterator j=i+1; j<a.end(); j++ ) {
               R=rdr ( dR,*i,*j,box );
               if ( R<rc ) {
                    fR=utils::LennardJonesdR ( R );
                    fx=-fR*dR[0]/R;
                    fy=-fR*dR[1]/R;
                    fz=-fR*dR[2]/R;
                    ( *i ).addtoForces ( fx,fy,fz );
                    ( *j ).addtoForces ( -fx,-fy,-fz );
                    energy+=utils::LennardJones ( R )-ec;
                    vir+=fR*R;
               }
          }
     }
     return energy;
}

double solution::velocityVerlet ( vector< atom >& a, const double& rc, const double& box, const double& dt , double& kin, double &vir )
{
     kin=0.0;
     vir=0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVV ( dt );
          ( *i ).putInBox ( box );
     }
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVV ( dt );
     }
     double energy = solution::computeForces ( a,rc,box,vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVV ( dt );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }
     kin=kin/2.0;
     return energy;
}

void solution::MDDriverVV ( vector< atom >& a, generalInputs& in )
{

     double potEnergy, kinEnergy, vir,pressure;
     potEnergy = PotentialEnergy ( a,in.cut,in.box, in.shift,vir );
     kinEnergy = KineticEnergy ( a );
     double iT,tT;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     consQ=potEnergy+kinEnergy;
     iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
     tT=iT;
     gutenberg::mdHeader ( cout,in.fancy );
     gutenberg::mdRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );
     vector<double> avg ( 2 );
     int nsamp=0;
     for ( int k=0; k<avg.size(); k++ ) avg[k]=0.0;
     for ( int i=1; i<=in.nSteps; i++ ) {
          potEnergy = solution::velocityVerlet ( a,in.cut,in.box,in.dt,kinEnergy,vir );

          iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
          tT+= ( iT-tT ) / ( i+1.0 );

          if ( i%in.frequency==0 ) {
               gutenberg::printForces ( a,in.log );
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
               gutenberg::printAtoms ( a,in.debug );
          }
          consQ=potEnergy+kinEnergy;
          avg[0]+=consQ;
          avg[1]+=consQ*consQ;
          nsamp++;
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::mdRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,cout );
     }
     for ( int k=0; k<avg.size(); k++ ) avg[k]=avg[k]/nsamp;
     utils::Print ( "CV:",cout,red );
     utils::Print ( ( avg[1]-avg[0]*avg[0] ) /utils::Kb/tT/tT,cout, green );
     cout<<endl;

}

double solution::VECIntegrator ( vector< atom >& a, const double&rc , const double& box , const double& dt, const double& s1, const double& s2, double& kin, double& vir )
{

     kin=0.0;
     vir = 0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
     }

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVEC ( dt,s1 );
     }
     double energy = solution::computeForces ( a,rc,box, vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }
     kin=kin/2.0;
     return energy;
}

void solution::MDDriverVEC ( vector< atom >& a, generalInputs & in )
{
     //set the random generators...
     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     vector<double> b ( 6 );
     double potEnergy, kinEnergy, vir;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     kinEnergy = KineticEnergy ( a );

     double iT,tT;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     gutenberg::logLine ( "Volume: ",volume,in.log );
     gutenberg::logLine ( "Density: ",rho,in.log );
     consQ=potEnergy+kinEnergy;
     iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
     tT=iT;
     gutenberg::mdHeader ( cout,in.fancy );
     gutenberg::mdRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,in.log );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );

     double s1=sqrt ( utils::Kb*in.TBath*in.dt/6.0 ) *in.dt;
     double s2=sqrt ( 2.0*utils::Kb*in.TBath );
     double v2=0.0;

     vector<double> K,E,vx,vy,vz,P,v;
//     K.push_back ( kinEnergy/a.size() );
//     E.push_back ( consQ/a.size() );
//     P.push_back ( potEnergy/a.size() );
//     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
//         vx.push_back ( ( *it ).gvx() );
//         vy.push_back ( ( *it ).gvy() );
//         vz.push_back ( ( *it ).gvz() );
//         v.push_back ( sqrt ( ( *it ).v2() ) );
//         }
     double aene=0.0, aene2=0.0,cv;
     int nsamp=0;
     for ( int i=1; i<=in.nSteps; i++ ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               b[2]=xi();
               b[3]=xi();
               b[4]=xi();
               b[5]=xi();
               ( *it ).setNoise ( b );
          }
          potEnergy = solution::VECIntegrator ( a,in.cut,in.box,in.dt,s1,s2,kinEnergy,vir );

//         pressure=rho* ( utils::Kb*tT ) +vir/volume/3.0;
          gutenberg::mdRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,in.log );

          if ( i%in.frequency==0 )  {
//             gutenberg::printForces ( a,in.log );
//             gutenberg::printXYZ ( in.xyz,in.name,a,true );
//             gutenberg::printAtoms ( a,in.debug );
               iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );

               consQ=potEnergy+kinEnergy;
               if ( i == in.nEquil ) {
                    nsamp=0;
                    E.erase ( E.begin(),E.end() );
               }
               if ( nsamp==0 ) {

                    aene=consQ;
                    tT=iT;
                    nsamp=0;
               } else {
                    nsamp++;
                    aene+= ( aene-consQ ) / ( nsamp+1.0 );
                    tT+= ( iT-tT ) / ( nsamp+1.0 );
               }
               E.push_back ( consQ/a.size() );

               cv= utils::sigma2 ( E ) / ( utils::Kb*tT*tT );
               if ( i>in.nEquil ) {
                    gutenberg::logInfo ( "step ",i-in.nEquil,cout );

               } else {
                    gutenberg::logInfo ( "step ",i,cout );
               }
               gutenberg::logInfo ( "T ",tT,cout );
               gutenberg::logInfo ( "<E> ",aene,cout );
               gutenberg::logLine ( "Cv: ", cv*a.size(),cout );
               gutenberg::logLine ( "Cv/N: ", cv,cout );

               if ( i>in.nEquil ) {

                    P.push_back ( potEnergy/a.size() );
                    for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
                         vx.push_back ( ( *it ).gvx() );
                         vy.push_back ( ( *it ).gvy() );
                         vz.push_back ( ( *it ).gvz() );
                         v2= ( *it ).v2();
                         v.push_back ( sqrt ( v2 ) );
                         K.push_back ( v2* ( *it ).getMass() /2.0 );
                    }
               }
          }
     }

     cv=utils::sigma2 ( E );
     utils::Print ( "CV:",cout,red );
     utils::Print ( cv/ ( utils::Kb*tT*tT ) *a.size(),cout, green );
     cout<<endl;
     utils::Print ( "sigma^2 K",cout, red );
     utils::Print ( utils::sigma2 ( K ),cout,green );
     cout<<endl;
     utils::Print ( "sigma^2 E",cout, red );
     utils::Print ( cv,cout,green );
     cout<<endl;
     solution::histogramWrapper ( K,in.bin,"Kvec.hist" );
     solution::histogramWrapper ( E,in.bin,"Evec.hist" );
     solution::histogramWrapper ( P,in.bin,"Pvec.hist" );
     solution::histogramWrapper ( vx,in.bin,"vxvec.hist" );
     solution::histogramWrapper ( vy,in.bin,"vyvec.hist" );
     solution::histogramWrapper ( vz,in.bin,"vzvec.hist" );
     solution::histogramWrapper ( v,in.bin,"vvec.hist" );
}

double solution::computeForcesTAMD ( vector< atom >& a, vector<colVar>& cv,const double & rc, const double& box, double& vir )
{
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).setForces ( 0.0,0.0,0.0 );
     }

     double R,fR,fx,fy,fz,energy;
     vector<double> dR ( 3 );
     double ec=utils::LennardJones ( rc );
     int k=0;
     energy=0.0;
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).force ( a,box );
     }
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          fx=0.0;
          fy=0.0;
          fz=0.0;
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               ( *it ).dtheta ( dR,k,a,box );
               fx=fx+ ( *it ).f*dR[0];
               fy=fy+ ( *it ).f*dR[1];
               fz=fz+ ( *it ).f*dR[2];
          }
          k++;
          ( *i ).addtoForces ( -fx,-fy,-fz );
          for ( vector<atom>::iterator j=i+1; j<a.end(); j++ ) {
               R=rdr ( dR,*i,*j,box );
               if ( R<rc ) {
                    fR=utils::LennardJonesdR ( R );
                    fx=-fR*dR[0]/R;
                    fy=-fR*dR[1]/R;
                    fz=-fR*dR[2]/R;
                    ( *i ).addtoForces ( fx,fy,fz );
                    ( *j ).addtoForces ( -fx,-fy,-fz );
                    energy+=utils::LennardJones ( R )-ec;
                    vir+=fR*R;
               }
          }
     }
     return energy;
}

double solution::TAMDIntegrator ( vector< atom >& a, vector<colVar>&cv, const double&rc , const double& box , const double& dt, const double& s1,const double& c1, const double& s2, double& kin, double&kinz, double&vir )
{

     kin=0.0;
     kinz=0.0;
     vir =0.0;

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
     }
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezvVEC ( dt );
     }

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVEC ( dt,s1 );
     }
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezpVEC ( dt,c1 );
     }
     double energy = solution::computeForcesTAMD ( a,cv,rc,box, vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }

     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezvVEC ( dt );
          kinz+= ( *i ).v* ( *i ).v* ( *i ).mu;
     }

     kin=kin/2,0;
     kinz=kinz/2.0;
     return energy;
}

///
///  @brief the driver for TAMD
///  @details http://dx.doi.org/10.1016/j.cplett.2006.05.062
///   it uses the VEC integrator for EOM
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAMD ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     vector<double> b ( 6 );
     double potEnergy, kinEnergy, vir;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     kinEnergy = KineticEnergy ( a );

     double iT,tT;
     double iTz,tTz;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     consQ=potEnergy+kinEnergy;
     iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );
     tT=iT;
     iTz=0.0;
     tTz=iTz;
     double kinz=0.0;
     gutenberg::mdTAMDHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMDRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),tT,tTz,pressure,kinz/cv.size(),consQ/a.size(),VCM ( a ),cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );

     double s1=sqrt ( utils::Kb*in.TBath*in.dt/6.0 ) *in.dt;
     double c1=sqrt ( utils::Kb*in.Tcv*in.dt/6.0 ) *in.dt;
     double s2=sqrt ( 2.0*utils::Kb*in.TBath );

     vector<double> KX,KZ,vxX,vyX,vyZ,vX,vzX,vZ;
     double v2;
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int i=1; i<=in.nSteps; i++ ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               b[2]=xi();
               b[3]=xi();
               b[4]=xi();
               b[5]=xi();
               ( *it ).setNoise ( b );
          }
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          potEnergy = solution::TAMDIntegrator ( a,cv,in.cut,in.box,in.dt,s1,c1,s2,kinEnergy, kinz,vir );

          iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );
          tT+= ( iT-tT ) / ( i+1.0 );

          iTz=2.0*kinz/ ( cv.size() *utils::Kb );
          tTz+= ( iTz-tTz ) / ( i+1.0 );
          if ( i%in.frequency==0 && i>in.nEquil ) {
               gutenberg::printForces ( a,in.log );
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
               gutenberg::printAtoms ( a,in.debug );
               for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
                    vxX.push_back ( ( *it ).gvx() );
                    vyX.push_back ( ( *it ).gvy() );
                    vzX.push_back ( ( *it ).gvz() );
                    v2= ( *it ).v2();
                    vX.push_back ( sqrt ( v2 ) );
                    KX.push_back ( v2* ( *it ).getMass() /2.0 );
               }
               for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                    vZ.push_back ( ( *it ).v );
                    KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
               }
          }
          consQ=potEnergy+kinEnergy;
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::mdTAMDRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),tT,tTz,pressure,kinz/cv.size(),consQ/a.size(),VCM ( a ),cv,in.fancy,cout );
     }
     solution::histogramWrapper ( vxX,in.bin,"vxX.hist" );
     solution::histogramWrapper ( vyX,in.bin,"vyX.hist" );
     solution::histogramWrapper ( vzX,in.bin,"vzX.hist" );
     solution::histogramWrapper ( vX,in.bin,"vX.hist" );
     solution::histogramWrapper ( KX,in.bin,"KX.hist" );
     solution::histogramWrapper ( vZ,in.bin,"vZ.hist" );
     solution::histogramWrapper ( KZ,in.bin,"KZ.hist" );
}

void solution::optimalDr ( double& d , int& otry, int& ogood, const int& itry, const int& igood, const double& box )
{
     if ( itry!=otry ) {
          double f= ( double ) ( igood-ogood ) / ( double ) ( itry-otry );
          double od=d;
          d=d*f/0.5; //0.5 is the target accteptance ratio;
          if ( d/od>1.5 ) d=od*1.5;
          if ( d/od<0.5 ) d=od*0.5;
          if ( d>box/2 ) d=box/2;
          otry=itry;
          ogood=igood;
     }

}

///
///  @brief the driver for a metropolis montecarlo simulation
///  @param a a vector of atoms
///  @param in contains the input parameters
///
void solution::MMCDriver ( vector< atom >& a, generalInputs& in )
{
     boost::mt19937 igen ( in.seeds[0] );
     boost::uniform_01<boost::mt19937> rn ( igen );

     double ei,ef;
     int rAtom;
     double rx,ry,rz;

     int itry=0;
     int igood=0;
     int otry, ogood;
     otry=itry;
     ogood=igood;
     int nsamp=0;
     double fact;
     double et,vt;
     double aene=0.0, ap=0.0,aene2=0.0;
     double rho=density ( a,in.box );
     double volume=in.box*in.box*in.box;
     double vir, viri,virf;
     double etot=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     gutenberg::logLine ( "Volume: ",volume,in.log );
     gutenberg::logLine ( "Density: ",rho,in.log );
     utils::Print ( etot,cout,32,16 );
     cout<<endl;

     vector<double>ene;

     double pressure;
     int cycles;
     int startsamp;
     pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     gutenberg::MMCHeader ( cout, in.fancy );
     gutenberg::MMCRepLine ( 0,etot/a.size(),pressure,in.fancy,cout );
     gutenberg::logLine ( "Initial etot ", etot,cout );
     gutenberg::logLine ( "Initial etot/N ", etot/a.size(),cout );
     gutenberg::logLine ( "Initial virial ", vir,cout );
     solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
     for ( int stage=0; stage<2; stage++ ) {
          if ( stage==0 ) {
               cycles=in.nEquil;
               startsamp=cycles/10;
               utils::Print ( "Equilibration stage:", cout, blue );
               cout<<endl;
          } else {
               startsamp=0;
               cycles=in.mccycles;
               utils::Print ( "Production stage:", cout, blue );
               cout <<endl;
               etot=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
               ene.push_back ( etot/a.size() );
               gutenberg::MMCHeader ( cout, in.fancy );
               gutenberg::MMCRepLine ( 0,etot/a.size(),pressure,in.fancy,cout );
          }
          gutenberg::logInfo ( "old dr:",in.dr,cout );
          solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
          gutenberg::logLine ( "new dr:",in.dr,cout );
          itry=0;
          igood=0;
          aene=0.0;
          aene2=0.0;
          ap=0.0;
          nsamp=0;
          for ( int i=0; i<cycles; i++ ) {
               // one should compute the optimal dr here...
               for ( int j=0; j<in.mcsteps; j++ ) {
                    itry++;
                    rAtom=int ( rn() *a.size() );
                    ei=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,viri );
                    a[rAtom].saveOldPositions ( rx,ry,rz );
                    a[rAtom].randomMove ( in.dr,rn(),rn(),rn() );
                    ef=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,virf );
                    fact=- ( ef-ei ) / ( utils::Kb*in.TBath );
                    if ( rn() <exp ( fact ) ) {
                         igood++;
                         etot=etot+ef-ei;
                         vir=vir+virf-viri;
                         a[rAtom].putInBox ( in.box );
                    } else { //if rejected
                         a[rAtom].setPositions ( rx,ry,rz );
                    }
               }
               if ( ( ( i+1 ) %in.frequency==0 ) && ( i+1>startsamp ) ) {
                    //sampling point...
                    pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
                    gutenberg::MMCRepLine ( ( i+1 ) *in.mcsteps,etot/a.size(),pressure,in.fancy,in.log );
//                 gutenberg::printXYZ ( in.xyz,in.name,a,true );
                    ene.push_back ( etot/a.size() );
                    aene+=etot;
                    aene2+=etot*etot;
                    ap+=pressure;
                    nsamp++;

                    gutenberg::logInfo ( "|cycle: ",i+1,in.log );
                    gutenberg::logInfo ( "|attempts: ",itry, in.log );
                    gutenberg::logInfo ( "|accepted: ",igood, in.log );
                    gutenberg::logLine ( "|ratio", ( double ) ( igood ) / ( double ) ( itry ),in.log );
               }
               if ( ( i+1 ) % ( cycles/5 ) ==0 ) {
                    gutenberg::logInfo ( "|cycle: ",i+1,cout );
                    gutenberg::logInfo ( "|attempts: ",itry-otry, cout );
                    gutenberg::logInfo ( "|accepted: ",igood-ogood, cout );
                    gutenberg::logLine ( "|ratio", ( double ) ( igood-ogood ) / ( double ) ( itry-otry ),cout );
                    gutenberg::logInfo ( "old dr:",in.dr,cout );
                    solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
                    gutenberg::logInfo ( "new dr:",in.dr,cout );
                    cout<<endl;
               }

          }
          gutenberg::logLine ( "Etot: ",etot,cout );
          gutenberg::logLine ( "Erot/N",etot/a.size(),cout );
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::logLine ( "virial: ",vir,cout );
          gutenberg::logLine ( "pressure: ",pressure,cout );
          et=PotentialEnergy ( a,in.cut,in.box,in.shift,vt );
          gutenberg::logLine ( "diff energy: ",et-etot,cout );
          gutenberg::logLine ( "diff virial: ", vt-vir,cout );
          gutenberg::logInfo ( "|cycle: ",cycles,cout );
          gutenberg::logInfo ( "|attempts: ",itry, cout );
          gutenberg::logInfo ( "|accepted: ",igood, cout );
          gutenberg::logLine ( "|ratio", ( double ) ( igood ) / ( double ) ( itry ),cout );
          aene2=aene2/nsamp;
          aene=aene/nsamp;
          gutenberg::logLine ( "Avg energy:", aene,cout );
          gutenberg::logLine ( "Avg energy/N:", aene/a.size(),cout );
          gutenberg::logLine ( "Avg pressure:", ap/nsamp,cout );
          gutenberg::logLine ( "CV: ",1.5*utils::Kb*a.size() + ( aene2-aene*aene ) / ( utils::Kb*in.TBath*in.TBath ),cout );
     }

     solution::histogramWrapper ( ene,in.bin,"Pmmc.hist" );

}

///
///  @brief the driver for TAMC
///  @details TAMC paper
///   it uses the VEC integrator for EOM
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAMC ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     boost::uniform_01<boost::mt19937> rn ( igen );
     vector<double> b ( 2 );
     double potEnergy, vir,fact,etot;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir ) +zpot ( a,cv,in.box );

     double iT=0.0,tT=0.0;
     double consQ,kin;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     int itry=0, otry=0,igood=0, ogood=0;
     double ei,ef, viri,virf;
     int rAtom;
     double rx,ry,rz;
     double kinz=0.0;

     gutenberg::mdTAMCHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMCRepLine ( 0.0,potEnergy/a.size(),tT,pressure,kinz,cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );

     double c1=sqrt ( utils::Kb*in.Tcv*in.dt/6.0 ) *in.dt;

     double aene=0.0, ap=0.0;
     int nsamp=0;
     vector<double> avgfz ( cv.size() ), KZ,vZ;
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int step=1; step<=in.nSteps; step++ ) {
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          vir =0.0;
          aene=0.0;
          ap=0.0;
          for ( int k=0; k<cv.size(); k++ ) avgfz[k]=0.0;
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dt );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezpVEC ( in.dt,c1 );
          }
          //steps MC

          for ( int i=0; i<in.mccycles; i++ ) {
               for ( int j=0; j<in.mcsteps; j++ ) {
                    itry++;
                    rAtom=int ( rn() *a.size() );
                    ei=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,viri ) +zpot ( a,cv,in.box );
                    a[rAtom].saveOldPositions ( rx,ry,rz );
                    a[rAtom].randomMove ( in.dr,rn(),rn(),rn() );
                    a[rAtom].putInBox ( in.box );
                    ef=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,virf ) +zpot ( a,cv,in.box );
                    fact=- ( ef-ei ) / ( utils::Kb*in.TBath );
                    if ( rn() <exp ( fact ) ) {
                         igood++;
                         etot=etot+ef-ei;
                         vir=vir+virf-viri;
                    } else { //if rejected
                         a[rAtom].setPositions ( rx,ry,rz );
                    }
               }
               //sampling point...

               pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
               aene+=etot/a.size();
               ap+=pressure;
               nsamp++;
               for ( int k=0; k<cv.size(); k++ ) {
                    cv[k].force ( a,in.box );
                    avgfz[k]+=cv[k].f;
               }
               solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
          }
          for ( int k=0; k<cv.size(); k++ ) {
               cv[k].f=avgfz[k]/nsamp;
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dt );
          }

          if ( step%in.frequency==0 ) {
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
          }
          kinz=zKin ( cv );

          iT=2.0*kinz/ ( cv.size() *utils::Kb );
          if ( step>=in.nEquil ) {
               tT+= ( iT-tT ) / ( step-in.nEquil+1.0 );
               if ( step%in.frequency==0 ) {
                    for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                         vZ.push_back ( ( *it ).v );
                         KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
                    }
               }
          }
          gutenberg::mdTAMCRepLine ( step*in.dt,aene/nsamp,tT,pressure/nsamp,kinz,cv,in.fancy,cout );
          nsamp=0;
     }

     solution::histogramWrapper ( vZ,in.bin,"vZtdmc.hist" );
     solution::histogramWrapper ( KZ,in.bin,"KZtdmc.hist" );

}

///
///  @brief the driver for TAHMC
///  @details TAMC paper and HMC paper
///   it uses the VEC integrator for EOM and velocity verlet for atoms
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAHMC ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     boost::uniform_01<boost::mt19937> rn ( igen );
     vector<double> b ( 2 );
     double aPot, vir,fact,etot;
     double etoti,etotf;

     aPot = PotentialEnergy ( a,in.cut,in.box,in.shift,vir ) + zpot ( a,cv,in.box );

     double iT=0.0,tT=0.0;
     double consQ,kin;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     int itry=0, otry=0,igood=0, ogood=0;
     double zpoti,zpotf, viri,virf;
     int irseed;

     double kinz=0.0;
     double kini,kinf;
     vector<double> pos ( 3* a.size() );
     cout << pos.size() <<" "<<a.size() <<endl;
     for ( int i=0; i<a.size(); i++ ) {
          a[i].saveOldPositions ( pos[3*i],pos[3*i+1],pos[3*i+2] );
     }
     gutenberg::mdTAMCHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMCRepLine ( 0.0,aPot/a.size(),tT,pressure,kinz,cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );

     double c1=sqrt ( utils::Kb*in.Tcv*in.dtz/6.0 ) *in.dt;

     double aene=0.0, ap=0.0,acc;
     int nsamp;
     vector<double> avgfz ( cv.size() ), KZ,vZ;
     irseed=rn();
     solution::zHMCSampler ( a, cv, in, irseed,pos, avgfz );
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int step=1; step<=in.Nz; step++ ) {
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dtz );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezpVEC ( in.dtz,c1 );
          }
          //steps HMC
          irseed=rn();
          solution::zHMCSampler ( a, cv, in, irseed,pos, avgfz );
//         solution::zSample

          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dtz );
          }
          if ( step%in.frequency==0 ) {
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
          }
          kinz=zKin ( cv );

          iT=2.0*kinz/ ( cv.size() *utils::Kb );

          tT+= ( iT-tT ) / ( step+1.0 );
          if ( step%in.frequency==0 ) {
               for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                    vZ.push_back ( ( *it ).v );
                    KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
               }
          }

          gutenberg::mdTAMCRepLine ( step*in.dt,aene/nsamp,tT,pressure/nsamp,kinz,cv,in.fancy,cout );
          nsamp=0;
     }

//     solution::histogramWrapper ( vZ,in.bin,"vZtdmc.hist" );
//     solution::histogramWrapper ( KZ,in.bin,"KZtdmc.hist" );

}

void solution::zHMCSampler ( vector< atom >& a, vector<colVar>& cv, generalInputs & in, const int& irseed,
                             vector<double>& pos, vector<double>& avgfz )
{

     boost::mt19937 igen ( irseed );
     boost::uniform_01<boost::mt19937> rn ( igen );

     double epoti,epotf,zpoti,zpotf,kini,kinf;
     int igood,iseed,nsamp;
     double aHg,aH,dHg,fact,vir,acc;

     aHg=0.0;
     aH=0.0;
     nsamp=0;
     for ( int k=0; k<cv.size(); k++ ) avgfz[k]=0.0;
     epoti=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     zpoti=zpot ( a,cv,in.box );
     igood=0;
     for ( int i=0; i<in.mccycles; i++ ) {
          iseed=int ( rn() *10000 );
          utils::Print ( "Start Mini-Trajectory ",cout,colours::green );
          utils::Print ( i,cout,colours::green );
          cout <<endl;

          solution::RandomVelocities ( a, in.TBath, in.box,iseed );
          kini=KineticEnergy ( a );
          solution::MDDriverVV ( a,in );
          epotf=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
          zpotf=zpot ( a,cv,in.box );
          kinf=KineticEnergy ( a );
          fact=- ( epotf-epoti+zpotf-zpoti+kinf-kini ) / ( utils::Kb*in.TBath );
          dHg=- ( epotf-epoti+kinf-kini ) / ( utils::Kb*in.TBath );
          if ( rn() <exp ( fact ) ) {
               igood++;
               epoti=epotf;
               zpoti=zpotf;
               for ( int j=0; j<a.size(); j++ ) {
                    a[j].saveOldPositions ( pos[3*j],pos[3*j+1],pos[3*j+2] );
               }
          } else { //if rejected
               for ( int j=0; j<a.size(); j++ ) {
                    a[j].setPositions ( pos[3*j],pos[3*j+1],pos[3*j+2] );
               }
          }
          utils::Print ( "End Mini-Trajectory ",cout,colours::green );
          utils::Print ( i,cout,colours::green );
          utils::Print ( fact,cout,colours::green );
          cout <<endl;
          if ( ( i%in.frequency==0 ) && ( i>=in.nEquil ) ) {
               nsamp++;
               for ( int k=0; k<cv.size(); k++ ) {
                    cv[k].force ( a,in.box );
                    avgfz[k]+=cv[k].f;
               }
               aHg+= ( exp ( dHg )-aHg ) / ( i-in.nEquil+1.0 );
               aH+= ( exp ( fact )-aH ) / ( i-in.nEquil+1.0 );
               utils::Print ( "runing averages ",cout,colours::red );
               utils::Print ( aHg,cout,colours::red );
               utils::Print ( aH,cout,colours::red );
               cout<<endl;
          }
     }
     utils::Print ( "runing averages ",cout,colours::red );
     utils::Print ( aHg,cout,colours::red );
     utils::Print ( aH,cout,colours::red );
     acc= ( double ) igood/ ( double ) in.mccycles;
     utils::Print ( "acceptance ",cout,colours::green );
     utils::Print ( acc,cout,colours::green );
     cout <<endl;

     for ( int k=0; k<cv.size(); k++ ) {
          cv[k].f=avgfz[k]/nsamp;
     }

}

void solution::histogramWrapper ( std::vector< double >& a, const double& bin, const string& filename )
{

     vector<double> aHist;
     double amin;
     utils::histogram ( a,bin,aHist,amin );
     utils::NormaliseHistogram ( aHist,bin );
     gutenberg::printHistogram ( aHist,amin,bin,filename );

}

// kate: indent-mode cstyle; indent-width 5; replace-tabs on;