#include "simulation/generic-path-generator.h"

#include <Eigen/Dense>
#include <glog/logging.h>
#include <maplab-common/gravity-provider.h>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>

#include <simulation/eigen-visualization.h>

// Basic usage examples:
// rosrun simulation simulation_generic // Generates circular trajectory with
// default parameters.
// rosrun simulation simulation_generic -mode 1 // Same as previous line.
// rosrun simulation simulation_generic -mode 2 // Generates elliptic trajectory
// with default parameters.
// rosrun simulation simulation_generic -mode 2 -kappa 0.1 -lambda 0.2
// rosrun simulation simulation_generic -mode 3 // Generate rotation-only path
// rosrun simulation simulation_generic -mode 4 // Generate translation-only
// path
// rosrun simulation simulation_generic -mode 5 -filename_waypoints
// "/tmp/waypoints_translation_only.txt"
// rosrun simulation simulation_generic -mode 5 -filename_waypoints
// "/tmp/waypoints_rotation_only.txt"

// gflags: DEFINE_type(name, default value, help)
// Prefix tg_* stands for "trajectory generator".
DEFINE_bool(tg_visualization, true, "Publish visualization topics for RVIZ.");
DEFINE_double(
    tg_acc_bias_sigma_meter_by_sqrt_second5, 0.1,
    "Accelerometer noise bias standard deviation");
DEFINE_double(
    tg_acc_sigma_meter_by_sqrt_second2, 0.1,
    "Accelerometer noise standard deviation");
DEFINE_double(tg_circle_radius_meter, 10.0, "Circle radius");
DEFINE_double(tg_distance_to_keypoints_meter, 5.0, "Distance to keypoints");
DEFINE_double(
    tg_gyro_bias_sigma_rad_by_sqrt_second3, 0.1,
    "Gyroscope bias noise standard deviation");
DEFINE_double(
    tg_gyro_sigma_rad_by_sqrt_second, 0.1,
    "Gyroscope noise standard deviation");
DEFINE_double(
    tg_kappa, 0.1,
    "Kappa defining the ellipse shape according to x = (1.0 / kappa) "
    "* cos(t).");
DEFINE_double(
    tg_lambda, 0.05,
    "Lambda defining the ellipse shape according to y = (1.0 / "
    "lambda) * sin(t).");
DEFINE_double(
    tg_landmark_offset_x_meter, 50.0, "Landmark volume offset in x direction");
DEFINE_double(
    tg_landmark_offset_y_meter, 0.0, "Landmark volume offset in y direction");
DEFINE_double(
    tg_landmark_offset_z_meter, 0.5, "Landmark volume offset in z direction");
DEFINE_double(tg_landmark_variance_meter, 3.0, "Landmark variance");
DEFINE_double(
    tg_landmark_volume_x_meter, 10.0,
    "Landmark volume in x direction, centered around offset");
DEFINE_double(
    tg_landmark_volume_y_meter, 10.0,
    "Landmark volume in y direction, centered around offset");
DEFINE_double(
    tg_landmark_volume_z_meter, 5.0,
    "Landmark volume in z direction, centered around offset");
DEFINE_double(tg_num_of_rounds, 1.0, "Number of rounds");
DEFINE_double(tg_sampling_time_second, 0.1, "Sampling time");
DEFINE_double(
    tg_start_offset_x_meter, 0.0, "Offset from the origin in x direction");
DEFINE_double(
    tg_start_offset_y_meter, 0.0, "Offset from the origin in y direction");
DEFINE_double(tg_start_offset_yaw_radians, 0.0, "Yaw start offset");
DEFINE_double(
    tg_start_offset_z_meter, 0.0,
    "Offset from the origin in z direction. This is the constant height.");
DEFINE_double(
    tg_vertical_to_radial_variance_factor, 2.0,
    "Ratio between radial and vertical standard deviation of the "
    "keypoint positions.");
DEFINE_int32(tg_imu_noise_bias_seed, 10, "IMU noise bias seed");
DEFINE_int32(tg_landmark_seed, 5, "Landmark seed");
DEFINE_int32(
    tg_mode, 1,
    "Trajectory mode: [1] circle, [2] ellipse, [3] rotation, [4] translation, "
    "[5] from file");
DEFINE_int32(tg_num_of_path_constraints, 8, "Number of path constraints");
DEFINE_int32(tg_sub_sampling_factor, 5, "Sub-sampling factor");
DEFINE_int32(tg_num_of_landmarks, 1000, "Number of landmarks");
DEFINE_string(
    tg_filename_waypoints, "waypoints_translation_only.txt",
    "List with waypoints: x y z yaw");
DEFINE_string(
    tg_visualization_frame_id, "/map",
    "Visualization frame id of the simulation");
DEFINE_string(
    tg_visualization_topic, "path", "Visualization topic of the simulation");

void insertMarkers(
    const visualization_msgs::MarkerArray& to_insert,
    visualization_msgs::MarkerArray* m) {
  CHECK_NOTNULL(m);
  m->markers.insert(
      m->markers.end(), to_insert.markers.begin(), to_insert.markers.end());
}

void setUp(test_trajectory_gen::PathAndLandmarkSettings* settings) {
  CHECK_NOTNULL(settings);
  // Retrieve Zurich's gravity.
  common::GravityProvider gravity_provider(
      common::locations::kAltitudeZurichMeters,
      common::locations::kLatitudeZurichDegrees);
  // Accelerometer noise bias standard deviation.
  settings->imu_sigmas.acc_bias_random_walk_noise_density =
      FLAGS_tg_acc_bias_sigma_meter_by_sqrt_second5;
  // Accelerometer noise standard deviation.
  settings->imu_sigmas.acc_noise_density =
      FLAGS_tg_acc_sigma_meter_by_sqrt_second2;
  // Circle radius.
  settings->circle_radius_meter = FLAGS_tg_circle_radius_meter;
  // Distance to keypoints.
  settings->distance_to_keypoints_meter = FLAGS_tg_distance_to_keypoints_meter;
  // Gravity magnitude.
  settings->gravity_meter_by_second2 = gravity_provider.getGravityMagnitude();
  // Gyroscope bias noise standard deviation.
  settings->imu_sigmas.gyro_bias_random_walk_noise_density =
      FLAGS_tg_gyro_bias_sigma_rad_by_sqrt_second3;
  // Gyroscope noise standard deviation.
  settings->imu_sigmas.gyro_noise_density =
      FLAGS_tg_gyro_sigma_rad_by_sqrt_second;
  // Kappa defining the ellipse shape according to
  // x = (1.0 / kappa) * cos(t).
  settings->kappa = FLAGS_tg_kappa;
  // Lambda defining the ellipse shape according to
  // y = (1.0 / lambda) * sin(t).
  settings->lambda = FLAGS_tg_lambda;
  // Landmark volume offset in x direction.
  settings->landmark_offset_x_meter = FLAGS_tg_landmark_offset_x_meter;
  // Landmark volume offset in y direction.
  settings->landmark_offset_y_meter = FLAGS_tg_landmark_offset_y_meter;
  // Landmark volume offset in z direction.
  settings->landmark_offset_z_meter = FLAGS_tg_landmark_offset_z_meter;
  // Landmark variance.
  settings->landmark_variance_meter = FLAGS_tg_landmark_variance_meter;
  // Landmark volume in x direction, centered around offset.
  settings->landmark_volume_x_meter = FLAGS_tg_landmark_volume_x_meter;
  // Landmark volume in y direction, centered around offset.
  settings->landmark_volume_y_meter = FLAGS_tg_landmark_volume_y_meter;
  // Landmark volume in z direction, centered around offset.
  settings->landmark_volume_z_meter = FLAGS_tg_landmark_volume_z_meter;
  // Number of rounds. Currently only applies for elliptical mode.
  settings->num_of_rounds = FLAGS_tg_num_of_rounds;
  // Sampling time in seconds.
  settings->sampling_time_second = FLAGS_tg_sampling_time_second;
  // Offset from the origin in x direction.
  settings->start_offset_x_meter = FLAGS_tg_start_offset_x_meter;
  // Offset from the origin in y direction.
  settings->start_offset_y_meter = FLAGS_tg_start_offset_y_meter;
  // Yaw start offset.
  settings->start_offset_yaw_radians = FLAGS_tg_start_offset_yaw_radians;
  // Offset from the origin in z direction. This is the constant height.
  settings->start_offset_z_meter = FLAGS_tg_start_offset_z_meter;
  // Ratio between radial and vertical standard deviation of the
  // keypoint positions.
  settings->vertical_to_radial_variance_factor =
      FLAGS_tg_vertical_to_radial_variance_factor;
  // IMU noise bias seed.
  settings->imu_noise_bias_seed =
      static_cast<size_t>(FLAGS_tg_imu_noise_bias_seed);
  // Landmark seed.
  settings->landmark_seed = static_cast<size_t>(FLAGS_tg_landmark_seed);
  // Trajectory mode.
  settings->mode = static_cast<test_trajectory_gen::Path>(FLAGS_tg_mode);
  // Number of path constraints.
  settings->num_of_path_constraints =
      static_cast<size_t>(FLAGS_tg_num_of_path_constraints);
  // Number of landmarks.
  settings->num_of_landmarks = static_cast<size_t>(FLAGS_tg_num_of_landmarks);
  // List with waypoints: x y z yaw.
  settings->filename_waypoints = FLAGS_tg_filename_waypoints;
}

int main(int argc, char** argv) {
  // Initialize google logging and flags.
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  // Initialize ros node handle and visualization publisher.
  ros::init(argc, argv, "path_generator_demo");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<visualization_msgs::MarkerArray>(
      FLAGS_tg_visualization_topic, 1, true);

  // Define settings based on gflags.
  test_trajectory_gen::PathAndLandmarkSettings settings;
  setUp(&settings);

  // **************************************************************************
  //  Generating path and incorporating sensor biases.
  // **************************************************************************
  test_trajectory_gen::GenericPathGenerator path_generator(settings);
  path_generator.generatePath();
  path_generator.generateLandmarks();

  const mav_planning_utils::Motion4D<5, 2>::Vector& data =
      path_generator.getPathData();
  const Eigen::Matrix3Xd& landmarks = path_generator.getLandmarks();

  ROS_INFO("Generated %lu points", data.size());

  // **************************************************************************
  //  Visualization of the generated path and landmarks.
  // **************************************************************************
  if (FLAGS_tg_visualization) {
    visualization_msgs::MarkerArray markers, markers_tmp;
    std_msgs::Header header;
    header.frame_id = FLAGS_tg_visualization_frame_id;
    header.stamp = ros::Time::now();
    header.seq = 0;

    mav_planning_utils::MavState s;
    for (size_t i = 0; i < data.size(); i += FLAGS_tg_sub_sampling_factor) {
      path_generator.motionVectorToMavState(data[i], &s);
      mav_viz::drawAxesArrows(markers_tmp, s.p, s.q, 0.3);
      insertMarkers(markers_tmp, &markers);

      visualization_msgs::Marker marker;
      mav_viz::drawArrow(
          marker, s.p, s.p + s.a, mav_viz::createColorRGBA(1, 1, 0, 1));
      markers.markers.push_back(marker);

      mav_viz::drawArrow(
          marker, s.p, s.p + s.v, mav_viz::createColorRGBA(0, 1, 1, 1));
      markers.markers.push_back(marker);
    }

    for (int i = 0; i < landmarks.cols(); ++i) {
      visualization_msgs::Marker marker;
      marker.type = visualization_msgs::Marker::POINTS;
      marker.action = visualization_msgs::Marker::ADD;
      marker.color = mav_viz::createColorRGBA(0.8, 0.5, 0.2, 1);

      geometry_msgs::Point point;
      point.x = landmarks(0, i);
      point.y = landmarks(1, i);
      point.z = landmarks(2, i);
      marker.points.push_back(point);

      marker.scale.x = 0.1;
      marker.scale.y = 0.1;
      marker.scale.z = 0.1;

      markers.markers.push_back(marker);
    }

    int cnt = 0;
    for (visualization_msgs::MarkerArray::_markers_type::iterator it =
             markers.markers.begin();
         it != markers.markers.end(); ++it) {
      it->action = visualization_msgs::Marker::ADD;
      it->header = header;
      it->id = cnt;
      it->ns = FLAGS_tg_visualization_topic;
      ++cnt;
    }
    pub.publish(markers);
  }

  while (ros::ok()) {
    usleep(1e5);
  }
}


#define _USE_MATH_DEFINES
#include <ctgmath>
#include <cmath>

#include "ConstraintJoint.h"

#include "RigidBodyMain.h"
#include "ChronoTimer.h"

#include <queue>
#include <memory>

#ifndef EIGEN_NO_STATIC_ASSERT
#define EIGEN_NO_STATIC_ASSERT
#endif
#include <Eigen/SparseCholesky>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include <Eigen/Geometry>

#ifdef ONLINE_MODE
#include "online/GLSL.h"
#include <glm/gtc/type_ptr.hpp>
#endif

/// Requires OpenMP 4
//#include <omp.h>
//#pragma omp declare reduction (+: Eigen::VectorXd: omp_out=omp_out+omp_in) initializer(omp_priv=Eigen::VectorXd::Zero(omp_orig.size()))

void ConstraintJoint::init(std::unique_ptr<StateSolve> &SS, const std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S)
{
	SS->Mr_dimension = getConstraintNumReduced(LS);
	SS->Mm_dimension = getConstraintNumMaximal(LS);
	SS->Gr_dimension = getConstraintNumAdditnl(LS);
	SS->Gm_dimension = getConstraintNumAdditnlMaximal(LS);

	int numObj = (int)LS->joints.size();
	if (reducedCoordList.find(simtype) != reducedCoordList.end())
	{
		int constraint_index = SS->Mr_dimension;
		// loop through joints in order
		for (int i = 0; i < numObj; ++i)
		{
			if (simtype == simType::PCG)
				LS->joints[LS->joint_map[i]]->initState_SoA(constraint_index, LS, S);
			else
				LS->joints[LS->joint_map[i]]->initState(constraint_index, LS, S);
			constraint_index -= LS->joints[LS->joint_map[i]]->constraintNum;
		}

		// init extra constraints for joint solver
		for (int i = 0; i < LS->constraints.size(); ++i)
		{
			LS->constraints[i]->initJoint(SS, LS, S);
		}

		// save permanents
		// joint Stiffness and Damping
		int index;
		std::shared_ptr<Joint> j;
		SS->Drlist.clear();
		SS->Krlist.clear();
		for (int i = 0; i < LS->joints.size(); ++i)
		{
			j = LS->joints[i];
			int ci = j->constraint_index;

			for (int cnum = 0; cnum < j->constraintNum; ++cnum)
			{
				if (j->d > THRESHOLD)
					SS->Drlist.push_back(Eigen::Triplet<double>(ci + cnum, ci + cnum, j->d));
				if (j->k > THRESHOLD)
					SS->Krlist.push_back(Eigen::Triplet<double>(ci + cnum, ci + cnum, j->k));
			}
		}
		SS->Kr = Eigen::SparseMatrix<double>(SS->Mr_dimension, SS->Mr_dimension);
		SS->Kr.setFromTriplets(SS->Krlist.begin(), SS->Krlist.end());
		SS->Dr = Eigen::SparseMatrix<double>(SS->Mr_dimension, SS->Mr_dimension);
		SS->Dr.setFromTriplets(SS->Drlist.begin(), SS->Drlist.end());

		// body Damping
		std::shared_ptr<Block> block;
		SS->bDmlist.clear();
		for (int i = 0; i < LS->blocks.size(); ++i)
		{
			block = LS->blocks[i].second;
			index = block->joint->jindex;
			if (block->damping > THRESHOLD)
			{
				for (int z = 0; z < 6; ++z)
				{
					SS->bDmlist.push_back(Eigen::Triplet<double>(index * 6 + z, index * 6 + z, block->damping));
				}
			}
		}
		SS->bDm = Eigen::SparseMatrix<double>(numObj * 6, numObj * 6);
		SS->bDm.setFromTriplets(SS->bDmlist.begin(), SS->bDmlist.end());

		// load Pr
		int num_constraints = 0;
		for (int i = 0; i < numObj; ++i)
		{
			num_constraints = LS->joints[LS->joint_map[i]]->constraintNum;
			LS->joints[LS->joint_map[i]]->Pr = Eigen::MatrixXd::Identity(num_constraints, num_constraints) *
				(SS->h * LS->joints[LS->joint_map[i]]->d + SS->h * SS->h * LS->joints[LS->joint_map[i]]->k);
		}
	}
}

void ConstraintJoint::loadIntoLinkageSystem(const std::unique_ptr<LinkageSystem> & LS)
{
	Eigen::Matrix4d E_wjp;
	Eigen::Matrix4d E_wji;

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	std::shared_ptr<Joint> j;
	for (int jindex = 0; jindex < LS->joints.size(); ++jindex)
	{
		j = LS->joints[jindex];

		E_wjp = Eigen::Matrix4d::Zero();
		E_wji = Eigen::Matrix4d::Zero();
		Eigen::Matrix4d temp;
		Eigen::Matrix4d temp_ji;
		if (!j->root)
		{
			parent = LS->find(j->parentName);
			child = LS->find(j->childName);

			// transform from the parent block to the current joint
			E_wjp.block<3, 3>(0, 0) = parent->E_wi0.block<3, 3>(0, 0);
			E_wjp.block<3, 1>(0, 3) = (parent->E_wi0.block<3, 3>(0, 0)*j->parentPos + parent->E_wi0.block<3, 1>(0, 3));
			E_wjp(3, 3) = 1.0;
			//LS->joints[jindex]->E_jp0 = Rigid::inverse(E_wjp)*parent->E_wi0;
			//LS->joints[jindex]->E_pj0 = Rigid::inverse(LS->joints[jindex]->E_jp0);
			//LS->joints[jindex]->ad_jp0 = Rigid::adjoint(LS->joints[jindex]->E_jp0);

			LS->joints[jindex]->E_pj_ij = Eigen::Matrix4d::Identity();
			LS->joints[jindex]->E_pj_ij.block<3, 1>(0, 3) = j->parentPos - parent->joint->childPos;
			LS->joints[jindex]->ad_pj_ij = Rigid::adjoint(LS->joints[jindex]->E_pj_ij);
			LS->joints[jindex]->ad_ij_pj = Rigid::adjoint(Rigid::inverse(LS->joints[jindex]->E_pj_ij));
		}
		else
		{
			// the root's coordinates are defined in world coordinates
			child = LS->find(j->childName);

			////// transform from the current joint to the parent joint
			LS->joints[jindex]->E_pj_ij = Eigen::Matrix4d::Identity();
			LS->joints[jindex]->E_pj_ij.block<3, 1>(0, 3) = j->parentPos;
			LS->joints[jindex]->ad_pj_ij = Rigid::adjoint(LS->joints[jindex]->E_pj_ij);
			LS->joints[jindex]->ad_ij_pj = Rigid::adjoint(Rigid::inverse(LS->joints[jindex]->E_pj_ij));

			E_wjp.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
			E_wjp.block<3, 1>(0, 3) = j->parentPos;
			E_wjp(3, 3) = 1.0;
			//LS->joints[jindex]->E_pj0 = E_wjp;
			//LS->joints[jindex]->E_jp0 = Rigid::inverse(E_wjp);
			//LS->joints[jindex]->ad_jp0 = Rigid::adjoint(LS->joints[jindex]->E_jp0);
		}

		if (j->E_ji0_)
		{
			LS->joints[jindex]->E_ji = j->E_ji0;
		}
		else
		{
			E_wji.block<3, 3>(0, 0) = child->E_wi0.block<3, 3>(0, 0);
			E_wji.block<3, 1>(0, 3) = child->E_wi0.block<3, 3>(0, 0)*j->childPos + child->E_wi0.block<3, 1>(0, 3);
			E_wji(3, 3) = 1.0;
			LS->joints[jindex]->E_ji = Rigid::inverse(E_wji)*child->E_wi0;
		}

		LS->joints[jindex]->E_ij = Rigid::inverse(LS->joints[jindex]->E_ji);
		LS->joints[jindex]->ad_ji = Rigid::adjoint(LS->joints[jindex]->E_ji);
		LS->joints[jindex]->ad_ij = Rigid::adjoint(LS->joints[jindex]->E_ij);
		LS->joints[jindex]->ad_ijT = LS->joints[jindex]->ad_ij.transpose();

		if (j->E_pj0_)
		{
			j->E_pj_ij = j->E_pj0;
			j->ad_pj_ij = Rigid::adjoint(j->E_pj_ij);
			j->ad_ij_pj = Rigid::adjoint(Rigid::inverse(j->E_pj_ij));
		}
	}
}

void ConstraintJoint::loadOptimalJointOrdering(const std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State> & S)
{
	/// perform DFS and reverse
	int root_joint = - 1;
	int numObj = (int)LS->joints.size();
	int numConstraints = 0;
	for (int j = 0; j < numObj; ++j)
	{
		if (LS->joints[j]->root)
		{
			root_joint = j;
		}
		numConstraints += LS->joints[j]->constraintNum;
	}

	// dont handle any free falling stuff for now
	assert(root_joint != -1);
	assert(LS->joints.size() == LS->blocks.size());
	LS->joint_map.clear();

	// assign optimized index values
	int blockID = -1;
	int jointID = 0;

	/// DFS indecies
	LS->joints[root_joint]->DFSIndexSet(blockID, jointID, LS->blocks, LS->joint_map);

	/// BFS indecies
	//std::queue<std::shared_ptr<Joint>> waitlist;
	//std::queue<std::shared_ptr<Joint>> postlist;
	//waitlist.push(LS->joints[root_joint]);
	//blockID = LS->blocks.size();
	//jointID = numConstraints;
	//while (!waitlist.empty())
	//{
	//	std::shared_ptr<Joint> j = waitlist.front();
	//	waitlist.pop();
	//	std::shared_ptr<Block> block = nullptr;
	//	for (int i = 0; i < LS->blocks.size(); ++i)
	//	{
	//		if (LS->blocks[i].first.compare(j->childName) == 0)
	//			block = LS->blocks[i].second;
	//	}
	//	int numChildren = (int)block->c_joints.size();
	//	for (int i = 0; i < numChildren; ++i)
	//	{
	//		waitlist.push(block->c_joints[i]);
	//	}
	//	LS->joint_map.insert(LS->joint_map.end(), j->index0); // looping through joint_map will access all joints such that every parent is in front of it's children
	//	blockID = blockID - 1;
	//	j->jindex = blockID;
	//	jointID = jointID - j->constraintNum;
	//	j->constraint_index = jointID;
	//}

	/// Mirror MATLAB - bridge scene
	//blockID = 0;
	//jointID = 0;
	//std::shared_ptr<Joint> j;
	//for (int i = (int)LS->joints.size() - 1; i >= 0; --i)
	//{
	//	j = LS->joints[i];
	//	LS->joint_map.insert(LS->joint_map.begin(), j->index0); // looping through joint_map will access all joints such that every parent is in front of it's children
	//	j->jindex = blockID;
	//	blockID = blockID + 1;
	//	if (j->constraintNum == 0)
	//		j->constraint_index = jointID;
	//	else
	//		j->constraint_index = jointID;
	//	jointID = jointID + j->constraintNum;
	//}

	// no matrix CG solve
	int jointCI = 0;
	if (reducedNoMatrixList.find(simtype) != reducedNoMatrixList.end())
	{
		jointID = 0;
		//jointCI = 0;
		for (int i = 0; i < LS->constraints.size(); ++i)
		{
			LS->constraints[i]->constraint_index = jointID;
			jointID = jointID + LS->constraints[i]->constraintNum;
			//LS->constraints[i]->constraint_index_m = jointCI;
			//jointCI = jointCI + LS->constraints[i]->constraintNumMax;
		}
	}
	else  // reduced solve with G matrix
	{
		jointID = 0;
		jointCI = 0;
		for (int i = 0; i < LS->constraints.size(); ++i)
		{
			LS->constraints[i]->constraint_index = jointID;
			jointID = jointID + LS->constraints[i]->constraintNum;
			//LS->constraints[i]->constraint_index_m = jointCI;
			//jointCI = jointCI + LS->constraints[i]->constraintNumMax;
		}
	}
}

int ConstraintJoint::getConstraintNumMaximal(const std::unique_ptr<LinkageSystem> & LS)
{
	// Preprocessing - how big does f need to be?
	int constraints = 0;
	for (int i = 0; i < LS->joints.size(); ++i)
	{
		if (LS->joints[i]->type == Joint::JType::Ball || LS->joints[i]->type == Joint::JType::Springy)
			constraints += 3;
		else if (LS->joints[i]->type == Joint::JType::Powered)
			constraints += 4;
		else if (LS->joints[i]->type == Joint::JType::Slider)
			constraints += 2;
		else if (LS->joints[i]->type == Joint::JType::Hinge)
			constraints += 5;
	}
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		constraints += LS->constraints[i]->constraintNum;
	}
	return constraints;
}

int ConstraintJoint::getConstraintNumReduced(const std::unique_ptr<LinkageSystem>& LS)
{
	// Preprocessing - how big does f need to be?
	//if (ConstraintJoint::red_constraint_num_save)
	//	return ConstraintJoint::red_constraint_num_save;

	int constraints = 0;
	for (int i = 0; i < LS->joints.size(); ++i)
	{
		constraints += LS->joints[i]->constraintNum;
	}
	return constraints;
}

int ConstraintJoint::getConstraintNumAdditnl(const std::unique_ptr<LinkageSystem>& LS)
{
	int constraints = 0;
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		constraints += LS->constraints[i]->constraintNum;
	}
	return constraints;
}

int ConstraintJoint::getConstraintNumAdditnlMaximal(const std::unique_ptr<LinkageSystem>& LS)
{
	int constraints = 0;
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		constraints += LS->constraints[i]->constraintNumMax;
	}
	return constraints;
}

void ConstraintJoint::updateJacobian(std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	int row = SS->row;

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int parentIndex;
	int childIndex;
	std::shared_ptr<Joint> j;

	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		//std::cout << j->childName << std::endl;
		//std::cout << j->E_ji << std::endl << std::endl;
		//std::cout << j->E_ij << std::endl << std::endl;
		//std::cout << j->ad_ij << std::endl << std::endl;
		//std::cout << j->E_pj_ij << std::endl << std::endl;
		//std::cout << j->ad_pj_ij << std::endl << std::endl;
		//std::cout << j->ad_ij_pj << std::endl << std::endl;
		//std::cout << "E_wi \n" << S->E[childIndex] << std::endl;

		for (int cnum = 0; cnum < j->constraintNum; ++cnum)
		{
			// load J(i,i) with ifromJiAdS
			if (j->type != Joint::JType::Fixed)
			{
				int rowindex = childIndex * 6;
				int colindex = j->constraint_index + cnum;

				SS->J.block<6, 1>(rowindex, colindex) = j->ad_ij*j->S.block<6, 1>(0, cnum);

				//std::cout << j->ad_ij << std::endl << std::endl;
				//std::cout << j->S << std::endl << std::endl;

				SS->Jd.block<6, 1>(rowindex, colindex) = j->ad_ij*j->Sdot.block<6, 1>(0, cnum);

				// sparse matrix
				for (int z = 0; z < 6; ++z)
				{
					if (std::abs(SS->J(rowindex + z, colindex)) > THRESHOLD)
					{
						SS->Jlist.push_back(Eigen::Triplet<double>(rowindex + z, colindex, SS->J(rowindex + z, colindex)));
						SS->JTlist.push_back(Eigen::Triplet<double>(colindex, rowindex + z, SS->J(rowindex + z, colindex)));
					}
					if (std::abs(SS->Jd(rowindex + z, colindex)) > THRESHOLD)
					{
						SS->Jdlist.push_back(Eigen::Triplet<double>(rowindex + z, colindex, SS->Jd(rowindex + z, colindex)));
					}
				}
			}
		}

		if (!j->root)
		{
			parent = j->parentBlock;
			parentIndex = parent->joint->jindex;

			if (!j->ad_ip_)
			{
				j->ad_ip = Rigid::adjoint(j->E_iw * S->E[parentIndex]);
				j->ad_ipT = j->ad_ip.transpose();
				j->ad_ip_ = true;
			}

			if (!j->ad_iw_)
			{
				j->ad_iw = Rigid::adjoint(j->E_iw);
				j->ad_iw_ = true;
			}

			if (!j->addot_)
			{
				j->addot = Rigid::addot(S->E[childIndex], S->v.segment<6>(childIndex * 6));
				j->addot_ = true;
			}

			if (!parent->joint->ad_wi_)
			{
				parent->joint->ad_wi = Rigid::adjoint(S->E[parentIndex]);
				parent->joint->ad_wi_ = true;
			}

			if (!parent->joint->addot_)
			{
				parent->joint->addot = Rigid::addot(S->E[parentIndex], S->v.segment<6>(parentIndex * 6));
				parent->joint->addot_ = true;
			}

			j->add_ip = -j->ad_iw * (j->addot *	j->ad_iw * parent->joint->ad_wi - parent->joint->addot);

			// add dependencies into the jacobian until the parent block is rooted (the top of the tree)
	 		std::shared_ptr<Block> ancestor = j->parentBlock;
			std::shared_ptr<Joint> p_joint;
			std::shared_ptr<Joint> c_joint;
			int og_child = childIndex;
			int og_parent = parentIndex;
			while (ancestor != nullptr)
			{
				p_joint = ancestor->joint;
				c_joint = ancestor->joint->childBlock->joint;

				for (int cnum = 0; cnum < p_joint->constraintNum; ++cnum)
				{
					int rowindex = og_child * 6;
					int colindex = p_joint->constraint_index + cnum;

					// J(i,a) = pToiAd*J(p,a)
					SS->J.block<6, 1>(rowindex, colindex) =
						j->ad_ip * SS->J.block<6, 1>(og_parent * 6, colindex);

					// Jd(i,a) = pToiAdd*J(p,a) + pToiAd*Jd(p,a)
					SS->Jd.block<6, 1>(rowindex, colindex) =
						j->add_ip * SS->J.block<6, 1>(og_parent * 6, colindex) +
						j->ad_ip * SS->Jd.block<6, 1>(og_parent * 6, colindex);

					// we actually want a sparse matrix
					for (int z = 0; z < 6; ++z)
					{
						if (std::abs(SS->J(rowindex + z, colindex)) > THRESHOLD)
						{
							SS->Jlist.push_back(Eigen::Triplet<double>(rowindex + z, colindex, SS->J(rowindex + z, colindex)));
							SS->JTlist.push_back(Eigen::Triplet<double>(colindex, rowindex + z, SS->J(rowindex + z, colindex)));
						}
						if (std::abs(SS->Jd(rowindex + z, colindex)) > THRESHOLD)
						{
							SS->Jdlist.push_back(Eigen::Triplet<double>(rowindex + z, colindex, SS->Jd(rowindex + z, colindex)));
						}
					}
				}

				// move up in the family tree
				ancestor = ancestor->joint->parentBlock;
			}
		}
		row = row + j->constraintNum;
	}
	SS->row = row;
}

void ConstraintJoint::computeJ_Jdot_x_unopt(Eigen::VectorXd &J_x, Eigen::VectorXd &Jdot_x, const Eigen::VectorXd &x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	J_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);
	Jdot_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int parentIndex;
	int childIndex;
	std::shared_ptr<Joint> j;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// y(i) = ad_ij * S * x(j)
		Vector6d Sx = Vector6d::Zero();
		Vector6d Sdotx = Vector6d::Zero();
		for (int row = 0; row < 6; ++row)
		{
			for (int xi = 0; xi < j->constraintNum; ++xi)
			{
				Sx[row] += j->S(row, xi) * x[j->constraint_index + xi];  // j->S*x[childIndex];
				Sdotx[row] += j->Sdot(row, xi) * x[j->constraint_index + xi];
			}
		}
		J_x.segment<6>(childIndex * 6) = j->ad_ij*Sx;
		Jdot_x.segment<6>(childIndex * 6) = j->ad_ij*Sdotx;

		if (!j->root)
		{
			parent = j->parentBlock;
			parentIndex = parent->joint->jindex;

			// compute pToi ad
			if (!j->ad_ip_)
			{
				j->ad_ip = Rigid::adjoint(j->E_iw * S->E[parentIndex]);
				j->ad_ipT = j->ad_ip.transpose();
				j->ad_ip_ = true;
			}

			// compute pToi addot
			if (!j->ad_iw_)
			{
				j->ad_iw = Rigid::adjoint(j->E_iw);
				j->ad_iw_ = true;
			}

			if (!j->addot_)
			{
				j->addot = Rigid::addot(S->E[childIndex], S->v.segment<6>(childIndex * 6));
				j->addot_ = true;
			}

			if (!parent->joint->ad_wi_)
			{
				parent->joint->ad_wi = Rigid::adjoint(S->E[parentIndex]);
				parent->joint->ad_wi_ = true;
			}

			if (!parent->joint->addot_)
			{
				parent->joint->addot = Rigid::addot(S->E[parentIndex], S->v.segment<6>(parentIndex * 6));
				parent->joint->addot_ = true;
			}

			if (!j->add_ip_)
			{
				j->add_ip = -j->ad_iw * (j->addot *	j->ad_iw * parent->joint->ad_wi - parent->joint->addot);
				j->add_ip_ = true;
			}

			//add_ip = -ad_iw*(Rigid::addot(S->E[childIndex], S->v.segment<6>(childIndex * 6)) *
			//	ad_iw * Rigid::adjoint(S->E[parentIndex]) - Rigid::addot(S->E[parentIndex], S->v.segment<6>(parentIndex * 6)));

			// y(i) += ad_ip * y(p)
			J_x.segment<6>(childIndex * 6) += j->ad_ip * J_x.segment<6>(parentIndex * 6);

			// z(i) += ad_ip * z(p) + addot_ip * y(p)
			Jdot_x.segment<6>(childIndex * 6) += j->ad_ip * Jdot_x.segment<6>(parentIndex * 6) +
				j->add_ip * J_x.segment<6>(parentIndex * 6);
		}
	}
}

void ConstraintJoint::computeJ_x_unopt(Eigen::VectorXd &J_x, const Eigen::VectorXd &x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	J_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int parentIndex;
	int childIndex;
	std::shared_ptr<Joint> j;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// y(i) = ad_ij * S * x(j)
		//Vector6d Sx = Vector6d::Zero();
		//for (int row = 0; row < 6; ++row)
		//{
		//	for (int xi = 0; xi < j->constraintNum; ++xi)
		//	{
		//		Sx[row] += j->S(row, xi) * x[j->constraint_index + xi];  // j->S*x[childIndex];
		//	}
		//}
		//J_x.segment<6>(childIndex * 6) = j->ad_ij*Sx;
		for (int row = 0; row < 6; ++row)
		{
			for (int xi = 0; xi < j->constraintNum; ++xi)
			{
				J_x[childIndex * 6 + row] += j->adij_S(row, xi) * x[j->constraint_index + xi];  // j->S*x[childIndex];
			}
		}

		if (!j->root)
		{
			parent = j->parentBlock;
			parentIndex = parent->joint->jindex;

			// compute pToi ad
			if (!j->ad_ip_)
			{
				j->ad_ip = Rigid::adjoint(j->E_iw * S->E[parentIndex]);
				j->ad_ipT = j->ad_ip.transpose();
				j->ad_ip_ = true;
			}

			// y(i) += ad_ip * y(p)
			J_x.segment<6>(childIndex * 6) += j->ad_ip * J_x.segment<6>(parentIndex * 6);
		}
	}
}

Eigen::VectorXd ConstraintJoint::computeM_x_unopt(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Eigen::VectorXd M_x = Eigen::VectorXd::Zero(x.size());

	int jindex;
	int childIndex;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		jindex = LS->joint_map[jmapi];
		childIndex = LS->joints[jindex]->jindex;

		// M * JX
		for (int mi = 0; mi < 6; ++mi)
		{
			M_x[childIndex * 6 + mi] = S->M[childIndex](mi, mi)*x[childIndex * 6 + mi];
		}
	}
	return M_x;
}

Eigen::VectorXd ConstraintJoint::computeLHS_x_unopt(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Eigen::VectorXd M_x = Eigen::VectorXd::Zero(x.size());

	int childIndex;
	std::shared_ptr<Block> b;
	for (int bodyi = 0; bodyi < LS->blocks.size(); ++bodyi)
	{
		b = LS->blocks[bodyi].second;
		childIndex = b->joint->jindex;

		// M * JX
		for (int mi = 0; mi < 6; ++mi)
		{
			M_x[childIndex * 6 + mi] = S->M[childIndex](mi, mi)*x[childIndex * 6 + mi];
		}

		// body damping
		if (b->damping > 0)
		{
			// Mr = (Mr + J'*(h * Dm + h * Bd + h * h * Km)*J + (h * Dr - h * h * Kr)) * qdot
			M_x.segment<6>(childIndex * 6) += (SS->h * b->damping * x.segment<6>(childIndex * 6));
			//SS->fr.segment<6>(childIndex * 6) = SS->fr.segment<6>(childIndex * 6) - (b->damping * S->v.segment<6>(childIndex * 6));
		}
	}
//	Eigen::VectorXd temp;
//#pragma omp parallel for private(temp)// reduction(+ : M_x)
//	for (int i = 0; i < LS->constraints.size(); ++i)
//	{
//		temp = Eigen::VectorXd::Zero(M_x.size());
//		LS->constraints[i]->computeCGProd(temp, x, SS, LS, S);  // most expensive in this function
//#pragma omp critical
//		M_x += temp;
//	}
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		LS->constraints[i]->computeCGProd(M_x, x, SS, LS, S);  // most expensive in this function
	}
	return M_x;
}

void ConstraintJoint::computeStiffnessDampingJoint_unopt(Eigen::VectorXd &LHSx, const Eigen::VectorXd & qdot, const Eigen::VectorXd & q, std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	int childIndex;
	std::shared_ptr<Joint> j;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		j = LS->joints[jmapi];
		childIndex = j->constraint_index;

		for (int cnum = 0; cnum < j->constraintNum; ++cnum)
		{
			// Mr = Mr + (h * Dr - h * h * Kr) * qdot
			LHSx[childIndex + cnum] += ((SS->h * j->d + SS->h * SS->h * j->k) * qdot[childIndex + cnum]);

			// update on SS->fr
			SS->fr[childIndex + cnum] -= j->k * (q[childIndex + cnum] - j->q0[cnum]);
		}
	}
}

Eigen::VectorXd ConstraintJoint::computeJT_x_unopt(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, std::shared_ptr<State::local_mt> lmt)
{
	Eigen::VectorXd JT_x = Eigen::VectorXd::Zero(SS->Mr_dimension);
	Eigen::VectorXd y = Eigen::VectorXd::Zero(x.size());
	Eigen::VectorXd res;

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int parentIndex;
	int childIndex;
	std::shared_ptr<Joint> j;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		int list_index = (int)LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// initialize y so MJx is not changed out of scope
		y.segment<6>(childIndex * 6) = x.segment<6>(childIndex * 6);

		// yi += child_alpha
		for (int ci = 0; ci < child->c_joints.size(); ++ci)
		{
			if (lmt == nullptr)
				y.segment<6>(childIndex * 6) += child->c_joints[ci]->alpha;
			else
				y.segment<6>(childIndex * 6) += lmt->alpha_[child->c_joints[ci]->jindex];
		}

		if (!j->root)
		{
			parent = j->parentBlock;
			parentIndex = parent->joint->jindex;

			// set alpha for this joint
			if (!j->ad_ip_)
			{
				j->ad_ip = Rigid::adjoint(j->E_iw * S->E[parentIndex]);
				j->ad_ipT = j->ad_ip.transpose();
				j->ad_ip_ = true;
			}
			if(lmt == nullptr)
				j->alpha = j->ad_ipT * y.segment<6>(childIndex * 6);
			else
				lmt->alpha_[childIndex] = j->ad_ipT * y.segment<6>(childIndex * 6);
		}

		res = j->ST_adijT * y.segment<6>(childIndex * 6);  // most expensive in this fucntion

		assert(res.size() == j->constraintNum);
		for (int resi = 0; resi < res.size(); ++resi)
		{
			JT_x[j->constraint_index + resi] = res[resi];
		}
	}
	return JT_x;
}

// ... broken - errors on the order of 1e-1
void ConstraintJoint::computeJT_x_parallel_unopt(Eigen::VectorXd &Mrtilde, const Eigen::VectorXd &LHSJqd, Eigen::VectorXd &Mr, const Eigen::VectorXd &MJqd, Eigen::VectorXd &fr, const Eigen::VectorXd &fm_MJdqd, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Eigen::VectorXd y1 = Eigen::VectorXd::Zero(LHSJqd.size());
	Eigen::VectorXd y2 = Eigen::VectorXd::Zero(MJqd.size());
	Eigen::VectorXd y3 = Eigen::VectorXd::Zero(fm_MJdqd.size());
	Eigen::VectorXd res1;
	Eigen::VectorXd res2;
	Eigen::VectorXd res3;

	Mrtilde = Eigen::VectorXd::Zero(SS->Mr_dimension);
	Mr = Eigen::VectorXd::Zero(SS->Mr_dimension);
	//fr = Eigen::VectorXd::Zero(SS->Mr_dimension);   // already initialized

	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int parentIndex;
	int childIndex;
	std::shared_ptr<Joint> j;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		int list_index = (int)LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// initialize y so MJx is not changed out of scope
		y1.segment<6>(childIndex * 6) = LHSJqd.segment<6>(childIndex * 6);
		y2.segment<6>(childIndex * 6) = MJqd.segment<6>(childIndex * 6);
		y3.segment<6>(childIndex * 6) = fm_MJdqd.segment<6>(childIndex * 6);

		// yi += child_alpha
		for (int ci = 0; ci < child->c_joints.size(); ++ci)
		{
			y1.segment<6>(childIndex * 6) += child->c_joints[ci]->alpha1;
			y2.segment<6>(childIndex * 6) += child->c_joints[ci]->alpha2;
			y3.segment<6>(childIndex * 6) += child->c_joints[ci]->alpha3;
		}

		if (!j->root)
		{
			parent = j->parentBlock;
			parentIndex = parent->joint->jindex;

			// set alpha for this joint
			if (!j->ad_ip_)
			{
				j->ad_ip = Rigid::adjoint(j->E_iw * S->E[parentIndex]);
				j->ad_ipT = j->ad_ip.transpose();
				j->ad_ip_ = true;
			}
			j->alpha1 = j->ad_ipT * y1.segment<6>(childIndex * 6);
			j->alpha2 = j->ad_ipT * y2.segment<6>(childIndex * 6);
			j->alpha3 = j->ad_ipT * y3.segment<6>(childIndex * 6);
		}

		res1 = j->ST * j->ad_ijT * y1.segment<6>(childIndex * 6);
		res2 = j->ST * j->ad_ijT * y2.segment<6>(childIndex * 6);
		res3 = j->ST * j->ad_ijT * y3.segment<6>(childIndex * 6);

		assert(res1.size() == j->constraintNum);
		for (int resi = 0; resi < res1.size(); ++resi)
		{
			Mrtilde[j->constraint_index + resi] = res1[resi];
			Mr[j->constraint_index + resi] = res2[resi];
			fr[j->constraint_index + resi] += res3[resi];
		}
	}
}

void ConstraintJoint::preprocess_preconditioner_unopt(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, bool blkdiag)
{
	std::shared_ptr<Block> child;
	int childIndex;
	std::shared_ptr<Joint> j;
	std::shared_ptr<Joint> j_child;

	// backward traversal
	Matrix6d accum_adj;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		int list_index = (int)LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// Maximal and Reduced damping terms
		// updated outside preconditioner

		accum_adj = Matrix6d::Zero();
		for (int ci = 0; ci < child->c_joints.size(); ++ci)
		{
			j_child = child->c_joints[ci];
			accum_adj = accum_adj + j_child->ad_jpT * j_child->Pi * j_child->ad_jp;
		}

		// toggle use of damping and stiffness blkdiag in Minv
		if (blkdiag)
		{
			//std::cout << "Dmd:\n" << (-j->Dmd) << std::endl << std::endl;
			//std::cout << "Dd:\n" << (child->D) << std::endl << std::endl;
			//std::cout << "Kmd:\n" << (j->Kmd) << std::endl << std::endl;
			//std::cout << "total:\n" << (-j->Dmd + child->D - SS->h * j->Kmd) << std::endl << std::endl;
			//std::cout << SS->h * j->ad_ijT * (-j->Dmd + child->D - SS->h * j->Kmd) * j->ad_ij << std::endl << std::endl;
			j->Mhat = j->I_j + SS->h * j->ad_ijT * (-j->Dmd + child->D - SS->h * j->Kmd) * j->ad_ij + accum_adj;
		}
		else
			j->Mhat = j->I_j + accum_adj;

		// fixed joints do not have index in reduced state
		if (j->type != Joint::JType::Fixed)
		{
			j->Psi = (j->ST * j->Mhat * j->S + j->Pr).inverse();
			j->Pi = j->Mhat - j->Mhat * j->S * j->Psi * j->ST * j->Mhat;
		}

		// precomputations
		j->ST_Mhat = j->ST * j->Mhat;
		j->S_Psi = j->S * j->Psi;
		j->Mhat_S_Psi = j->Mhat * j->S_Psi;
	}
}

Eigen::VectorXd ConstraintJoint::computeMinv_x_unopt(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, std::shared_ptr<State::local_mt> lmt)
{
	std::shared_ptr<Block> parent;
	std::shared_ptr<Block> child;
	int childIndex;
	std::shared_ptr<Joint> j;
	std::shared_ptr<Joint> j_child;
	Eigen::VectorXd Minv_x = Eigen::VectorXd::Zero(x.size());

	// backward traversal
	Eigen::VectorXd x_STB;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		int list_index = (int)LS->joint_map[jmapi];
		j = LS->joints[list_index];

		child = j->childBlock;
		childIndex = j->jindex;

		// Bhat_j
		Vector6d Bhat = Vector6d::Zero();
		for (int ci = 0; ci < child->c_joints.size(); ++ci)
		{
			// beta changes throughout Minv solve
			if(lmt == nullptr)
				Bhat.noalias() += child->c_joints[ci]->ad_jpT * child->c_joints[ci]->beta;
			else
				Bhat.noalias() += child->c_joints[ci]->ad_jpT * lmt->beta_[child->c_joints[ci]->jindex];
		}
		if (lmt == nullptr)
		{
			j->Bhat = Bhat;
			j->ST_Bhat = j->ST * Bhat;
			x_STB = j->ST_Bhat;
		}
		else
		{
			lmt->Bhat_[childIndex] = Bhat;
			lmt->ST_Bhat_[childIndex] = j->ST * Bhat;
			x_STB = lmt->ST_Bhat_[childIndex];
		}

		// beta_j
		for (int i = 0; i < j->constraintNum; ++i)
		{
			x_STB[i] = x[j->constraint_index + i] - x_STB[i];
		}
		Vector6d beta = Bhat;
		if (j->type != Joint::JType::Fixed)
		{
			beta.noalias() += j->Mhat_S_Psi * x_STB;
		}
		if (lmt == nullptr)
			j->beta = beta;
		else
			lmt->beta_[childIndex] = beta;
	}

	// forward traversal
	Eigen::VectorXd y;	// qddot
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		j = LS->joints[list_index];
		childIndex = j->jindex;

		if (lmt == nullptr)
			x_STB = j->ST_Bhat;
		else
			x_STB = lmt->ST_Bhat_[childIndex];
		for (int i = 0; i < j->constraintNum; ++i)
		{
			x_STB[i] = x[j->constraint_index + i] - x_STB[i];
		}

		if(lmt == nullptr)
			j->Vdot = Vector6d::Zero();
		else
			lmt->Vdot_[childIndex] = Vector6d::Zero();
		if (!j->root)
		{
			if (lmt == nullptr)
			{
				j->Vdot = j->ad_jp * j->parentBlock->joint->Vdot;
				x_STB -= j->ST_Mhat * j->Vdot;
			}
			else
			{
				lmt->Vdot_[childIndex] = j->ad_jp * lmt->Vdot_[j->parentBlock->joint->jindex];
				x_STB -= j->ST_Mhat * lmt->Vdot_[childIndex];
			}
		}

		y = j->Psi * x_STB;
		if (j->type != Joint::JType::Fixed)
		{
			if (lmt == nullptr)
				j->Vdot += j->S*y;
			else
				lmt->Vdot_[childIndex] += j->S*y;
		}
		else
		{
			if (lmt == nullptr)
				j->Vdot = Vector6d::Zero();
			else
				lmt->Vdot_[childIndex] = Vector6d::Zero();
		}

		// load into result vector
		for (int i = 0; i < y.size(); ++i)
		{
			Minv_x[j->constraint_index + i] = y[i];
		}
	}
	return Minv_x;
}

void ConstraintJoint::preprocess_PCG(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	int list_index;
	int jindex;
	std::shared_ptr<Joint> j;
	// Iterate through the joints in order
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		list_index = LS->joint_map[jmapi];
		jindex = S->jindex[list_index];

		j = LS->joints[list_index];

		S->ad_ip[jindex] = Rigid::adjoint(j->E_iw * S->E[S->parentIndex[jindex]]);
		S->ad_ipT[jindex] = S->ad_ip[jindex].transpose();
		S->ad_iw[jindex] = Rigid::adjoint(j->E_iw);
		S->ad_wi[jindex] = Rigid::adjoint(S->E[jindex]);
		S->addot[jindex] = Rigid::addot(S->E[jindex], S->v.segment<6>(jindex * 6));
	}

	// Iterate through the joints in order
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		list_index = LS->joint_map[jmapi];
		jindex = S->jindex[list_index];

		if (!S->root[jindex])
		{
			S->add_ip[jindex] = -S->ad_iw[jindex] * (S->addot[jindex] * S->ad_iw[jindex] * S->ad_wi[S->parentIndex[jindex]] - S->addot[S->parentIndex[jindex]]);
		}
	}
}

void ConstraintJoint::computeJ_Jdot_x(Eigen::VectorXd & J_x, Eigen::VectorXd & Jdot_x, const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	J_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);
	Jdot_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);

	int parentIndex;
	int jindex;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		jindex = S->jindex[list_index];

		// y(i) = ad_ij * S * x(j)
		Vector6d Sx = Vector6d::Zero();
		Vector6d Sdotx = Vector6d::Zero();
		for (int row = 0; row < 6; ++row)
		{
			for (int xi = 0; xi < S->constraintNum[jindex]; ++xi)
			{
				Sx[row] += S->S[jindex](row, xi) * x[S->constraint_index[jindex] + xi];
				Sdotx[row] += S->Sdot[jindex](row, xi) * x[S->constraint_index[jindex] + xi];
			}
		}
		J_x.segment<6>(jindex * 6) = S->ad_ij[jindex]*Sx;
		Jdot_x.segment<6>(jindex * 6) = S->ad_ij[jindex]*Sdotx;

		if (!S->root[jindex])
		{
			parentIndex = S->parentIndex[jindex];

			// y(i) += ad_ip * y(p)
			J_x.segment<6>(jindex * 6) += S->ad_ip[jindex] * J_x.segment<6>(parentIndex * 6);

			// z(i) += ad_ip * z(p) + addot_ip * y(p)
			Jdot_x.segment<6>(jindex * 6) += S->ad_ip[jindex] * Jdot_x.segment<6>(parentIndex * 6) +
				S->add_ip[jindex] * J_x.segment<6>(parentIndex * 6);
		}
	}
}

void ConstraintJoint::computeJ_x(Eigen::VectorXd & J_x, const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	J_x = Eigen::VectorXd::Zero(LS->blocks.size() * 6);

	int parentIndex;
	int jindex;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		int list_index = LS->joint_map[jmapi];
		jindex = S->jindex[list_index];

		for (int row = 0; row < 6; ++row)
		{
			for (int xi = 0; xi < S->constraintNum[jindex]; ++xi)
			{
				J_x[jindex * 6 + row] += S->adij_S[jindex](row, xi) * x[S->constraint_index[jindex] + xi];
			}
		}

		if (!S->root[jindex])
		{
			parentIndex = S->parentIndex[jindex];

			// y(i) += ad_ip * y(p)
			J_x.segment<6>(jindex * 6) += S->ad_ip[jindex] * J_x.segment<6>(parentIndex * 6);
		}
	}
}

Eigen::VectorXd ConstraintJoint::computeM_x(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Eigen::VectorXd M_x = Eigen::VectorXd::Zero(x.size());

	int jindex;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		// M * JX
		for (int mi = 0; mi < 6; ++mi)
		{
			M_x[jindex * 6 + mi] = S->M[jindex](mi, mi)*x[jindex * 6 + mi];
		}
	}
	return M_x;
}

Eigen::VectorXd ConstraintJoint::computeLHS_x(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Eigen::VectorXd M_x = Eigen::VectorXd::Zero(x.size());

	int jindex;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		// M * JX
		for (int mi = 0; mi < 6; ++mi)
		{
			M_x[jindex * 6 + mi] = S->M[jindex](mi, mi)*x[jindex * 6 + mi];
		}

		// body damping
		if (S->damping[jindex] > 0)
		{
			// Mr = (Mr + J'*(h * Dm + h * Bd + h * h * Km)*J + (h * Dr - h * h * Kr)) * qdot
			M_x.segment<6>(jindex * 6) += (SS->h * S->damping[jindex] * x.segment<6>(jindex * 6));
			//SS->fr.segment<6>(childIndex * 6) = SS->fr.segment<6>(childIndex * 6) - (b->damping * S->v.segment<6>(childIndex * 6));
		}
	}
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		/// SoA
		LS->constraints[i]->computeCGProd(M_x, x, SS, LS, S);  // most expensive in this function
	}
	return M_x;
}

void ConstraintJoint::computeStiffnessDampingJoint(Eigen::VectorXd & LHSx, const Eigen::VectorXd & qdot, const Eigen::VectorXd & q, std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	int jindex;
	int const_index;
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		jindex = S->jindex[jmapi];
		const_index = S->constraint_index[jindex];

		for (int cnum = 0; cnum < S->constraintNum[jindex]; ++cnum)
		{
			// Mr = Mr + (h * Dr - h * h * Kr) * qdot
			LHSx[const_index + cnum] += ((SS->h * S->d[jindex] + SS->h * SS->h * S->k[jindex]) * qdot[const_index + cnum]);

			// update on SS->fr
			SS->fr[const_index + cnum] -= S->k[jindex] * (q[const_index + cnum] - S->q0[jindex][cnum]);
		}
	}
}

Eigen::VectorXd ConstraintJoint::computeJT_x(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, std::shared_ptr<State::local_mt> lmt)
{
	Eigen::VectorXd JT_x = Eigen::VectorXd::Zero(SS->Mr_dimension);
	Eigen::VectorXd y = Eigen::VectorXd::Zero(x.size());
	Eigen::VectorXd res;

	int parentIndex;
	int jindex;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		// initialize y so MJx is not changed out of scope
		y.segment<6>(jindex * 6) = x.segment<6>(jindex * 6);

		// yi += child_alpha
		int child_jindex;
		for (int ci = 0; ci < S->childIndex[jindex].size(); ++ci)
		{
			child_jindex = S->childIndex[jindex][ci];
			if (lmt == nullptr)
				y.segment<6>(jindex * 6) += S->alpha[child_jindex];
			else
				y.segment<6>(jindex * 6) += lmt->alpha_[child_jindex];
		}

		if (!S->root[jindex])
		{
			parentIndex = S->parentIndex[jindex];

			// set alpha for this joint
			if (lmt == nullptr)
				S->alpha[jindex] = S->ad_ipT[jindex] * y.segment<6>(jindex * 6);
			else
				lmt->alpha_[jindex] = S->ad_ipT[jindex] * y.segment<6>(jindex * 6);
		}

		res = S->ST_adijT[jindex] * y.segment<6>(jindex * 6);  // most expensive in this fucntion

		assert(res.size() == S->constraintNum[jindex]);
		for (int resi = 0; resi < res.size(); ++resi)
		{
			JT_x[S->constraint_index[jindex] + resi] = res[resi];
		}
	}
	return JT_x;
}

void ConstraintJoint::preprocess_PCG_preconditioner(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, bool blkdiag)
{
	// backward traversal
	Matrix6d accum_adj;
	int jindex;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		// Maximal and Reduced damping terms
		// updated outside preconditioner

		accum_adj = Matrix6d::Zero();
		int child_jindex;
		for (int ci = 0; ci < S->childIndex[jindex].size(); ++ci)
		{
			child_jindex = S->childIndex[jindex][ci];
			accum_adj = accum_adj + S->ad_jpT[child_jindex] * S->Pi[child_jindex] * S->ad_jp[child_jindex];
		}

		// toggle use of damping and stiffness blkdiag in Minv
		if (blkdiag)
		{
			S->Mhat[jindex] = S->I_j[jindex] + SS->h * S->ad_ijT[jindex] * (-S->Dmd[jindex] + S->D[jindex] - SS->h * S->Kmd[jindex]) * S->ad_ij[jindex] + accum_adj;
		}
		else
			S->Mhat[jindex] = S->I_j[jindex] + accum_adj;

		// fixed joints do not have index in reduced state
		if (S->type[jindex] != Joint::JType::Fixed)
		{
			S->Psi[jindex] = (S->ST[jindex] * S->Mhat[jindex] * S->S[jindex] + S->Pr[jindex]).inverse();
			S->Pi[jindex] = S->Mhat[jindex] - S->Mhat[jindex] * S->S[jindex] * S->Psi[jindex] * S->ST[jindex] * S->Mhat[jindex];
		}

		// precomputations
		S->ST_Mhat[jindex] = S->ST[jindex] * S->Mhat[jindex];
		S->S_Psi[jindex] = S->S[jindex] * S->Psi[jindex];
		S->Mhat_S_Psi[jindex] = S->Mhat[jindex] * S->S_Psi[jindex];

		//Matrix6d accumsoa = accum_adj;
		//int list_index = (int)LS->joint_map[jmapi];
		//std::shared_ptr<Joint> j = LS->joints[list_index];
		//std::shared_ptr<Block> child = j->childBlock;
		//int childIndex = j->jindex;
		//// Maximal and Reduced damping terms
		//// updated outside preconditioner
		//accum_adj = Matrix6d::Zero();
		//for (int ci = 0; ci < child->c_joints.size(); ++ci)
		//{
		//	std::shared_ptr<Joint> j_child = child->c_joints[ci];
		//	accum_adj = accum_adj + j_child->ad_jpT * j_child->Pi * j_child->ad_jp;
		//}
		//// toggle use of damping and stiffness blkdiag in Minv
		//if (blkdiag)
		//{
		//	j->Mhat = j->I_j + SS->h * j->ad_ijT * (-j->Dmd + child->D - SS->h * j->Kmd) * j->ad_ij + accum_adj;
		//}
		//else
		//	j->Mhat = j->I_j + accum_adj;
		//// fixed joints do not have index in reduced state
		//if (j->type != Joint::JType::Fixed)
		//{
		//	j->Psi = (j->ST * j->Mhat * j->S + j->Pr).inverse();
		//	j->Pi = j->Mhat - j->Mhat * j->S * j->Psi * j->ST * j->Mhat;
		//}
		//// precomputations
		//j->ST_Mhat = j->ST * j->Mhat;
		//j->S_Psi = j->S * j->Psi;
		//j->Mhat_S_Psi = j->Mhat * j->S_Psi;

		//std::cout << j->childName << "---------------------------------------" << std::endl;
		//std::cout << "Dmd:" << std::endl;
		//for (int i = 0; i < j->Dmd.rows(); ++i)
		//{
		//	for (int k = 0; k < j->Dmd.cols(); ++k)
		//	{
		//		double diff = j->Dmd(i, k) - S->Dmd[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "Kmd:" << std::endl;
		//for (int i = 0; i < j->Kmd.rows(); ++i)
		//{
		//	for (int k = 0; k < j->Kmd.cols(); ++k)
		//	{
		//		double diff = j->Kmd(i, k) - S->Kmd[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "ad_ij:" << std::endl;
		//for (int i = 0; i < j->ad_ij.rows(); ++i)
		//{
		//	for (int k = 0; k < j->ad_ij.cols(); ++k)
		//	{
		//		double diff = j->ad_ij(i, k) - S->ad_ij[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "ad_ijT:" << std::endl;
		//for (int i = 0; i < j->ad_ijT.rows(); ++i)
		//{
		//	for (int k = 0; k < j->ad_ijT.cols(); ++k)
		//	{
		//		double diff = j->ad_ijT(i, k) - S->ad_ijT[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "I_j:" << std::endl;
		//for (int i = 0; i < j->I_j.rows(); ++i)
		//{
		//	for (int k = 0; k < j->I_j.cols(); ++k)
		//	{
		//		double diff = j->I_j(i, k) - S->I_j[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "accum_adj:" << std::endl;
		//for (int i = 0; i < accum_adj.rows(); ++i)
		//{
		//	for (int k = 0; k < accum_adj.cols(); ++k)
		//	{
		//		double diff = accum_adj(i, k) - accumsoa(i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "Mhat:" << std::endl;
		//for (int i = 0; i < j->Mhat.rows(); ++i)
		//{
		//	for (int k = 0; k < j->Mhat.cols(); ++k)
		//	{
		//		double diff = j->Mhat(i, k) - S->Mhat[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "ST_Mhat:" << std::endl;
		//for (int i = 0; i < j->ST_Mhat.rows(); ++i)
		//{
		//	for (int k = 0; k < j->ST_Mhat.cols(); ++k)
		//	{
		//		double diff = j->ST_Mhat(i, k) - S->ST_Mhat[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "S_Psi:" << std::endl;
		//for (int i = 0; i < j->S_Psi.rows(); ++i)
		//{
		//	for (int k = 0; k < j->S_Psi.cols(); ++k)
		//	{
		//		double diff = j->S_Psi(i, k) - S->S_Psi[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "Mhat_S_Psi:" << std::endl;
		//for (int i = 0; i < j->Mhat_S_Psi.rows(); ++i)
		//{
		//	for (int k = 0; k < j->Mhat_S_Psi.cols(); ++k)
		//	{
		//		double diff = j->Mhat_S_Psi(i, k) - S->Mhat_S_Psi[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
	}
}

Eigen::VectorXd ConstraintJoint::computeMinv_x(const Eigen::VectorXd & x, const std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S, std::shared_ptr<State::local_mt> lmt)
{
	int jindex;
	Eigen::VectorXd Minv_x = Eigen::VectorXd::Zero(x.size());

	// backward traversal
	Eigen::VectorXd x_STB;
	for (int jmapi = (int)LS->joints.size() - 1; jmapi >= 0; --jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		// Bhat_j
		Vector6d Bhat = Vector6d::Zero();
		int child_jindex;
		for (int ci = 0; ci < S->childIndex[jindex].size(); ++ci)
		{
			child_jindex = S->childIndex[jindex][ci];
			// beta changes throughout Minv solve
			if (lmt == nullptr)
				Bhat.noalias() += S->ad_jpT[child_jindex] * S->beta[child_jindex];
			else
				Bhat.noalias() += S->ad_jpT[child_jindex] * lmt->beta_[child_jindex];
		}
		if (lmt == nullptr)
		{
			S->Bhat[jindex] = Bhat;
			S->ST_Bhat[jindex] = S->ST[jindex] * Bhat;
			x_STB = S->ST_Bhat[jindex];
		}
		else
		{
			lmt->Bhat_[jindex] = Bhat;
			lmt->ST_Bhat_[jindex] = S->ST[jindex] * Bhat;
			x_STB = lmt->ST_Bhat_[jindex];
		}

		// beta_j
		for (int i = 0; i < S->constraintNum[jindex]; ++i)
		{
			x_STB[i] = x[S->constraint_index[jindex] + i] - x_STB[i];
		}
		Vector6d beta = Bhat;
		if (S->type[jindex] != Joint::JType::Fixed)
		{
			beta.noalias() += S->Mhat_S_Psi[jindex] * x_STB;
		}
		if (lmt == nullptr)
			S->beta[jindex] = beta;
		else
			lmt->beta_[jindex] = beta;

		//lmt = nullptr;

		//int list_index = (int)LS->joint_map[jmapi];
		//std::shared_ptr<Joint> j = LS->joints[list_index];
		//std::shared_ptr<Block> child = j->childBlock;
		//int childIndex = j->jindex;
		//std::cout << j->childName << "---------------------------------------" << std::endl;
		//std::cout << "CHILD CHECK:" << std::endl;
		//// Bhat_j
		//Bhat = Vector6d::Zero();
		//for (int ci = 0; ci < child->c_joints.size(); ++ci)
		//{
		//	// beta changes throughout Minv solve
		//	if (lmt == nullptr)
		//		Bhat.noalias() += child->c_joints[ci]->ad_jpT * child->c_joints[ci]->beta;
		//	else
		//		Bhat.noalias() += child->c_joints[ci]->ad_jpT * lmt->beta_[child->c_joints[ci]->jindex];

		//	int child_jindex = S->childIndex[jindex][ci];
		//	Vector6d tempj = child->c_joints[ci]->ad_jpT * child->c_joints[ci]->beta;
		//	Vector6d tempsoa = S->ad_jpT[child_jindex] * S->beta[child_jindex];
		//	//std::cout << child->c_joints[ci]->ad_jpT << std::endl;
		//	//std::cout << S->ad_jpT[child_jindex] << std::endl;
		//	for (int i = 0; i < j->beta.size(); ++i)
		//	{
		//		double diff = child->c_joints[ci]->beta[i] - S->beta[child_jindex][i];
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << ci << ":beta: " << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//	std::cout << "   ";
		//	for (int i = 0; i < 6; ++i)
		//	{
		//		for (int k = 0; k < 6; ++k)
		//		{
		//			double diff = child->c_joints[ci]->ad_jpT(i,k) - S->ad_jpT[child_jindex](i,k);
		//			if (isnan(diff))
		//				std::cout << "NaN ";
		//			else if (std::abs(diff) > 1e-8)
		//				std::cout << ci << ":ad_jp: " << diff << "      ";
		//			//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//		}
		//	}
		//	std::cout << "   ";
		//	for (int i = 0; i < j->Bhat.size(); ++i)
		//	{
		//		double diff = tempj[i] - tempsoa[i];
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << ci << ":Bhat: " << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//	std::cout << "   ";
		//}
		//std::cout << std::endl;
		//if (lmt == nullptr)
		//{
		//	j->Bhat = Bhat;
		//	j->ST_Bhat = j->ST * Bhat;
		//	x_STB = j->ST_Bhat;
		//}
		//else
		//{
		//	lmt->Bhat_[childIndex] = Bhat;
		//	lmt->ST_Bhat_[childIndex] = j->ST * Bhat;
		//	x_STB = lmt->ST_Bhat_[childIndex];
		//}

		//// beta_j
		//for (int i = 0; i < j->constraintNum; ++i)
		//{
		//	x_STB[i] = x[j->constraint_index + i] - x_STB[i];
		//}
		//beta = Bhat;
		//if (j->type != Joint::JType::Fixed)
		//{
		//	beta.noalias() += j->Mhat_S_Psi * x_STB;
		//}
		//if (lmt == nullptr)
		//	j->beta = beta;
		//else
		//	lmt->beta_[childIndex] = beta;

		//std::cout << "Mhat_S_Psi:" << std::endl;
		//for (int i = 0; i < j->Mhat_S_Psi.rows(); ++i)
		//{
		//	for (int k = 0; k < j->Mhat_S_Psi.cols(); ++k)
		//	{
		//		double diff = j->Mhat_S_Psi(i, k) - S->Mhat_S_Psi[jindex](i, k);
		//		if (isnan(diff))
		//			std::cout << "NaN ";
		//		else if (std::abs(diff) > 1e-8)
		//			std::cout << diff << "      ";
		//		//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//	}
		//}
		//std::cout << std::endl;
		//std::cout << "Bhat:" << std::endl;
		//for (int i = 0; i < j->Bhat.size(); ++i)
		//{
		//	double diff = j->Bhat[i] - S->Bhat[jindex][i];
		//	if (isnan(diff))
		//		std::cout << "NaN ";
		//	else if (std::abs(diff) > 1e-8)
		//		std::cout << diff << "      ";
		//	//std::cout << Jdot_x[i] << " - " << Jdot_x_SoA[i] << " = " << diff << "      ";
		//}
		//std::cout << std::endl;

	}

	// forward traversal
	Eigen::VectorXd y;	// qddot
	for (int jmapi = 0; jmapi < LS->joints.size(); ++jmapi)
	{
		jindex = S->jindex[LS->joint_map[jmapi]];

		if (lmt == nullptr)
			x_STB = S->ST_Bhat[jindex];
		else
			x_STB = lmt->ST_Bhat_[jindex];
		for (int i = 0; i < S->constraintNum[jindex]; ++i)
		{
			x_STB[i] = x[S->constraint_index[jindex] + i] - x_STB[i];
		}

		if (lmt == nullptr)
			S->Vdot[jindex] = Vector6d::Zero();
		else
			lmt->Vdot_[jindex] = Vector6d::Zero();
		if (!S->root[jindex])
		{
			if (lmt == nullptr)
			{
				S->Vdot[jindex] = S->ad_jp[jindex] * S->Vdot[S->parentIndex[jindex]];
				x_STB -= S->ST_Mhat[jindex] * S->Vdot[jindex];
			}
			else
			{
				lmt->Vdot_[jindex] = S->ad_jp[jindex] * lmt->Vdot_[S->parentIndex[jindex]];
				x_STB -= S->ST_Mhat[jindex] * lmt->Vdot_[jindex];
			}
		}

		y = S->Psi[jindex] * x_STB;
		if (S->type[jindex] != Joint::JType::Fixed)
		{
			if (lmt == nullptr)
				S->Vdot[jindex] += S->S[jindex]*y;
			else
				lmt->Vdot_[jindex] += S->S[jindex]*y;
		}
		else
		{
			if (lmt == nullptr)
				S->Vdot[jindex] = Vector6d::Zero();
			else
				lmt->Vdot_[jindex] = Vector6d::Zero();
		}

		// load into result vector
		for (int i = 0; i < y.size(); ++i)
		{
			Minv_x[S->constraint_index[jindex] + i] = y[i];
		}
	}
	return Minv_x;
}

void ConstraintJoint::update(std::unique_ptr<StateSolve> &SS, const std::unique_ptr<LinkageSystem> &LS, const std::unique_ptr<State> &S)
{
	switch (simtype)
	{
	case simType::Pardiso:
		updateJacobian(SS, LS, S);
		break;
	default:
		throw std::runtime_error("Solver type has no implementation for joint constraints");
	}
}

void ConstraintJoint::draw(const std::unique_ptr<StateSolve> &SS, const std::unique_ptr<LinkageSystem> & LS, const std::unique_ptr<State> & S)
{
#ifdef ONLINE_MODE
	Eigen::Vector3d ppos, cpos;
	Eigen::Matrix4d E = Eigen::Matrix4d::Identity();
	glm::mat4 temp;
	int parenti, childi;
	std::shared_ptr<Joint> j;

	for (int jindex = 0; jindex < LS->joints.size(); ++jindex)
	{
		j = LS->joints[jindex];

		if (!j->root)
		{
			parenti = LS->find(j->parentName)->joint->jindex;
			E = S->E[parenti];

			ppos = E.block<3, 3>(0, 0)*j->parentPos + E.block<3, 1>(0, 3);
		}
		else
		{
			ppos = j->parentPos;
		}

		glLineWidth(8);
		glBegin(GL_LINES);
		glVertex3f((float)ppos[0], (float)ppos[1], (float)ppos[2]);
		if (j->type == Joint::JType::Hinge)
		{
			ppos = ppos + j->S.block<3, 1>(0, 0) * 0.5;
		}
		else
		{
			ppos = ppos + Eigen::Vector3d(0,0,0.5);
		}
		glVertex3f((float)ppos[0], (float)ppos[1], (float)ppos[2]);
		glEnd();
		glLineWidth(1);

		childi = LS->find(j->childName)->joint->jindex;
		E = S->E[childi];

		cpos = E.block<3, 3>(0, 0)*j->childPos + E.block<3, 1>(0, 3);
		glLineWidth(4);
		glBegin(GL_LINES);
		glVertex3f((float)cpos[0], (float)cpos[1], (float)cpos[2]);
		if (j->type == Joint::JType::Hinge)
		{
			cpos = cpos - j->S.block<3, 1>(0, 0) * 0.5;
		}
		else
		{
			cpos = cpos - Eigen::Vector3d(0, 0, 0.5);
		}
		glVertex3f((float)cpos[0], (float)cpos[1], (float)cpos[2]);
		glEnd();
		glLineWidth(1);
	}
#endif // ONLINE_MODE
}


/*
 * Copyright (c) 2017 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#ifndef INCLUDED_PKMN_RNG_HPP
#define INCLUDED_PKMN_RNG_HPP

#include <pkmn/config.hpp>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <limits>

namespace pkmn {

template <typename T>
class rng {
    public:
        inline rng() {
            _rng.seed(uint32_t(
                boost::posix_time::microsec_clock::universal_time().time_of_day().total_microseconds() %
                std::numeric_limits<uint32_t>::max()
            ));
        }

        inline T rand(
            T min = std::numeric_limits<T>::min(),
            T max = std::numeric_limits<T>::max()
        ) {
            return boost::random::uniform_int_distribution<T>(min, max)(_rng);
        }

    private:
        boost::random::mt19937 _rng;
};

}

#endif /* INCLUDED_PKMN_RNG_HPP */


//
// Created by jianping on 17-9-11.
//

#include <iostream>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <fstream>
#include <Eigen/Geometry>

//image intrinsics
//reference image r,t
//source image r,t
//https://github.com/opencv/opencv/blob/master/samples/cpp/stereo_match.cpp

void saveXYZ(const char* filename, const cv::Mat& mat,float tx)
{
    const double max_z = 10000;
    FILE* fp = fopen(filename, "wt");
    for(int y = 0; y < mat.rows; y++)
    {
        for(int x = 0; x < mat.cols; x++)
        {
            cv::Vec3f point = mat.at<cv::Vec3f>(y, x);
            if(fabs(fabs(point[2]) - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
        }
    }
    fclose(fp);
}

void readParams(const std::string& intrinsicFile,
                Eigen::Matrix3d& K,
                Eigen::Matrix3d& C,
                Eigen::Vector3d& t)
{
    std::ifstream ifs(intrinsicFile.c_str(),std::ios_base::in);
    char line[256];
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(0,0),&K(0,1),&K(0,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(1,0),&K(1,1),&K(1,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(2,0),&K(2,1),&K(2,2));
    ifs.getline(line,256);//useless
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(0,0),&C(0,1),&C(0,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(1,0),&C(1,1),&C(1,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(2,0),&C(2,1),&C(2,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&t(0),&t(1),&t(2));
    std::cout<<K<<"\n"<<C<<"\n"<<t.transpose()<<"\n";

}

const int scale = 1;
int main()
{
    LOG(INFO)<<"matching"<<std::endl;
    cv::Mat image_ref_origin = cv::imread("rdimage.001.ppm");
    cv::Mat image_source_origin = cv::imread("rdimage.000.ppm");
    cv::Mat image_ref,image_source;

    cv::resize(image_ref_origin,image_ref,
               cv::Size(image_ref_origin.cols/scale,image_ref_origin.rows/scale),
               0,0,cv::INTER_CUBIC);
    cv::resize(image_source_origin,image_source,
               cv::Size(image_ref_origin.cols/scale,image_ref_origin.rows/scale),
               0,0,cv::INTER_CUBIC);

    Eigen::Matrix3d K0,C0,K1,C1,C;
    Eigen::Vector3d r0,r1,r;
    readParams("rdimage.001.ppm.camera",K0,C0,r0);
    readParams("rdimage.000.ppm.camera",K1,C1,r1);
    {
        K0(0,0)/=scale;
        K0(1,1)/=scale;
        K0(0,2)/=scale;
        K0(1,2)/=scale;
        K1(0,0)/=scale;
        K1(1,1)/=scale;
        K1(0,2)/=scale;
        K1(1,2)/=scale;
    }
    //C = C0.transpose()*C1;
    //r = C0.transpose()*(r1-r0);
    C = C1.transpose()*C0;// transform p from frame 0 to frame 1
    r = C1.transpose()*(r0-r1);
    Eigen::Quaterniond q(C);
    C = q.toRotationMatrix();
    cv::Mat Q;
    cv::Rect roi1, roi2;
    cv::Mat M0,M1,R01(3,3,CV_64FC1),t(3,1,CV_64FC1),R0__,R1__,P0__,P1__;
    cv::Mat D0(5,1,CV_64FC1),D1(5,1,CV_64FC1);
    D0.setTo(0);
    D1.setTo(0);
    cv::eigen2cv(K0,M0);
    cv::eigen2cv(K1,M1);
    cv::eigen2cv(C,R01);
    cv::eigen2cv(r,t);

    cv::Size size(image_ref.cols, image_ref.rows);
    cv::stereoRectify(M0,D0,M1,D1,
                      size,R01,t,
                      R0__,R1__,
                      P0__,P1__,
                      Q,cv::CALIB_FIX_INTRINSIC,-1,
                      size, &roi1, &roi2
    );

    cv::Mat map11, map12, map21, map22;
    cv::initUndistortRectifyMap(M0, D0, R0__, P0__, size, CV_16SC2, map11, map12);
    cv::initUndistortRectifyMap(M1, D1, R1__, P1__, size, CV_16SC2, map21, map22);
    //std::cout<<P0__<<"\n"<<R1__<<std::endl;
    cv::Mat img1r, img2r;
    cv::remap(image_ref, img1r, map11, map12, cv::INTER_LINEAR);
    cv::remap(image_source, img2r, map21, map22, cv::INTER_LINEAR);

    image_ref = img1r;
    image_source = img2r;

//    cv::resize(img1r,image_ref,cv::Size(size.width/3,size.height/3));
//    cv::resize(img2r,image_source,cv::Size(size.width/3,size.height/3));

    //cv::waitKey(0);

    cv::StereoSGBM sgbm(-512,1024,
                        8,8*3*3*3,32*3*3*3,2,
                        16,5,100,
                        2,false
    );

    cv::Mat disp,disp8,dispnorm(image_ref.rows,image_ref.cols,CV_64FC1);
    sgbm(image_ref,image_source,disp);

//    cv::normalize(disp,dispnorm,255,0,cv::NORM_MINMAX);
//    dispnorm.convertTo(disp8, CV_8U);
//    cv::namedWindow("re",cv::WINDOW_NORMAL);
//    cv::imshow("re",disp8);

    cv::namedWindow("ref",cv::WINDOW_NORMAL);
    cv::namedWindow("source",cv::WINDOW_NORMAL);

    for (int i = 0; i < image_ref.rows; i += image_ref.rows/20 ) {
        cv::line(image_ref,cv::Point(0,i),cv::Point(image_ref.cols,i),cv::Scalar(0,255,0),1);
        cv::line(image_source,cv::Point(0,i),cv::Point(image_ref.cols,i),cv::Scalar(0,255,0),1);

    }

    cv::imshow("ref",image_ref);
    cv::imshow("source",image_source);

    cv::Mat xyz,xyz8;
    reprojectImageTo3D(disp, xyz, Q, false);

    saveXYZ("./pts.xyz", xyz, r(0));
    cv::waitKey(0);
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_CBRT_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_CBRT_HPP_INCLUDED

#include <nt2/exponential/functions/cbrt.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/real_splat.hpp>
#include <nt2/include/constants/third.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/bitofsign.hpp>
#include <nt2/include/functions/simd/bitwise_or.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/fast_frexp.hpp>
#include <nt2/include/functions/simd/fast_ldexp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/is_gez.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/negate.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <nt2/include/constants/smallestposval.hpp>
#include <nt2/include/constants/twotomnmbo_3.hpp>
#include <nt2/include/constants/twotonmb.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( cbrt_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<double_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type             stype;
      typedef typename meta::as_logical<int_type>::type     b_type;
      A0 z =  nt2::abs(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      typedef typename meta::as_logical<A0>::type             b_A0;
      b_A0 denormal = lt(z, Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
#endif
      const A0 CBRT2  = double_constant< A0, 0x3ff428a2f98d728bll> ();
      const A0 CBRT4  = double_constant< A0, 0x3ff965fea53d6e3dll> ();
      const A0 CBRT2I = double_constant< A0, 0x3fe965fea53d6e3dll> ();
      const A0 CBRT4I = double_constant< A0, 0x3fe428a2f98d728bll> ();
      int_type e;
      A0  x = fast_frexp(z, e);
      x = horner < NT2_HORNER_COEFF_T(stype, 5,
                            (0xbfc13c93386fdff6ll,
                             0x3fe17e1fc7e59d58ll,
                             0xbfee8a4ca3ba37b8ll,
                             0x3ff23d6ee505873all,
                             0x3fd9c0c12122a4fell)
                            ) > (x);
      const b_type flag = is_gez(e);
      int_type e1 =  nt2::abs(e);
      int_type rem = e1;
      e1 /= Three<int_type>();
      rem -= e1*Three<int_type>();
      e =  negate(e1, e);
      const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
      const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
      A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
      fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
      x = fast_ldexp(x*fact, e);
      x -= (x-z/sqr(x))*Third<A0>();
      x -= (x-z/sqr(x))*Third<A0>(); //two newton passes
#ifndef BOOST_SIMD_NO_DENORMALS
      x = b_or(x, bitofsign(a0))*f;
#else
      x = b_or(x, bitofsign(a0));
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
      return if_else(l_or(is_eqz(a0),is_inf(a0)), a0, x);
#else
      return if_else(is_eqz(a0), a0, x);
#endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( cbrt_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<single_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      A0 z =  nt2::abs(a0);
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type             stype;
      typedef typename meta::as_logical<int_type>::type     b_type;
#ifndef BOOST_SIMD_NO_DENORMALS
      typedef typename meta::as_logical<A0>::type             b_A0;
      b_A0 denormal = lt(z, Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
#endif
      const A0 CBRT2  = single_constant< A0, 0x3fa14518> ();
      const A0 CBRT4  = single_constant< A0, 0x3fcb2ff5> ();
      const A0 CBRT2I = single_constant< A0, 0x3f4b2ff5> ();
      const A0 CBRT4I = single_constant< A0, 0x3f214518> ();
      int_type e;
      A0 x = fast_frexp(z, e);
      x = horner < NT2_HORNER_COEFF_T(stype, 5,
                            (0xbe09e49a,
                             0x3f0bf0fe,
                             0xbf745265,
                             0x3f91eb77,
                             0x3ece0609)
                            ) > (x);
      const b_type flag = is_gez(e);
      int_type e1 =  nt2::abs(e);
      int_type rem = e1;
      e1 /= Three<int_type>();
      rem -= e1*Three<int_type>();
      e = negate(e1, e);

      const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
      const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
      A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
      fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
      x = fast_ldexp(x*fact, e);
      x -= (x-z/sqr(x))*Third<A0>();
#ifndef BOOST_SIMD_NO_DENORMALS
      x = b_or(x, bitofsign(a0))*f;
#else
      x = b_or(x, bitofsign(a0));
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
      return if_else(l_or(is_eqz(a0),is_inf(a0)), a0, x);
#else
      return if_else(is_eqz(a0), a0, x);
#endif
    }
  };
} }

#endif


//    Copyright 2021 Jij Inc.

//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at

//        http://www.apache.org/licenses/LICENSE-2.0

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

#ifndef OPENJIJ_GRAPH_DENSE_HPP__
#define OPENJIJ_GRAPH_DENSE_HPP__

#include <map>
#include <vector>
#include <cassert>
#include <cstddef>
#include <type_traits>
#include <algorithm>
#include <exception>

#include <utility/disable_eigen_warning.hpp>
#include <Eigen/Dense>

#include <graph/json/parse.hpp>
#include <graph/graph.hpp>

namespace openjij {
    namespace graph {

        /**
         * @brief two-body all-to-all interactions
         * The Hamiltonian is like
         * \f[
         * H = \sum_{i<j}J_{ij} \sigma_i \sigma_j + \sum_{i}h_{i} \sigma_i
         * \f]
         *
         * @tparam FloatType float type of Sparse class (double or float)
         */
        template<typename FloatType>
            class Dense : public Graph{
                static_assert(std::is_floating_point<FloatType>::value, "FloatType must be floating-point type.");
                public:

                    /**
                     * @brief interaction type (Eigen)
                     * The stored matrix has the following triangular form:
                     *
                     * \f[
                     * \begin{pmatrix}
                     * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
                     * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
                     * \vdots & \vdots & \vdots & \vdots & \vdots \\
                     * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
                     * 0 & 0 & \cdots & 0 & 1 \\
                     * \end{pmatrix}
                     * \f]
                     */
                    using Interactions = Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

                    /**
                     * @brief float type
                     */
                    using value_type = FloatType;

                private:

                    /**
                     * @brief interactions
                     */
                    Interactions _J;

                public:

                    /**
                     * @brief Dense constructor
                     *
                     * @param num_spins the number of spins
                     */
                    explicit Dense(std::size_t num_spins)
                        : Graph(num_spins), _J(Interactions::Zero(num_spins+1, num_spins+1)){
                            _J(num_spins, num_spins) = 1;
                        }

                    /**
                     * @brief Dense constructor (from nlohmann::json)
                     *
                     * @param j JSON object this object must be a serialized object of dense BQM.
                     */
                    Dense(const json& j) : Dense(static_cast<size_t>(j["num_variables"])){
                        //define bqm with ising variables
                        auto bqm = json_parse<FloatType, cimod::Dense>(j);
                        //interactions
                        //for(auto&& elem : bqm.get_quadratic()){
                        //    const auto& key = elem.first;
                        //    const auto& val = elem.second;
                        //    J(key.first, key.second) += val;
                        //}
                        //local field
                        //for(auto&& elem : bqm.get_linear()){
                        //    const auto& key = elem.first;
                        //    const auto& val = elem.second;
                        //    h(key) += val;
                        //}
                        // the above insertion is simplified as
                        this->_J = bqm.interaction_matrix();
                    }

                    /**
                     * @brief set interaction matrix from Eigen Matrix.
                     *
                     * @param interaction Eigen matrix
                     */
                    void set_interaction_matrix(const Interactions& interaction){
                        if(interaction.rows() != interaction.cols()){
                            std::runtime_error("interaction.rows() != interaction.cols()");
                        }

                        if((size_t)interaction.rows() != get_num_spins() + 1){
                            throw std::runtime_error("invalid matrix size.");
                        }

                        //check if diagonal elements are zero
                        for(size_t i=0; i<(size_t)(interaction.rows()-1); i++){
                            if(interaction(i,i) != 0){
                                throw std::runtime_error("The diagonal elements of interaction matrix must be zero.");
                            }
                        }

                        if(interaction(interaction.rows()-1,interaction.rows()-1) != 1){
                            throw std::runtime_error("The right bottom element of interaction matrix must be unity.");
                        }

                        _J = interaction.template selfadjointView<Eigen::Upper>();
                    }

                    /**
                     * @brief Dense copy constructor
                     */
                    Dense(const Dense<FloatType>&) = default;

                    /**
                     * @brief Dense move constructor
                     */
                    Dense(Dense<FloatType>&&) = default;

                    /**
                     * @brief calculate total energy
                     *
                     * @param spins
                     * @deprecated use energy(spins)
                     *
                     * @return corresponding energy
                     */
                    FloatType calc_energy(const Spins& spins) const{
                        return this->energy(spins);
                    }

                    FloatType calc_energy(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>& spins) const{
                        return this->energy(spins);
                    }

                    /**
                     * @brief calculate total energy
                     *
                     * @param spins
                     *
                     * @return corresponding energy
                     */
                    FloatType energy(const Spins& spins) const{
                        if(spins.size() != this->get_num_spins()){
                            throw std::out_of_range("Out of range in energy in Dense graph.");
                        }

                        using Vec = Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>;
                        Vec s(get_num_spins()+1);
                        for(size_t i=0; i<spins.size(); i++){
                            s(i) = spins[i];
                        }
                        s(get_num_spins()) = 1;

                        // the energy must be consistent with BinaryQuadraticModel.
                        return (s.transpose()*(_J.template triangularView<Eigen::Upper>()*s))(0,0)-1;
                    }

                    FloatType energy(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>& spins) const{
                        graph::Spins temp_spins(get_num_spins());
                        for(size_t i=0; i<temp_spins.size(); i++){
                            temp_spins[i] = spins(i);
                        }
                        return energy(temp_spins);

                    }

                    /**
                     * @brief access J_{ij}
                     *
                     * @param i Index i
                     * @param j Index j
                     *
                     * @return J_{ij}
                     */
                    FloatType& J(Index i, Index j){
                        assert(i < get_num_spins());
                        assert(j < get_num_spins());

                        if(i != j)
                            return _J(std::min(i, j), std::max(i, j));
                        else
                            return _J(std::min(i, j), get_num_spins());
                    }

                    /**
                     * @brief access J_{ij}
                     *
                     * @param i Index i
                     * @param j Index j
                     *
                     * @return J_{ij}
                     */
                    const FloatType& J(Index i, Index j) const{
                        assert(i < get_num_spins());
                        assert(j < get_num_spins());

                        if(i != j)
                            return _J(std::min(i, j), std::max(i, j));
                        else
                            return _J(std::min(i, j), get_num_spins());
                    }

                    /**
                     * @brief access h_{i} (local field)
                     *
                     * @param i Index i
                     *
                     * @return h_{i}
                     */
                    FloatType& h(Index i){
                        assert(i < get_num_spins());
                        return J(i, i);
                    }

                    /**
                     * @brief access h_{i} (local field)
                     *
                     * @param i Index i
                     *
                     * @return h_{i}
                     */
                    const FloatType& h(Index i) const{
                        assert(i < get_num_spins());
                        return J(i, i);
                    }

                    /**
                     * @brief get interactions (Eigen Matrix)
                     *
                     * The returned matrix has the following symmetric form:
                     *
                     * \f[
                     * \begin{pmatrix}
                     * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
                     * J_{0,1} & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
                     * \vdots & \vdots & \vdots & \vdots & \vdots \\
                     * J_{0,N-1} & J_{N-1,1} & \cdots & J_{N-1,N-1} & h_{N-1}\\
                     * h_{0} & h_{1} & \cdots & h_{N-1} & 1 \\
                     * \end{pmatrix}
                     * \f]
                     *
                     * @return Eigen Matrix
                     */
                    const Interactions get_interactions() const{
                        return this->_J.template selfadjointView<Eigen::Upper>();
                    }

            };
    } // namespace graph
} // namespace openjij

#endif


// Copyright (C) 2016 Kasper Kristensen
// License: GPL-2

#ifndef TINY_AD_H
#define TINY_AD_H

/* Standalone ? */
#ifndef R_RCONFIG_H
#include <cmath>
#include <iostream>
#define CSKIP(x) x
#endif

/* Select the vector class to use (Default: tiny_vec) */
#if defined(TINY_AD_USE_STD_VALARRAY)
#include "tiny_valarray.hpp"
#define TINY_VECTOR(type,size) tiny_vector<type, size>
#elif defined(TINY_AD_USE_EIGEN_VEC)
#include <Eigen/Dense>
#define TINY_VECTOR(type,size) Eigen::Array<type, size, 1>
#else
#include "tiny_vec.hpp"
#define TINY_VECTOR(type,size) tiny_vec<type, size>
#endif

namespace tiny_ad {
  template<class Type, class Vector>
  struct ad {
    Type value;
    Vector deriv;
    ad(){}
    ad(Type v, Vector d){value = v; deriv = d;}
    ad(double v)        {value = v; deriv.setZero();}
    ad operator+ (const ad &other) const{
      return ad(value + other.value,
		deriv + other.deriv);
    }
    ad operator+ () const{
      return *this;
    }
    ad operator- (const ad &other) const{
      return ad(value - other.value,
		deriv - other.deriv);
    }
    ad operator- () const{
      return ad(-value, -deriv);
    }
    ad operator* (const ad &other) const{
      return ad(value * other.value,
		value * other.deriv +
		deriv * other.value);
    }
    ad operator/ (const ad &other) const{
      Type res = value / other.value;
      return ad(res,
		(deriv - res * other.deriv) /
		other.value );
    }
    /* Comparison operators */
#define COMPARISON_OPERATOR(OP)			\
    template<class other>			\
    bool operator OP (const other &x) const{	\
      return (value OP x);			\
    }
    COMPARISON_OPERATOR(<)
    COMPARISON_OPERATOR(>)
    COMPARISON_OPERATOR(<=)
    COMPARISON_OPERATOR(>=)
    COMPARISON_OPERATOR(==)
    COMPARISON_OPERATOR(!=)
#undef COMPARISON_OPERATOR
    /* Combine ad with other types (constants) */
    ad operator+ (const double &x) const{
      return ad(value + x, deriv);
    }
    ad operator- (const double &x) const{
      return ad(value - x, deriv);
    }
    ad operator* (const double &x) const{
      return ad(value * x, deriv * x);
    }
    ad operator/ (const double &x) const{
      return ad(value / x, deriv / x);
    }
    /* Note: 'this' and 'other' may point to the same object */
    ad& operator+=(const ad &other){
      value += other.value;
      deriv += other.deriv;
      return *this;
    }
    ad& operator-=(const ad &other){
      value -= other.value;
      deriv -= other.deriv;
      return *this;
    }
    ad& operator*=(const ad &other){
      if (this != &other) {
	deriv *= other.value;
	deriv += other.deriv * value;
	value *= other.value;
      } else {
	deriv *= value * 2.;
	value *= value;
      }
      return *this;
    }
    ad& operator/=(const ad &other){
      value /= other.value;
      deriv -= other.deriv * value;
      deriv /= other.value;
      return *this;
    }
  };
  /* Binary operators where a constant is first argument */
  template<class T, class V>
  ad<T, V> operator+ (const double &x, const ad<T, V> &y) {
    return y + x;
  }
  template<class T, class V>
  ad<T, V> operator- (const double &x, const ad<T, V> &y) {
    return -(y - x);
  }
  template<class T, class V>
  ad<T, V> operator* (const double &x, const ad<T, V> &y) {
    return y * x;
  }
  template<class T, class V>
  ad<T, V> operator/ (const double &x, const ad<T, V> &y) {
    T value = x / y.value;
    return ad<T, V>(value, T(-value / y.value) * y.deriv);
  }
  /* Unary operators with trivial derivatives */
#define UNARY_MATH_ZERO_DERIV(F)		\
  template<class T, class V>			\
  double F (const ad<T, V> &x){			\
    return F(x.value);				\
  }
  using ::floor; using ::ceil;
  using ::trunc; using ::round;
  UNARY_MATH_ZERO_DERIV(floor)
  UNARY_MATH_ZERO_DERIV(ceil)
  UNARY_MATH_ZERO_DERIV(trunc)
  UNARY_MATH_ZERO_DERIV(round)
  template<class T>
  double sign(const T &x){return (x > 0) - (x < 0);}
  bool isfinite(const double &x)CSKIP( {return std::isfinite(x);} )
  template<class T, class V>
  bool isfinite(const ad<T, V> &x){return isfinite(x.value);}
#undef UNARY_MATH_ZERO_DERIV
  /* Unary operators with non-trivial derivatives */
#define UNARY_MATH_DERIVATIVE(F,DF)		\
  template<class T, class V>			\
  ad<T, V> F (const ad<T, V> &x){		\
    return ad<T, V>(F (x.value),		\
		    T(DF(x.value)) * x.deriv);	\
  }
  using ::exp;  using ::log;
  using ::sin;  using ::cos;  using ::tan;
  using ::sinh; using ::cosh; using ::tanh;
  using ::sqrt; using ::fabs;
  template<class T> T D_tan(const T &x) {
    T y = cos(x); return 1. / (y * y);
  }
  template<class T> T D_tanh(const T &x) {
    T y = cosh(x); return 1. / (y * y);
  }
  UNARY_MATH_DERIVATIVE(exp, exp)
  UNARY_MATH_DERIVATIVE(log, 1.0/)
  UNARY_MATH_DERIVATIVE(sin, cos)
  UNARY_MATH_DERIVATIVE(cos, -sin)
  UNARY_MATH_DERIVATIVE(tan, D_tan)
  UNARY_MATH_DERIVATIVE(sinh, cosh)
  UNARY_MATH_DERIVATIVE(cosh, sinh)
  UNARY_MATH_DERIVATIVE(tanh, D_tanh)
  UNARY_MATH_DERIVATIVE(sqrt, 0.5/sqrt)
  UNARY_MATH_DERIVATIVE(fabs, sign)
  using ::expm1; using ::log1p;
  UNARY_MATH_DERIVATIVE(expm1, exp)
  template<class T> T D_log1p(const T &x) {return 1. / (x + 1.);}
  UNARY_MATH_DERIVATIVE(log1p, D_log1p)
  /* asin, acos, atan */
  using ::asin; using ::acos; using ::atan;
  template<class T> T D_asin(const T &x) {
    return 1. / sqrt(1. - x * x);
  }
  template<class T> T D_acos(const T &x) {
    return -1. / sqrt(1. - x * x);
  }
  template<class T> T D_atan(const T &x) {
    return 1. / (1. + x * x);
  }
  UNARY_MATH_DERIVATIVE(asin, D_asin)
  UNARY_MATH_DERIVATIVE(acos, D_acos)
  UNARY_MATH_DERIVATIVE(atan, D_atan)
#undef UNARY_MATH_DERIVATIVE
  /* A few more ... */
  template<class T, class V>
  ad<T, V> pow (const ad<T, V> &x, const ad<T, V> &y){
    return exp(y * log(x));
  }
  using ::pow;
  template<class T, class V>
  ad<T, V> pow (const ad<T, V> &x, const double &y){
    return ad<T, V> (pow(x.value, y), // Note: x.value could be 0
		     T( y * pow(x.value, y - 1.) ) * x.deriv);
  }
  /* Comparison operators where a constant is first argument */
#define COMPARISON_OPERATOR_FLIP(OP1, OP2)			\
  template<class T, class V>					\
  bool operator OP1 (const double &x, const ad<T, V> &y) {	\
    return y OP2 x;						\
  }
  COMPARISON_OPERATOR_FLIP(<,>)
  COMPARISON_OPERATOR_FLIP(<=,>=)
  COMPARISON_OPERATOR_FLIP(>,<)
  COMPARISON_OPERATOR_FLIP(>=,<=)
  COMPARISON_OPERATOR_FLIP(==,==)
  COMPARISON_OPERATOR_FLIP(!=,!=)
#undef COMPARISON_OPERATOR_FLIP
  /* Utility: Return the value of a tiny_ad type */
  double asDouble(double x) CSKIP( {return x;} )
  template<class T, class V>
  double asDouble (const ad<T, V> &x){
    return asDouble(x.value);
  }
  /* Utility: Return the max absolute value of all members of a
     tiny_ad type */
  double max_fabs(double x) CSKIP( {return fabs(x);} )
  template<class T, class V>
  double max_fabs (const ad<T, V> &x){
    double ans = max_fabs(x.value);
    for(int i=0; i<x.deriv.size(); i++) {
      double tmp = max_fabs(x.deriv[i]);
      ans = (tmp > ans ? tmp : ans);
    }
    return ans;
  }
  /* R-specific derivatives (rely on Rmath)*/
#ifdef R_RCONFIG_H
  extern "C" {
    /* See 'R-API: entry points to C-code' (Writing R-extensions) */
    double	Rf_lgammafn(double);
    double	Rf_psigamma(double, double);
  }
  template<int deriv>
  double lgamma(const double &x) {
    return Rf_psigamma(x, deriv-1);
  }
  template<>
  double lgamma<0>(const double &x) CSKIP( {return Rf_lgammafn(x);} )
  double lgamma(const double &x) CSKIP( {return lgamma<0>(x);} )
  template<int deriv, class T, class V>
  ad<T, V> lgamma (const ad<T, V> &x){
    return ad<T, V> (lgamma< deriv >(x.value),
		     T(lgamma< deriv + 1 >(x.value)) * x.deriv);
  }
  template<class T, class V>
  ad<T, V> lgamma (const ad<T, V> &x){
    return lgamma<0>(x);
  }
#endif
  /* Print method */
  template<class T, class V>
  std::ostream &operator<<(std::ostream &os, const ad<T, V> &x) {
    os << "{";
    os << " value=" << x.value;
    os << " deriv=" << x.deriv;
    os << "}";
    return os;
  }

  /* Interface to higher order derivatives. Example:

     typedef tiny_ad::variable<3, 2> Float; // Track 3rd order derivs wrt. 2 parameters
     Float a (1.23, 0);                     // Let a = 1.23 have parameter index 0
     Float b (2.34, 1);                     // Let b = 2.34 have parameter index 1
     Float y = sin(a + b);                  // Run the algorithm
     y.getDeriv();                          // Get all 3rd order derivatives
  */
#define VARIABLE(order, nvar, scalartype) variable<order, nvar, scalartype>
  template<int order, int nvar, class Double=double>
  struct variable : ad< VARIABLE(order-1, nvar, Double),
			TINY_VECTOR( VARIABLE(order-1, nvar, Double) , nvar) > {
    typedef ad< VARIABLE(order-1, nvar, Double),
		TINY_VECTOR(VARIABLE(order-1, nvar, Double), nvar) > Base;
    typedef variable<order-1, nvar, Double> Type;
    static const int result_size = nvar * Type::result_size;
#define ___COMMON_CTORS___                      \
    variable() {}                               \
    variable(Base x) : Base(x) {}               \
    variable(double x) : Base(x) {}             \
    variable(double x, int id) : Base(x) {      \
      setid(id);                                \
    }                                           \
    template<int T1, int T2, class T3>          \
    variable(variable<T1,T2,T3> x) {            \
      Base::value = x; Base::deriv.setZero();   \
    }                                           \
    template<class T1, class T2>                \
    variable(ad<T1,T2> x) {                     \
      Base::value = x; Base::deriv.setZero();   \
    }                                           \
    template<int T1, int T2, class T3>          \
    variable(variable<T1,T2,T3> x, int id) {    \
      Base::value = x; Base::deriv.setZero();   \
      setid(id);                                \
    }                                           \
    template<class T1, class T2>                \
    variable(ad<T1,T2> x, int id) {             \
      Base::value = x; Base::deriv.setZero();   \
      setid(id);                                \
    }
    ___COMMON_CTORS___
    void setid(int i0, int count = 0){
      this->value.setid(i0, count);
      this->deriv[i0].setid(i0, count + 1);
    }
    TINY_VECTOR(Double, result_size) getDeriv(){
      TINY_VECTOR(Double, result_size) ans;
      int stride = result_size / nvar;
      for(int i=0; i<nvar; i++)
	ans.segment(i * stride, stride) = this->deriv[i].getDeriv();
      return ans;
    }
  };
#undef VARIABLE
  template<int nvar, class Double>
  struct variable<1, nvar, Double> : ad<Double, TINY_VECTOR(Double,nvar) >{
    typedef ad<Double, TINY_VECTOR(Double,nvar) > Base;
    static const int result_size = nvar;
    ___COMMON_CTORS___
    void setid(int i0, int count = 0){
      if(count == 0)
	this->deriv[i0] = 1.0;
      if(count == 1)
	this->value = 1.0;
    }
    TINY_VECTOR(Double, nvar) getDeriv(){
      return this->deriv;
    }
  };
#undef ___COMMON_CTORS___
#undef TINY_VECTOR
} // End namespace tiny_ad

#endif