// include for ROS
#include "ros/ros.h"
#include <unistd.h>
#include <boost/date_time/local_time/local_time.hpp>

#include <tms_msg_rc/katana_pos.h>
#include <tms_msg_rc/katana_pos_array.h>
#include <tms_msg_rc/katana_pos_single.h>
#include <tms_msg_rc/gripper_action.h>

#include "kniBase.h"
//#include "kmlMotBase.h"
#include "common/MathHelperFunctions.h"
#include <iostream>
#include <cstdio>
#include <memory>
#include <vector>
#include <fstream>
#include <pthread.h>
#include <stdlib.h>
#include <string.h>
#include "common/Timer.h"

//////////////////////////////////////////////////////////////////////////////////
#ifdef WIN32
#include <conio.h>
#else  // LINUX
#include "keyboard.h"
#endif
#define LEFT false
#define RIGHT true
//////////////////////////////////////////////////////////////////////////////////
// Thread structs:
pthread_mutex_t mutex;

struct TPoint
{
  double X, Y, Z;
  double phi, theta, psi;
};

struct TCurrentMot
{
  int idx;
  bool running;
  bool dir;
};

struct Tpos
{
  static std::vector< int > x, y, z, u, v, w;
  static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
};

// Katana obj.
std::auto_ptr< CLMBase > katana;

// std::vector<int> Foo::vec(array, array + sizeof(array)/sizeof(*array));
// positionen, hard-coded. Use values from file instead
const int Tpos::xArr[] = {30206, -23393, -3066, 14454, 30000, 30000};
const int Tpos::yArr[] = {24327, -7837, -16796, 5803, 30290, 31000};
const int Tpos::zArr[] = {24327, -7837, -16796, 5802, 30290, 10924};
const int Tpos::uArr[] = {5333, -13791, -9985, 11449, 30996, 12063};
const int Tpos::vArr[] = {-3799, -5703, -11676, 8210, 30995, 12063};
const int Tpos::wArr[] = {-3799, -5703, -11676, 8210, 30995, 30992};
std::vector< int > Tpos::x(xArr, xArr + sizeof(xArr) / sizeof(*xArr));
std::vector< int > Tpos::y(yArr, yArr + sizeof(yArr) / sizeof(*yArr));
std::vector< int > Tpos::z(zArr, zArr + sizeof(zArr) / sizeof(*zArr));
std::vector< int > Tpos::u(uArr, uArr + sizeof(uArr) / sizeof(*uArr));
std::vector< int > Tpos::v(vArr, vArr + sizeof(vArr) / sizeof(*vArr));
std::vector< int > Tpos::w(wArr, wArr + sizeof(wArr) / sizeof(*wArr));
std::vector< TPoint > points(0);
// void StartPointlistMovement();
// void StartProgram(int index);
pthread_t tid;
// void* RunProgram(void*);
pid_t threadPid;
int retVal = 0;
bool progRunning = false;
// const double PI = 3.14159265358979323846;

const double SafeDegreeMaxMot1 = 360;
const double SafeDegreeMinMot1 = 0;
const double closegripcom = 0;
const int hand_force = -30;

//関数名【 round() 】  丸め処理関数    小数点第1位でdoubleの正負の数を丸める。
int user_round(double val)
{
  double ret0;
  int ret;
  char buf[256] = {'¥0'};
  char *p;

  // ***** 丸めを行い文字列に変換 ***** */
  sprintf(buf, "%.*f", 1, val);
  // ***** 文字列から数値に再変換 ***** */
  ret0 = strtod(buf, &p);

  ret = (double)(int)ret;

  return ret0;
}
////////////////////////////////////////////////////////////////////////////////
int user_angle_rad2enc(CLMBase *robot, int index, double radian)
{
  // Init paramater の取得
  const TMotInit *param = robot->GetBase()->GetMOT()->arr[index].GetInitialParameters();

  double c;
  int d;

  c = (double)param->encoderOffset +
      (double)param->rotationDirection * ((param->angleOffset - radian) / (2 * M_PI)) * (double)param->encodersPerCycle;
  d = user_round(c);

  return c;
}
////////////////////////////////////////////////////////////////////////////////
double user_angle_deg2rad(double degree)
{
  double c;

  c = (degree * M_PI) / 180;
  return c;
}

bool katana_move_angle_array(tms_msg_rc::katana_pos_array::Request &req, tms_msg_rc::katana_pos_array::Response &res)
{
  //角度[6]配列(&req)→Enc値[6]配列(act_enc)→pose値[6]配列act_pose

  std::vector< std::vector< int > > act_enc;  // Encoder値
  std::vector< int > temp_enc;

  std::vector< std::vector< double > > act_pose;
  std::vector< double > temp_pose;

  unsigned int i = 0;  // for用カウンタ
  unsigned int j = 0;  // std::cout用カウンタ

  double actual_pose[6];
  //関節角度から関節Encoder値に変換
  for (i = 0; i < req.pose_array.size(); i++)
  {
    ROS_INFO("%f, %f, %f, %f, %f, %f, %f", req.pose_array[i].pose[0], req.pose_array[i].pose[1],
             req.pose_array[i].pose[2], req.pose_array[i].pose[3], req.pose_array[i].pose[4], req.pose_array[i].pose[5],
             req.pose_array[i].pose[6]);

    temp_enc.clear();

    actual_pose[0] = (double)req.pose_array[i].pose[0] + 180.0;
    actual_pose[1] = 90.0 - (double)req.pose_array[i].pose[1];
    actual_pose[2] = 180.0 - (double)req.pose_array[i].pose[2];
    actual_pose[3] = (double)req.pose_array[i].pose[3] + 180.0;
    actual_pose[4] = 180.0 - (double)req.pose_array[i].pose[4];
    actual_pose[5] = -1 * (double)(req.pose_array[i].pose[5] + req.pose_array[i].pose[6]);

    temp_enc.push_back(user_angle_rad2enc(katana.get(), 0, user_angle_deg2rad(actual_pose[0])));  // motor1
    temp_enc.push_back(user_angle_rad2enc(katana.get(), 1, user_angle_deg2rad(actual_pose[1])));  // motor2
    temp_enc.push_back(user_angle_rad2enc(katana.get(), 2, user_angle_deg2rad(actual_pose[2])));  // motor3
    temp_enc.push_back(user_angle_rad2enc(katana.get(), 3, user_angle_deg2rad(actual_pose[3])));  // motor4
    temp_enc.push_back(user_angle_rad2enc(katana.get(), 4, user_angle_deg2rad(actual_pose[4])));  // motor5
    temp_enc.push_back(user_angle_rad2enc(katana.get(), 5, user_angle_deg2rad(actual_pose[5])));  // motor6(=Gripper)

    for (j = 0; j < 6; j++)
      //			printf("actual:%fdeg \n",actual_pose[j]);
      printf("actual:%f, encoder:%d \n", actual_pose[j], temp_enc.at(j));
    printf("\n");

    //		ROS_INFO("movDegrees_%d", i);
    //		katana->movDegrees(0, actual_pose[0]);
    //		katana->movDegrees(1, actual_pose[1]);
    //		katana->movDegrees(2, actual_pose[2]);
    //		katana->movDegrees(3, actual_pose[3]);
    //		katana->movDegrees(4, actual_pose[4]);
    //		katana->movDegrees(5, actual_pose[5]);
    //		ROS_INFO("movDegrees%d", i);
    //		sleep(6);

    ROS_INFO("moveRobotToEnc_%d", i);
    katana->moveRobotToEnc(temp_enc, true);
    ROS_INFO("moveRobotToEnc%d", i);
    sleep(3);
    //		act_enc.push_back(temp_enc);
  }
  printf("\n");

  //関節Encoder値からロボット姿勢に変換
  //	for(i=0;i<act_enc.size();i++){
  //		ROS_INFO("Debug1");
  //		katana->getCoordinatesFromEncoders(temp_pose, act_enc[i]);	//motor1~6(Encoder) ->
  // RobotPose(x,y,z,phi,theta,psi)

  //		for(j=0;j<6;j++)
  //			printf("%lf ",temp_pose[j]);
  //		printf("\n");

  //		act_pose.push_back(temp_pose);
  //		temp_pose.clear();
  //	}
  //	printf("\n");

  //ロボット動作
  //	for(i=0;i<act_pose.size();i++){

  //		katana->moveRobotTo(act_pose[i],true);		//姿勢のパラメータ→逆運動学→Move

  //	}

  return true;
}

// for test (rosservice call / rqt)
//実際はkatana_move_angle_arrayを投げたほうが楽のはず
bool katana_move_angle(tms_msg_rc::katana_pos_single::Request &req, tms_msg_rc::katana_pos_single::Response &res)
{
  std::vector< int > temp_enc;
  std::vector< double > temp_pose;

  temp_enc.push_back(user_angle_rad2enc(katana.get(), 0, user_angle_deg2rad((double)req.pose[0])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 1, user_angle_deg2rad((double)req.pose[1])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 2, user_angle_deg2rad((double)req.pose[2])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 3, user_angle_deg2rad((double)req.pose[3])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 4, user_angle_deg2rad((double)req.pose[4])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 5, user_angle_deg2rad((double)req.pose[5])));  // motor1

  katana->getCoordinatesFromEncoders(temp_pose, temp_enc);

  katana->moveRobotTo(temp_pose, true);

  return true;
}

bool katana_move_pose_array(tms_msg_rc::katana_pos_array::Request &req, tms_msg_rc::katana_pos_array::Response &res)
{
  std::vector< double > temp_pose;

  for (unsigned int i = 0; i < req.pose_array.size(); i++)
  {
  }

  return true;
}

bool katana_move_enc(tms_msg_rc::katana_pos_single::Request &req, tms_msg_rc::katana_pos_single::Response &res)
{
  std::vector< int > temp_enc;

  temp_enc.push_back(user_angle_rad2enc(katana.get(), 0, user_angle_deg2rad((double)req.pose[0])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 1, user_angle_deg2rad((double)req.pose[1])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 2, user_angle_deg2rad((double)req.pose[2])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 3, user_angle_deg2rad((double)req.pose[3])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 4, user_angle_deg2rad((double)req.pose[4])));  // motor1
  temp_enc.push_back(user_angle_rad2enc(katana.get(), 5, user_angle_deg2rad((double)req.pose[5])));  // motor1

  katana->moveRobotToEnc(temp_enc, true);

  return true;
}

bool katana_move_motor_angle(tms_msg_rc::katana_pos_single::Request &req, tms_msg_rc::katana_pos_single::Response &res)
{
  katana->movDegrees((unsigned int)req.pose[0], req.pose[1]);

  return true;
}

bool katana_gripper_action(tms_msg_rc::gripper_action::Request &req, tms_msg_rc::gripper_action::Response &res)
{
  int hand_degree = -40;

  switch (req.gripper)
  {
    case (1):
      katana->openGripper();
      break;
    case (2):
      katana->closeGripper();
      break;
    case (3):  //力限界まで閉じる。物体を把持するときに利用する。
      while ((katana->getForce(6)) > hand_force)
      {
        hand_degree++;
        katana->movDegrees(5, hand_degree);
        sleep(2);

        std::cout << "getForce(6) = " << katana->getForce(6) << ", ";
        std::cout << "hand degree = " << hand_degree << std::endl;
        katana->freezeMotor(5);
        std::cout << "freeze" << std::endl;
        sleep(10);
        if (hand_degree > 15)
        {
          ROS_INFO("hand_degree is over 15");
          exit(1);
        }
      }
      break;

    default:
      ROS_INFO("error! [1:open gripper / 2:close gripper]");
      break;
  }

  return true;
}

////////////////////////////////////  main  ////////////////////////////////////
int main(int argc, char *argv[])
{
  ros::init(argc, argv, "ros_katana");
  ros::NodeHandle n;
  ROS_INFO("ros_katana : init");

  ros::ServiceServer service1 = n.advertiseService("katana_move_angle_array", katana_move_angle_array);
  ros::ServiceServer service2 = n.advertiseService("katana_move_pose_array", katana_move_pose_array);
  ros::ServiceServer service3 = n.advertiseService("katana_move_enc", katana_move_enc);
  ros::ServiceServer service4 = n.advertiseService("katana_move_motor_angle", katana_move_motor_angle);
  ros::ServiceServer service5 = n.advertiseService("katana_move_angle", katana_move_angle);
  ros::ServiceServer service6 = n.advertiseService("katana_gripper_action", katana_gripper_action);

  sleep(1);

  char dir[255];
  getcwd(dir, 255);
  std::cout << "Current Directory : " << dir << std::endl;

  // open device: a serial port is opened in this case
  std::auto_ptr< CCdlSocket > device;
  std::auto_ptr< CCplSerialCRC > protocol;

  try
  {
    int port = 5566;
    char *ip = "192.168.168.232";
    device.reset(new CCdlSocket(ip, port));

    std::cout << "-------------------------------------------" << std::endl;
    std::cout << "success:  port " << port << " open" << std::endl;
    std::cout << "-------------------------------------------" << std::endl;

    // init protocol:
    protocol.reset(new CCplSerialCRC());
    std::cout << "-------------------------------------------" << std::endl;
    std::cout << "success: protocol class instantiated" << std::endl;
    std::cout << "-------------------------------------------" << std::endl;
    protocol->init(device.get());  // fails if no response from Katana
    std::cout << "-------------------------------------------" << std::endl;
    std::cout << "success: communication with Katana initialized" << std::endl;
    std::cout << "-------------------------------------------" << std::endl;

    // init robot:
    katana.reset(new CLMBase());
    katana->create("./catkin_ws/src/ros_tms/tms_rc/tms_rc_katana/KNI_4.3.0/configfiles450/katana6M180_G.cfg",
                   protocol.get());
  }
  catch (Exception &e)
  {
    std::cout << "ERROR: " << e.message() << std::endl;
    return -1;
  }
  std::cout << "-------------------------------------------" << std::endl;
  std::cout << "success: katana initialized" << std::endl;
  std::cout << "-------------------------------------------" << std::endl;

  // DisplayHelp();

  // KatanaのInitializeここまで

  short counter = 0;
  bool loop = true;  //ループカウンタ trueなら操作続行
  int pos = 0;
  bool DispInRad = true;
  bool IsOff = false;
  bool useLinearMode = false;
  double RadToDeg = 1.0;
  std::vector< int > encoders(katana->getNumberOfMotors(), 0);

  const TKatMOT *motors;
  TCurrentMot mot[6];
  for (int i = 0; i < 6; i++)
  {
    mot[i].running = false;
    mot[i].idx = i;
    mot[i].dir = true;
    mot[i].dir = RIGHT;
  }
  bool stepMode = false;
  int stepSize = 100;
  bool runProgram = false;

  CSctBase *sensctrl = &katana->GetBase()->GetSCT()->arr[0];

  //グリッパー開放用
  bool closegrip = false;

  //設定変更・キャリブレーション
  katana->setMaximumLinearVelocity(30);  // set linear velocity to 30
  std::cout << "setMaximumLinearVelocity 30." << std::endl;
  katana->calibrate();  // caliblate katana
  std::cout << "Katana was calibrated." << std::endl;

  int hand_degree = -40;

  //	while((katana->getForce(6)) > hand_force){
  //		hand_degree++;
  //		katana->movDegrees(5,hand_degree);
  //		sleep(2);
  //
  //		std::cout<<"getForce(6) = "<<katana->getForce(6)<<", ";
  //		std::cout<<"hand degree = "<<hand_degree<<std::endl;
  //		katana->freezeMotor(5);
  //		std::cout<<"freeze"<<std::endl;
  //		Sleep(10000);
  //		if(hand_degree > 15){
  //			printf("hand_degree is over 15¥n");
  //			exit(1);
  //		}
  //	}

  // calibration後の姿勢の記録
  std::vector< int > enc_init;
  std::vector< double > pose_init;

  //直立姿勢の記録
  std::vector< int > enc_fst;
  std::vector< double > pose_fst;

  pose_init.resize(6);

  enc_init.push_back(katana->getMotorEncoders(0));
  enc_init.push_back(katana->getMotorEncoders(1));
  enc_init.push_back(katana->getMotorEncoders(2));
  enc_init.push_back(katana->getMotorEncoders(3));
  enc_init.push_back(katana->getMotorEncoders(4));
  enc_init.push_back(katana->getMotorEncoders(5));

  //	katana->getCoordinatesFromEncoders(pose_init,enc_init);
  katana->getCoordinates(pose_init[0], pose_init[1], pose_init[2], pose_init[3], pose_init[4], pose_init[5]);
  // calibration後の姿勢の記録ここまで

  ROS_INFO("Encoder Value init Motor 0 : %d", enc_init[0]);
  ROS_INFO("Encoder Value init Motor 1 : %d", enc_init[1]);
  ROS_INFO("Encoder Value init Motor 2 : %d", enc_init[2]);
  ROS_INFO("Encoder Value init Motor 3 : %d", enc_init[3]);
  ROS_INFO("Encoder Value init Motor 4 : %d", enc_init[4]);
  ROS_INFO("Encoder Value init Motor 5 : %d", enc_init[5]);

  ROS_INFO("Encoder degree init Motor 0 : %lf", pose_init[0]);
  ROS_INFO("Encoder degree init Motor 1 : %lf", pose_init[1]);
  ROS_INFO("Encoder degree init Motor 2 : %lf", pose_init[2]);
  ROS_INFO("Encoder degree init Motor 3 : %lf", pose_init[3]);
  ROS_INFO("Encoder degree init Motor 4 : %lf", pose_init[4]);
  ROS_INFO("Encoder degree init Motor 5 : %lf", pose_init[5]);

  // Motor1 to 180 degree (front)
  //	katana->movDegrees(0, 180);
  //	sleep(5);

  goto SPIN;

  //	// get actual position
  //	double x1, y1, z1, phi1, theta1, psi1;
  //	katana->getCoordinates(x1, y1, z1, phi1, theta1, psi1);
  //	// create second position
  //	double x2 = x1 + 100.0;
  //	double y2 = y1 - 50.0;
  //	double z2 = z1 + 80.0;
  //	double phi2 = phi1;
  //	double theta2 = theta1;
  //	double psi2 = psi1;
  //	// move to position 2
  //	katana->moveRobotTo(x2, y2, z2, phi2, theta2, psi2);
  //	sleep(7);
  //	// move linear back to position 1
  //	katana->moveRobotTo(x1, y1, z1, phi1, theta1, psi1);
  //	sleep(7);
  //	katana->moveRobotTo(pose_init);

  //	katana->movDegrees(1, 90);
  //	katana->movDegrees(2, 180);
  //	katana->movDegrees(3, 180);
  //	katana->movDegrees(4, 180);
  //	katana->movDegrees(5, -90);
  //	sleep(5);
  //
  //	ROS_INFO("Encoder Value fst Motor 0 : %d", katana->getMotorEncoders(0));
  //	ROS_INFO("Encoder Value fst Motor 1 : %d", katana->getMotorEncoders(1));
  //	ROS_INFO("Encoder Value fst Motor 2 : %d", katana->getMotorEncoders(2));
  //	ROS_INFO("Encoder Value fst Motor 3 : %d", katana->getMotorEncoders(3));
  //	ROS_INFO("Encoder Value fst Motor 4 : %d", katana->getMotorEncoders(4));
  //	ROS_INFO("Encoder Value fst Motor 5 : %d", katana->getMotorEncoders(5));

  //	exit(1);

  //回転命令後5秒停止
  //	sleep(5);

  //	std::cout<<"initEnc = " << user_angle_rad2enc(katana.get(),0, user_angle_deg2rad(270)) <<std::endl;
  //	katana->moveMotorToEnc(0, user_angle_rad2enc(katana.get(),0, user_angle_deg2rad(270)));
  ////モーター0を角度45度へEnc値で動かすテスト

  //初期姿勢(直立状態)へ
  //	katana->movDegrees(1, 90);
  //	katana->movDegrees(2, 180);
  //	katana->movDegrees(3, 180);
  //	katana->movDegrees(4, 180);

  //初期姿勢(直立状態)へ

  //	enc_fst.push_back(6346);	enc_fst.push_back(-10092);	enc_fst.push_back(-26080);
  //	enc_fst.push_back(11587);	enc_fst.push_back(6609);	enc_fst.push_back(18271);

  enc_fst.push_back(user_angle_rad2enc(katana.get(), 0, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 1, user_angle_deg2rad(45)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 2, user_angle_deg2rad(90)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 3, user_angle_deg2rad(200)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 4, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 5, user_angle_deg2rad(-30)));

  katana->moveRobotToEnc(enc_fst, true);
  exit(1);

  katana->getCoordinatesFromEncoders(pose_fst, enc_fst);

  katana->moveRobotTo(pose_fst);
  sleep(5);
  katana->movDegrees(0, 90);
  sleep(5);

  enc_fst.push_back(user_angle_rad2enc(katana.get(), 0, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 1, user_angle_deg2rad(90)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 2, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 3, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 4, user_angle_deg2rad(180)));
  enc_fst.push_back(user_angle_rad2enc(katana.get(), 5, user_angle_deg2rad(-90)));

  ROS_INFO("Encoder Value fst Motor 0 : %d", enc_fst[0]);
  ROS_INFO("Encoder Value fst Motor 1 : %d", enc_fst[1]);
  ROS_INFO("Encoder Value fst Motor 2 : %d", enc_fst[2]);
  ROS_INFO("Encoder Value fst Motor 3 : %d", enc_fst[3]);
  ROS_INFO("Encoder Value fst Motor 4 : %d", enc_fst[4]);
  ROS_INFO("Encoder Value fst Motor 5 : %d", enc_fst[5]);

  katana->moveRobotToEnc(enc_fst, true);
  sleep(10);

  katana->moveRobotTo(pose_init);

  exit(1);

  katana->getCoordinatesFromEncoders(pose_fst, enc_fst);

  ROS_INFO("Encoder pose fst Motor 0 : %lf", pose_fst[0]);
  ROS_INFO("Encoder pose fst Motor 1 : %lf", pose_fst[1]);
  ROS_INFO("Encoder pose fst Motor 2 : %lf", pose_fst[2]);
  ROS_INFO("Encoder pose fst Motor 3 : %lf", pose_fst[3]);
  ROS_INFO("Encoder pose fst Motor 4 : %lf", pose_fst[4]);
  ROS_INFO("Encoder pose fst Motor 5 : %lf", pose_fst[5]);

  katana->moveRobotTo(pose_fst);

  ROS_INFO("setting start position finished. Sleep this system for 1000 ms.");
  sleep(3);  // 3秒停止
  //初期姿勢移行　ここまで

  exit(1);

  /*

  ///////////////////////////////////////////////////////////////////
  //Gripper Action
  ///////////////////////////////////////////////////////////////////

  //	katana->closeGripper();

    int grip_number = 0;	//1回目の計測異常を読み飛ばす

    std::cout<<"first getForce(6) = "<<katana->getForce(6)<<", ";
    std::cout<<"hand degree = "<<hand_degree<<std::endl;

    while(((katana->getForce(6)) > hand_force)||(grip_number < 2)){
      grip_number++;
      hand_degree++;

      katana->movDegrees(5, hand_degree);
      Sleep(1000);

      std::cout<<"getForce(6) = "<<katana->getForce(6)<<", ";
      std::cout<<"hand degree = "<<hand_degree<<std::endl;
  //		katana->freezeMotor(5);
  //		std::cout<<"freeze"<<std::endl;
  //		Sleep(10000);
      if(hand_degree > 15){
        printf("hand_degree is over 15¥n");
        exit(1);
      }
    }

    Sleep(2000);

    katana->moveRobotTo(pose_fst);

  */
  katana->moveRobotTo(pose_init);

SPIN:
  ros::spin();

  return 0;
}


#include <boost/units/base_units/information/bit.hpp>


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <vector>

#include "theia/matching/cascade_hashing_feature_matcher.h"
#include "theia/matching/distance.h"
#include "theia/matching/feature_matcher.h"
#include "theia/matching/image_pair_match.h"

#include "gtest/gtest.h"

namespace theia {

using Eigen::VectorXf;

static const int kNumDescriptors = 10;
static const int kNumDescriptorDimensions = 10;

TEST(CascadeHashingFeatureMatcherTest, NoOptionsInCore) {
  // Set up descriptors.
  std::vector<VectorXf> descriptor1(kNumDescriptors);
  std::vector<VectorXf> descriptor2(kNumDescriptors);
  for (int i = 0; i < kNumDescriptors; i++) {
    // Avoid a zero vector.
    descriptor1[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    descriptor2[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    descriptor1[i].normalize();
    descriptor2[i].normalize();
  }

  // Set options.
  FeatureMatcherOptions options;
  options.match_out_of_core = false;
  options.keypoints_and_descriptors_output_dir = "";
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = false;

  // Add features.
  std::vector<Keypoint> keypoints1(descriptor1.size());
  std::vector<Keypoint> keypoints2(descriptor2.size());
  CascadeHashingFeatureMatcher matcher(options);
  matcher.AddImage("1", keypoints1, descriptor1);
  matcher.AddImage("2", keypoints2, descriptor2);

  // Match features
  std::vector<ImagePairMatch> matches;
  matcher.MatchImages(&matches);

  // Check that the results are valid.
  EXPECT_EQ(matches[0].correspondences.size(), kNumDescriptors);
}

TEST(CascadeHashingFeatureMatcherTest, RatioTestInCore) {
  // Set up descriptors.
  std::vector<VectorXf> descriptor1(1);
  std::vector<VectorXf> descriptor2(2);
  descriptor1[0] = VectorXf::Constant(kNumDescriptorDimensions, 1).normalized();

  // Set the two descriptors to be very close to each other so that they do not
  // pass the ratio test.
  descriptor2[0] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[0](0) = 0.9;
  descriptor2[0].normalize();
  descriptor2[1] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[1](0) = 0.89;
  descriptor2[1].normalize();

  // Set options.
  FeatureMatcherOptions options;
  options.match_out_of_core = false;
  options.keypoints_and_descriptors_output_dir = "";
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = true;

  // Add features.
  std::vector<Keypoint> keypoints1(descriptor1.size());
  std::vector<Keypoint> keypoints2(descriptor2.size());
  CascadeHashingFeatureMatcher matcher(options);
  matcher.AddImage("1", keypoints1, descriptor1);
  matcher.AddImage("2", keypoints2, descriptor2);

  // Match features.
  std::vector<ImagePairMatch> matches;
  matcher.MatchImages(&matches);

  // Check that the results are valid.
  EXPECT_EQ(matches[0].correspondences.size(), 0);
}

TEST(CascadeHashingFeatureMatcherTest, NoOptionsOutOfCore) {
  // Set up descriptors.
  std::vector<VectorXf> descriptor1(kNumDescriptors);
  std::vector<VectorXf> descriptor2(kNumDescriptors);
  for (int i = 0; i < kNumDescriptors; i++) {
    // Avoid a zero vector.
    descriptor1[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    descriptor2[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    descriptor1[i].normalize();
    descriptor2[i].normalize();
  }

  // Set options.
  FeatureMatcherOptions options;
  options.match_out_of_core = true;
  options.keypoints_and_descriptors_output_dir = GTEST_TESTING_OUTPUT_DIRECTORY;
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = false;

  // Add features.
  std::vector<Keypoint> keypoints1(descriptor1.size());
  std::vector<Keypoint> keypoints2(descriptor2.size());
  CascadeHashingFeatureMatcher matcher(options);
  matcher.AddImage("1", keypoints1, descriptor1);
  matcher.AddImage("2", keypoints2, descriptor2);

  // Match features
  std::vector<ImagePairMatch> matches;
  matcher.MatchImages(&matches);

  // Check that the results are valid.
  EXPECT_EQ(matches[0].correspondences.size(), kNumDescriptors);
}

TEST(CascadeHashingFeatureMatcherTest, RatioTestOutOfCore) {
  // Set up descriptors.
  std::vector<VectorXf> descriptor1(1);
  std::vector<VectorXf> descriptor2(2);
  descriptor1[0] = VectorXf::Constant(kNumDescriptorDimensions, 1).normalized();

  // Set the two descriptors to be very close to each other so that they do not
  // pass the ratio test.
  descriptor2[0] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[0](0) = 0.9;
  descriptor2[0].normalize();
  descriptor2[1] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[1](0) = 0.89;
  descriptor2[1].normalize();

  // Set options.
  FeatureMatcherOptions options;
  options.match_out_of_core = true;
  options.keypoints_and_descriptors_output_dir = GTEST_TESTING_OUTPUT_DIRECTORY;
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = true;

  // Add features.
  std::vector<Keypoint> keypoints1(descriptor1.size());
  std::vector<Keypoint> keypoints2(descriptor2.size());
  CascadeHashingFeatureMatcher matcher(options);
  matcher.AddImage("1", keypoints1, descriptor1);
  matcher.AddImage("2", keypoints2, descriptor2);

  // Match features.
  std::vector<ImagePairMatch> matches;
  matcher.MatchImages(&matches);

  // Check that the results are valid.
  EXPECT_EQ(matches[0].correspondences.size(), 0);
}

TEST(CascadeHashingFeatureMatcherTest, NoDescriptorsInCore) {
  // Set up descriptors.
  std::vector<VectorXf> descriptor1;
  std::vector<VectorXf> descriptor2(2);
  // Set the two descriptors to be very close to each other so that they do not
  // pass the ratio test.
  descriptor2[0] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[0].normalize();
  descriptor2[1] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  descriptor2[1].normalize();

  // Set options.
  FeatureMatcherOptions options;
  options.match_out_of_core = true;
  options.keypoints_and_descriptors_output_dir = GTEST_TESTING_OUTPUT_DIRECTORY;
  options.min_num_feature_matches = 30;
  options.keep_only_symmetric_matches = true;
  options.use_lowes_ratio = true;

  // Add features.
  std::vector<Keypoint> keypoints1(descriptor1.size());
  std::vector<Keypoint> keypoints2(descriptor2.size());
  CascadeHashingFeatureMatcher matcher(options);
  matcher.AddImage("1", keypoints1, descriptor1);
  matcher.AddImage("2", keypoints2, descriptor2);

  // Match features.
  std::vector<ImagePairMatch> matches;
  matcher.MatchImages(&matches);

  // Check that the results are valid.
  EXPECT_EQ(matches.size(), 0);
}

}  // namespace theia


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <algorithm>
#include <math.h>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>   // ends_with()

using namespace std;
namespace bf = boost::filesystem;

struct OdomRaw {
    long long int timestamp;
    double x, y, theta;
    double linearVelX, AngularVelZ;
    double deltaDistance, deltaTheta;

    OdomRaw() {
        timestamp = 0;
        x = y = theta = 0.0;
        linearVelX = AngularVelZ = 0.0;
        deltaDistance = deltaTheta = 0.0;
    }
};

struct OdomFrame {
    long long int timestamp;
    double linearVelX, AngularVelZ;
    double deltaDistance, deltaTheta;

    OdomFrame() {
        timestamp = 0;
        linearVelX = AngularVelZ = 0.0;
        deltaDistance = deltaTheta = 0.0;
    }
};

vector<OdomFrame> readOdomeFrame(const string& file) {
    vector<OdomFrame> result;

    ifstream reader;
    reader.open(file.c_str());
    if (!reader) {
        fprintf(stderr, "%s file open error!\n", file.c_str());
        return result;
    }

    // get data
    while (reader.peek() != EOF) {
        OdomFrame ofr;
        reader >> ofr.timestamp >> ofr.deltaTheta >> ofr.AngularVelZ
               >> ofr.deltaDistance >> ofr.linearVelX;
        if (ofr.timestamp != 0)
            result.push_back(ofr);
    }

    reader.close();

    return result;
}

vector<OdomRaw> readOdomeRaw(const string& file) {
    vector<OdomRaw> result;

    ifstream reader;
    reader.open(file.c_str());
    if (!reader) {
        fprintf(stderr, "%s file open error!\n", file.c_str());
        return result;
    }

    // get data
    while (reader.peek() != EOF) {
        OdomRaw oraw;
        reader >> oraw.timestamp >> oraw.x >> oraw.y >> oraw.theta
               >> oraw.linearVelX >> oraw.AngularVelZ
               >> oraw.deltaDistance >> oraw.deltaTheta;
        if (oraw.timestamp != 0)
            result.push_back(oraw);
    }

    reader.close();

    return result;
}

vector<string> readFolderFiles(const string& folder) {
    vector<string> files;

    bf::path folderPath(folder);
    if (!bf::exists(folderPath))
        return files;

    bf::directory_iterator end_iter;
    for (bf::directory_iterator iter(folderPath); iter != end_iter; ++iter) {
        if (bf::is_directory(iter->status()))
            continue;

        if (bf::is_regular_file(iter->status()))
            files.push_back(iter->path().string());
    }

    return files;
}

vector<string> readImageTimestamp(const vector<string>& vTimes) {
    vector<string> times;
    for (const auto& ts : vTimes) {
        size_t s = ts.find_last_of('w');
        size_t e = ts.find_last_of('.');
        times.push_back(ts.substr(s+1, e-s-1));
    }
    return times;
}

static double normalizeAngle(double angle) {
    return angle + 2*M_PI*floor((M_PI - angle)/(2*M_PI));
}

vector<string> readImageFromFile(const string& imageFile)
{
    vector<string> files;

    ifstream ifs(imageFile);
    if (!ifs.is_open())
        return files;

    string line;
    while (getline(ifs, line) && !line.empty())
        files.emplace_back(line);

    ifs.close();
    return files;
}

int main(int argc, char *argv[])
{
    if (argc < 2) {
        cerr << "[Error] Usage: " << argv[0] << " <rk_dataset_folder> [image_filename]" << endl;
        return -1;
    }
    string imageFile;
    if (argc == 3)
        imageFile = argv[2];

    string datasetFolder = string(argv[1]);
    if (!bf::exists(bf::path(datasetFolder))) {
        cerr << "[Error] " << datasetFolder << " folder dosn't exist!" << endl;
        return -1;
    }

    if (!boost::ends_with(datasetFolder, "/"))
        datasetFolder += "/";
    string odomRawFile = datasetFolder + "OdomRaw.txt";
    string imageFolder = datasetFolder + "slamimg/";
    if (!bf::exists(bf::path(imageFolder))) {
        cerr << "[Error] " << imageFolder << " folder dosn't exist!" << endl;
        return -1;
    }
    if (!bf::is_regular_file(bf::path(odomRawFile))) {
        cerr << "[Error] " << odomRawFile << " dosn't exist!" << endl;
        return -1;
    }

    vector<OdomRaw> vOdomRaws = readOdomeRaw(odomRawFile);
    vector<string> vImageNames;
    if (argc == 3)
        vImageNames = readImageFromFile(imageFile);
    else
        vImageNames = readFolderFiles(imageFolder);

    if (vImageNames.size() < 1) {
        cerr << "[Error] No images in this folder: " << datasetFolder << endl;
        return -1;
    }
    vector<string> vImageTimestamps = readImageTimestamp(vImageNames);
    if (vImageTimestamps.size() < 1) {
        cerr << "[Error] Read timestamps error!" << endl;
        return -1;
    }

    // sync data with upper_bound
    vector<long long int> timeOdomRaw, timeOdomFrame;
    for (auto & r : vOdomRaws)
        timeOdomRaw.push_back(r.timestamp);
    for (auto & f : vImageTimestamps)
        timeOdomFrame.push_back(stoll(f));
    sort(timeOdomFrame.begin(), timeOdomFrame.end());
    cout << "time stamp size: " << timeOdomFrame.size() << ", " << timeOdomRaw.size() << endl;

    string outOdomRaw = datasetFolder + "odo_raw.txt";
    string outOdomSyned = datasetFolder + "odomSyned.txt";
    ofstream ofs(outOdomRaw, ios_base::out);
    ofstream ofs2(outOdomSyned, ios_base::out);
    if (!ofs.is_open()) {
        cerr << "[Error] Wrong output file path." << endl;
        return -1;
    }
    int skipFrames = 0;
    ofs2 << "# ImageTimeStamp OdomTimeStampLower OdomTimeStampUpper x y theta";
    for (auto & t : timeOdomFrame) {
        size_t r = static_cast<size_t>(
                    upper_bound(timeOdomRaw.begin(), timeOdomRaw.end(), t) - timeOdomRaw.begin());
        if (r > timeOdomRaw.size() - 1) {
            cout << "[Warning] 跳过此帧，因为找不到它的对应帧. " << t << endl;
            skipFrames++;
            continue;
        }
        if (r == 0) {
            ofs << 1000*vOdomRaws[r].x << " " << 1000*vOdomRaws[r].y << " " << normalizeAngle(vOdomRaws[r].theta) << "\n";
            ofs2 << t << " " << vOdomRaws[r].timestamp << " " << vOdomRaws[r].timestamp << " "
                 << vOdomRaws[r].x << " " << vOdomRaws[r].y << " " << normalizeAngle(vOdomRaws[r].theta) << "\n";
            continue;
        }
        //! TODO 插值方式需要改进，目前是直接线性插值，可能要改成Eular法
        auto l = r - 1;
        double alpha = (t - vOdomRaws[l].timestamp)/(vOdomRaws[r].timestamp - vOdomRaws[l].timestamp);
        double x = vOdomRaws[l].x + alpha * (vOdomRaws[r].x - vOdomRaws[l].x);
        double y = vOdomRaws[l].y + alpha * (vOdomRaws[r].y - vOdomRaws[l].y);
        double theta = vOdomRaws[l].theta + alpha * (vOdomRaws[r].theta - vOdomRaws[l].theta);
        ofs << 1000*x << " " << 1000*y << " " << normalizeAngle(theta) << "\n";

        ofs2 << t << " " << vOdomRaws[r].timestamp << " " << vOdomRaws[l].timestamp
             << " " << x << " " << y << " " << normalizeAngle(theta) << "\n";
    }
    if (skipFrames > 0)
        printf("因为找不到对应帧共计跳过%d帧.\n", skipFrames);
    cout << "done." << endl;
    return 0;
}


#define BOOST_TEST_MODULE ParserTest

#include <boost/test/unit_test.hpp>
#include <sdm/common.hpp>
#include <sdm/parser/parser.hpp>
#include <sdm/worlds.hpp>

BOOST_AUTO_TEST_CASE(dpomdpParser)
{
    auto dpomdp_world = sdm::parser::parse_file("../data/world/dpomdp/mabc.dpomdp");
    auto dpomdp_2 = dpomdp_world;

    BOOST_CHECK_EQUAL(dpomdp_world->getNumAgents(), 2);
    BOOST_CHECK_EQUAL(dpomdp_world->getStateSpace()->getNumItems(), 4);
    BOOST_CHECK_EQUAL(dpomdp_world->getActionSpace()->getNumItems(), 4);
    BOOST_CHECK_EQUAL(dpomdp_world->getObsSpace()->getNumItems(), 4);
}

BOOST_AUTO_TEST_CASE(zsposgParser)
{
    // sdm::ZSPOSG zs_posg1 = sdm::parser::parse_file("../data/world/zsposg/fake_prisoners.zsposg");

    // BOOST_CHECK_EQUAL(zs_posg1.getNumAgents(), 2);
    // BOOST_CHECK_EQUAL(zs_posg1.getNumStates(), 1);
    // BOOST_CHECK_EQUAL(zs_posg1.getNumJActions(), 4);
    // BOOST_CHECK_EQUAL(zs_posg1.getNumJObservations(), 1);
}


/*
 *
 * Copyright (c) Kresimir Fresl 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

/* for UMFPACK Copyright, License and Availability see umfpack_inc.hpp */

#ifndef BOOST_NUMERIC_BINDINGS_UMFPACK_HPP
#define BOOST_NUMERIC_BINDINGS_UMFPACK_HPP

#include <boost/noncopyable.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/sparse_traits.hpp>
#include <boost/numeric/bindings/umfpack/umfpack_overloads.hpp>

namespace boost { namespace numeric { namespace bindings {  namespace umfpack {

  template <typename T = double>
  struct symbolic_type : private noncopyable {
    void *ptr;
    ~symbolic_type() {
      if (ptr)
        detail::free_symbolic (T(), 0, &ptr);
    }
    void free() {
      if (ptr)
        detail::free_symbolic (T(), 0, &ptr);
      ptr = 0;
    }
  };

  template <typename T>
  void free (symbolic_type<T>& s) { s.free(); }

  template <typename T = double>
  struct numeric_type : private noncopyable {
    void *ptr;
    ~numeric_type() {
      if (ptr)
        detail::free_numeric (T(), 0, &ptr);
    }
    void free() {
      if (ptr)
        detail::free_numeric (T(), 0, &ptr);
      ptr = 0;
    }
  };

  template <typename T>
  void free (numeric_type<T>& n) { n.free(); }

  template <typename T = double>
  struct control_type : private noncopyable {
    double ptr[UMFPACK_CONTROL];
    control_type() { detail::defaults (T(), 0, ptr); }
    double operator[] (int i) const { return ptr[i]; }
    double& operator[] (int i) { return ptr[i]; }
    void defaults() { detail::defaults (T(), 0, ptr); }
  };

  template <typename T>
  void defaults (control_type<T>& c) { c.defaults(); }

  template <typename T = double>
  struct info_type : private noncopyable {
    double ptr[UMFPACK_INFO];
    double operator[] (int i) const { return ptr[i]; }
    double& operator[] (int i) { return ptr[i]; }
  };

  /////////////////////////////////////
  // solving system of linear equations
  /////////////////////////////////////

  // symbolic
  /*
   * Given nonzero pattern of a sparse matrix A in column-oriented form,
   * umfpack_*_symbolic performs a column pre-ordering to reduce fill-in
   * (using COLAMD or AMD) and a symbolic factorisation.  This is required
   * before the matrix can be numerically factorised with umfpack_*_numeric.
   */
  namespace detail {

    template <typename MatrA>
    inline
    int symbolic (traits::compressed_t,
                  MatrA const& A, void **Symbolic,
                  double const* Control = 0, double* Info = 0)
    {
      return detail::symbolic (traits::spmatrix_size1 (A),
                               traits::spmatrix_size2 (A),
                               traits::spmatrix_index1_storage (A),
                               traits::spmatrix_index2_storage (A),
                               traits::spmatrix_value_storage (A),
                               Symbolic, Control, Info);
    }

    template <typename MatrA, typename QVec>
    inline
    int symbolic (traits::compressed_t,
                  MatrA const& A, QVec const& Qinit, void **Symbolic,
                  double const* Control = 0, double* Info = 0)
    {
      return detail::qsymbolic (traits::spmatrix_size1 (A),
                                traits::spmatrix_size2 (A),
                                traits::spmatrix_index1_storage (A),
                                traits::spmatrix_index2_storage (A),
                                traits::spmatrix_value_storage (A),
                                traits::vector_storage (Qinit),
                                Symbolic, Control, Info);
    }

    template <typename MatrA>
    inline
    int symbolic (traits::coordinate_t,
                  MatrA const& A, void **Symbolic,
                  double const* Control = 0, double* Info = 0)
    {
      int n_row = traits::spmatrix_size1 (A);
      int n_col = traits::spmatrix_size2 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n_col+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n_row, n_col, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      return detail::symbolic (n_row, n_col,
                               Ap.storage(), Ai.storage(),
                               traits::spmatrix_value_storage (A),
                               Symbolic, Control, Info);
    }

    template <typename MatrA, typename QVec>
    inline
    int symbolic (traits::coordinate_t,
                  MatrA const& A, QVec const& Qinit, void **Symbolic,
                  double const* Control = 0, double* Info = 0)
    {
      int n_row = traits::spmatrix_size1 (A);
      int n_col = traits::spmatrix_size2 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n_col+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n_row, n_col, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      return detail::qsymbolic (n_row, n_col,
                                Ap.storage(), Ai.storage(),
                                traits::spmatrix_value_storage (A),
                                traits::vector_storage (Qinit),
                                Symbolic, Control, Info);
    }

  } // detail

  template <typename MatrA>
  inline
  int symbolic (MatrA const& A,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    return detail::symbolic (storage_f(), A, &Symbolic.ptr, Control, Info);
  }

  template <typename MatrA>
  inline
  int symbolic (MatrA const& A,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                control_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                > const& Control,
                info_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Info)
  {
    return symbolic (A, Symbolic, Control.ptr, Info.ptr);
  }

  template <typename MatrA>
  inline
  int symbolic (MatrA const& A,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                control_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                > const& Control)
  {
    return symbolic (A, Symbolic, Control.ptr);
  }

  template <typename MatrA, typename QVec>
  inline
  int symbolic (MatrA const& A, QVec const& Qinit,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    assert (traits::spmatrix_size2 (A) == traits::vector_size (Qinit));

    return detail::symbolic (storage_f(), A, Qinit,
                             &Symbolic.ptr, Control, Info);
  }

  template <typename MatrA, typename QVec>
  inline
  int symbolic (MatrA const& A, QVec const& Qinit,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                control_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                > const& Control,
                info_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Info)
  {
    return symbolic (A, Qinit, Symbolic, Control.ptr, Info.ptr);
  }

  template <typename MatrA, typename QVec>
  inline
  int symbolic (MatrA const& A, QVec const& Qinit,
                symbolic_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                >& Symbolic,
                control_type<
                  typename traits::sparse_matrix_traits<MatrA>::value_type
                > const& Control)
  {
    return symbolic (A, Qinit, Symbolic, Control.ptr);
  }

  // numeric
  /*
   * Given a sparse matrix A in column-oriented form, and a symbolic analysis
   * computed by umfpack_*_*symbolic, the umfpack_*_numeric routine performs
   * the numerical factorisation, PAQ=LU, PRAQ=LU, or P(R\A)Q=LU, where P
   * and Q are permutation matrices (represented as permutation vectors),
   * R is the row scaling, L is unit-lower triangular, and U is upper
   * triangular.  This is required before the system Ax=b (or other related
   * linear systems) can be solved.
   */
  namespace detail {

    template <typename MatrA>
    inline
    int numeric (traits::compressed_t, MatrA const& A,
                 void *Symbolic, void** Numeric,
                 double const* Control = 0, double* Info = 0)
    {
      return detail::numeric (traits::spmatrix_size1 (A),
                              traits::spmatrix_size2 (A),
                              traits::spmatrix_index1_storage (A),
                              traits::spmatrix_index2_storage (A),
                              traits::spmatrix_value_storage (A),
                              Symbolic, Numeric, Control, Info);
    }

    template <typename MatrA>
    inline
    int numeric (traits::coordinate_t, MatrA const& A,
                 void *Symbolic, void** Numeric,
                 double const* Control = 0, double* Info = 0)
    {
      int n_row = traits::spmatrix_size1 (A);
      int n_col = traits::spmatrix_size2 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n_col+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n_row, n_col, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      return detail::numeric (n_row, n_col,
                              Ap.storage(), Ai.storage(),
                              traits::spmatrix_value_storage (A),
                              Symbolic, Numeric, Control, Info);
    }

  } // detail

  template <typename MatrA>
  inline
  int numeric (MatrA const& A,
               symbolic_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               > const& Symbolic,
               numeric_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               >& Numeric,
               double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    return detail::numeric (storage_f(), A,
                            Symbolic.ptr, &Numeric.ptr, Control, Info);
  }

  template <typename MatrA>
  inline
  int numeric (MatrA const& A,
               symbolic_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               > const& Symbolic,
               numeric_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               >& Numeric,
               control_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               > const& Control,
               info_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               >& Info)

  {
    // g++ (3.2) is unable to distinguish
    //           function numeric() and namespace boost::numeric ;o)
    return umfpack::numeric (A, Symbolic, Numeric, Control.ptr, Info.ptr);
  }

  template <typename MatrA>
  inline
  int numeric (MatrA const& A,
               symbolic_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               > const& Symbolic,
               numeric_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               >& Numeric,
               control_type<
                 typename traits::sparse_matrix_traits<MatrA>::value_type
               > const& Control)
  {
    return umfpack::numeric (A, Symbolic, Numeric, Control.ptr);
  }

  // factor
  /*
   * symbolic and numeric
   */
  namespace detail {

    template <typename MatrA>
    inline
    int factor (traits::compressed_t, MatrA const& A,
                void** Numeric, double const* Control = 0, double* Info = 0)
    {
      symbolic_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Symbolic;

      int status;
      status = detail::symbolic (traits::spmatrix_size1 (A),
                                 traits::spmatrix_size2 (A),
                                 traits::spmatrix_index1_storage (A),
                                 traits::spmatrix_index2_storage (A),
                                 traits::spmatrix_value_storage (A),
                                 &Symbolic.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      return detail::numeric (traits::spmatrix_size1 (A),
                              traits::spmatrix_size2 (A),
                              traits::spmatrix_index1_storage (A),
                              traits::spmatrix_index2_storage (A),
                              traits::spmatrix_value_storage (A),
                              Symbolic.ptr, Numeric, Control, Info);
    }

    template <typename MatrA>
    inline
    int factor (traits::coordinate_t, MatrA const& A,
                void** Numeric, double const* Control = 0, double* Info = 0)
    {
      int n_row = traits::spmatrix_size1 (A);
      int n_col = traits::spmatrix_size2 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n_col+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n_row, n_col, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      symbolic_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Symbolic;

      status = detail::symbolic (n_row, n_col,
                                 Ap.storage(), Ai.storage(),
                                 traits::spmatrix_value_storage (A),
                                 &Symbolic.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      return detail::numeric (n_row, n_col,
                              Ap.storage(), Ai.storage(),
                              traits::spmatrix_value_storage (A),
                              Symbolic.ptr, Numeric, Control, Info);
    }

  } // detail

  template <typename MatrA>
  inline
  int factor (MatrA const& A,
              numeric_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              >& Numeric,
              double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    return detail::factor (storage_f(), A, &Numeric.ptr, Control, Info);
  }

  template <typename MatrA>
  inline
  int factor (MatrA const& A,
              numeric_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              >& Numeric,
              control_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              > const& Control,
              info_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              >& Info)
  {
    return factor (A, Numeric, Control.ptr, Info.ptr);
  }

  template <typename MatrA>
  inline
  int factor (MatrA const& A,
              numeric_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              >& Numeric,
              control_type<
                typename traits::sparse_matrix_traits<MatrA>::value_type
              > const& Control)
  {
    return factor (A, Numeric, Control.ptr);
  }

  // solve
  /*
   * Given LU factors computed by umfpack_*_numeric and the right-hand-side,
   * B, solve a linear system for the solution X.  Iterative refinement is
   * optionally performed.  Only square systems are handled.
   */
  namespace detail {

    template <typename MatrA, typename VecX, typename VecB>
    inline
    int solve (traits::compressed_t, int sys,
               MatrA const& A, VecX& X, VecB const& B,
               void *Numeric, double const* Control = 0, double* Info = 0)
    {
      return detail::solve (sys, traits::spmatrix_size1 (A),
                            traits::spmatrix_index1_storage (A),
                            traits::spmatrix_index2_storage (A),
                            traits::spmatrix_value_storage (A),
                            traits::vector_storage (X),
                            traits::vector_storage (B),
                            Numeric, Control, Info);
    }

    template <typename MatrA, typename VecX, typename VecB>
    inline
    int solve (traits::coordinate_t, int sys,
               MatrA const& A, VecX& X, VecB const& B,
               void *Numeric, double const* Control = 0, double* Info = 0)
    {

      int n = traits::spmatrix_size1 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n, n, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      return detail::solve (sys, n, Ap.storage(), Ai.storage(),
                            traits::spmatrix_value_storage (A),
                            traits::vector_storage (X),
                            traits::vector_storage (B),
                            Numeric, Control, Info);
    }

  } // detail

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (int sys, MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    assert (traits::spmatrix_size1 (A) == traits::spmatrix_size1 (A));
    assert (traits::spmatrix_size2 (A) == traits::vector_size (X));
    assert (traits::spmatrix_size2 (A) == traits::vector_size (B));

    return detail::solve (storage_f(), sys, A, X, B,
                          Numeric.ptr, Control, Info);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (int sys, MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             control_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Control,
             info_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             >& Info)
  {
    return solve (sys, A, X, B, Numeric, Control.ptr, Info.ptr);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (int sys, MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             control_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Control)
  {
    return solve (sys, A, X, B, Numeric, Control.ptr);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             double const* Control = 0, double* Info = 0)
  {
    return solve (UMFPACK_A, A, X, B, Numeric, Control, Info);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             control_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Control,
             info_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             >& Info)
  {
    return solve (UMFPACK_A, A, X, B, Numeric,
                  Control.ptr, Info.ptr);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int solve (MatrA const& A, VecX& X, VecB const& B,
             numeric_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Numeric,
             control_type<
               typename traits::sparse_matrix_traits<MatrA>::value_type
             > const& Control)
  {
    return solve (UMFPACK_A, A, X, B, Numeric, Control.ptr);
  }

  // umf_solve
  /*
   * symbolic, numeric and solve
   */
  namespace detail {

    template <typename MatrA, typename VecX, typename VecB>
    inline
    int umf_solve (traits::compressed_t,
                   MatrA const& A, VecX& X, VecB const& B,
                   double const* Control = 0, double* Info = 0)
    {
      symbolic_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Symbolic;
      numeric_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Numeric;

      int status;
      status = detail::symbolic (traits::spmatrix_size1 (A),
                                 traits::spmatrix_size2 (A),
                                 traits::spmatrix_index1_storage (A),
                                 traits::spmatrix_index2_storage (A),
                                 traits::spmatrix_value_storage (A),
                                 &Symbolic.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      status = detail::numeric (traits::spmatrix_size1 (A),
                                traits::spmatrix_size2 (A),
                                traits::spmatrix_index1_storage (A),
                                traits::spmatrix_index2_storage (A),
                                traits::spmatrix_value_storage (A),
                                Symbolic.ptr, &Numeric.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      return detail::solve (UMFPACK_A, traits::spmatrix_size1 (A),
                            traits::spmatrix_index1_storage (A),
                            traits::spmatrix_index2_storage (A),
                            traits::spmatrix_value_storage (A),
                            traits::vector_storage (X),
                            traits::vector_storage (B),
                            Numeric.ptr, Control, Info);
    }

    template <typename MatrA, typename VecX, typename VecB>
    inline
    int umf_solve (traits::coordinate_t,
                   MatrA const& A, VecX& X, VecB const& B,
                   double const* Control = 0, double* Info = 0)
    {
      int n_row = traits::spmatrix_size1 (A);
      int n_col = traits::spmatrix_size2 (A);
      int nnz = traits::spmatrix_num_nonzeros (A);

      typedef typename traits::sparse_matrix_traits<MatrA>::value_type val_t;

      int const* Ti = traits::spmatrix_index2_storage (A);
      int const* Tj = traits::spmatrix_index1_storage (A);
      traits::detail::array<int> Ap (n_col+1);
      if (!Ap.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<int> Ai (nnz);
      if (!Ai.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = detail::triplet_to_col (n_row, n_col, nnz,
                                           Ti, Tj, static_cast<val_t*> (0),
                                           Ap.storage(), Ai.storage(),
                                           static_cast<val_t*> (0), 0);
      if (status != UMFPACK_OK) return status;

      symbolic_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Symbolic;
      numeric_type<typename traits::sparse_matrix_traits<MatrA>::value_type>
        Numeric;

      status = detail::symbolic (n_row, n_col,
                                 Ap.storage(), Ai.storage(),
                                 traits::spmatrix_value_storage (A),
                                 &Symbolic.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      status = detail::numeric (n_row, n_col,
                                Ap.storage(), Ai.storage(),
                                traits::spmatrix_value_storage (A),
                                Symbolic.ptr, &Numeric.ptr, Control, Info);
      if (status != UMFPACK_OK) return status;

      return detail::solve (UMFPACK_A, n_row, Ap.storage(), Ai.storage(),
                            traits::spmatrix_value_storage (A),
                            traits::vector_storage (X),
                            traits::vector_storage (B),
                            Numeric.ptr, Control, Info);
    }

  } // detail

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int umf_solve (MatrA const& A, VecX& X, VecB const& B,
                 double const* Control = 0, double* Info = 0)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    assert (traits::spmatrix_size1 (A) == traits::spmatrix_size1 (A));
    assert (traits::spmatrix_size2 (A) == traits::vector_size (X));
    assert (traits::spmatrix_size2 (A) == traits::vector_size (B));

    return detail::umf_solve (storage_f(), A, X, B, Control, Info);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int umf_solve (MatrA const& A, VecX& X, VecB const& B,
                 control_type<
                   typename traits::sparse_matrix_traits<MatrA>::value_type
                 > const& Control,
                 info_type<
                   typename traits::sparse_matrix_traits<MatrA>::value_type
                 >& Info)
  {
    return umf_solve (A, X, B, Control.ptr, Info.ptr);
  }

  template <typename MatrA, typename VecX, typename VecB>
  inline
  int umf_solve (MatrA const& A, VecX& X, VecB const& B,
                 control_type<
                   typename traits::sparse_matrix_traits<MatrA>::value_type
                 > const& Control)
  {
    return umf_solve (A, X, B, Control.ptr);
  }

  ///////////////////////
  // matrix manipulations
  ///////////////////////

  // scale

  template <typename VecX, typename VecB>
  inline
  int scale (VecX& X, VecB const& B,
             numeric_type<
               typename traits::vector_traits<VecB>::value_type
             > const& Numeric)
  {
    return detail::scale (traits::vector_size (B),
                          traits::vector_storage (X),
                          traits::vector_storage (B),
                          Numeric.ptr);
  }

  ////////////
  // reporting
  ////////////

  // report status

  template <typename T>
  inline
  void report_status (control_type<T> const& Control, int status) {
    detail::report_status (T(), 0, Control.ptr, status);
  }

#if 0
  template <typename T>
  inline
  void report_status (int printing_level, int status) {
    control_type<T> Control;
    Control[UMFPACK_PRL] = printing_level;
    detail::report_status (T(), 0, Control.ptr, status);
  }
  template <typename T>
  inline
  void report_status (int status) {
    control_type<T> Control;
    detail::report_status (T(), 0, Control.ptr, status);
  }
#endif

  // report control

  template <typename T>
  inline
  void report_control (control_type<T> const& Control) {
    detail::report_control (T(), 0, Control.ptr);
  }

  // report info

  template <typename T>
  inline
  void report_info (control_type<T> const& Control, info_type<T> const& Info) {
    detail::report_info (T(), 0, Control.ptr, Info.ptr);
  }

#if 0
  template <typename T>
  inline
  void report_info (int printing_level, info_type<T> const& Info) {
    control_type<T> Control;
    Control[UMFPACK_PRL] = printing_level;
    detail::report_info (T(), 0, Control.ptr, Info.ptr);
  }
  template <typename T>
  inline
  void report_info (info_type<T> const& Info) {
    control_type<T> Control;
    detail::report_info (T(), 0, Control.ptr, Info.ptr);
  }
#endif

  // report matrix (compressed column and coordinate)

  namespace detail {

    template <typename MatrA>
    inline
    int report_matrix (traits::compressed_t, MatrA const& A,
                       double const* Control)
    {
      return detail::report_matrix (traits::spmatrix_size1 (A),
                                    traits::spmatrix_size2 (A),
                                    traits::spmatrix_index1_storage (A),
                                    traits::spmatrix_index2_storage (A),
                                    traits::spmatrix_value_storage (A),
                                    1, Control);
    }

    template <typename MatrA>
    inline
    int report_matrix (traits::coordinate_t, MatrA const& A,
                       double const* Control)
    {
      return detail::report_triplet (traits::spmatrix_size1 (A),
                                     traits::spmatrix_size2 (A),
                                     traits::spmatrix_num_nonzeros (A),
                                     traits::spmatrix_index1_storage (A),
                                     traits::spmatrix_index2_storage (A),
                                     traits::spmatrix_value_storage (A),
                                     Control);
    }

  } // detail

  template <typename MatrA>
  inline
  int report_matrix (MatrA const& A,
                     control_type<
                       typename traits::sparse_matrix_traits<MatrA>::value_type
                     > const& Control)
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::matrix_structure,
      traits::general_t
    >::value));
    BOOST_STATIC_ASSERT((boost::is_same<
      typename traits::sparse_matrix_traits<MatrA>::ordering_type,
      traits::column_major_t
    >::value));
    BOOST_STATIC_ASSERT(traits::sparse_matrix_traits<MatrA>::index_base == 0);
#endif

    typedef
      typename traits::sparse_matrix_traits<MatrA>::storage_format storage_f;

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<storage_f, traits::compressed_t>::value
       ||
       boost::is_same<storage_f, traits::coordinate_t>::value
       ));
#endif

    return detail::report_matrix (storage_f(), A, Control.ptr);
  }

  // report vector

  template <typename VecX>
  inline
  int report_vector (VecX const& X,
                     control_type<
                       typename traits::vector_traits<VecX>::value_type
                     > const& Control)
  {
    return detail::report_vector (traits::vector_size (X),
                                  traits::vector_storage (X),
                                  Control.ptr);
  }

  // report numeric

  template <typename T>
  inline
  int report_numeric (numeric_type<T> const& Numeric,
                      control_type<T> const& Control)
  {
    return detail::report_numeric (T(), 0, Numeric.ptr, Control.ptr);
  }

  // report symbolic

  template <typename T>
  inline
  int report_symbolic (symbolic_type<T> const& Symbolic,
                       control_type<T> const& Control)
  {
    return detail::report_symbolic (T(), 0, Symbolic.ptr, Control.ptr);
  }

  // report permutation vector

  template <typename VecP, typename T>
  inline
  int report_permutation (VecP const& Perm, control_type<T> const& Control) {
    return detail::report_perm (T(), 0,
                                traits::vector_storage (Perm),
                                Control.ptr);
  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_UMFPACK_HPP


#include <boost/optional.hpp>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <cmath>

using namespace boost;

optional<int> get_even_random_number()
{
  int i = std::rand();
  return make_optional(i % 2 == 0, i);
}

int main()
{
  std::srand(static_cast<unsigned int>(std::time(0)));
  optional<int> i = get_even_random_number();
  double d = get_optional_value_or(i, 0);
  std::cout << std::sqrt(d) << '\n';
}


#include <cmath>
#include <string>
#include <vector>
#include <sstream>

#include "GameState.h"
#include "Log.h"
#include "Config.h"
#include "Map.h"

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

GameState::GameState(int turn, std::vector<PlanetPtr>& planets)
    : planets_(planets), turn_(turn) { }

GameState::GameState(const GameState& state)
    : orders_(state.orders_), turn_(state.turn_) {
    CopyPlanets(state.planets_);
}

int GameState::Turn() const {
    return turn_;
}

PlanetPtr& GameState::Planet(int planet_id) {
    return planets_[planet_id];
}
const PlanetPtr& GameState::Planet(int planet_id) const {
    return planets_[planet_id];
}

std::vector<PlanetPtr> GameState::Planets() const {
    return planets_;
}

// WTF: this is const but lets us return non-const PlanetPtr
//      I tried to overload but couldn't
//      Either me fail or C++ HUGE fail.
std::vector<PlanetPtr> GameState::PlanetsOwnedBy(Player player) const {
    std::vector<PlanetPtr> r;
    foreach ( const PlanetPtr& p, planets_ )  {
        if (p->Owner() == player) {
            r.push_back(p);
        }
    }
    return r;
}

std::vector<PlanetPtr> GameState::PlanetsNotOwnedBy(Player player) const {
    std::vector<PlanetPtr> r;
    foreach ( const PlanetPtr& p, planets_ )  {
        if (p->Owner() != player) {
            r.push_back(p);
        }
    }
    return r;
}

int GameState::Production(Player player) const {
    int production = 0;
    foreach ( const PlanetPtr& p, planets_ )  {
        if ( p->Owner() == player ) {
            production += p->GrowthRate();
        }
    }
    return production;
}

int GameState::FutureProduction(Player player) const {
    int production = 0;
    foreach ( const PlanetPtr& p, planets_ )  {
        if ( p->FutureOwner() == player ) {
            production += p->GrowthRate();
        }
    }
    return production;
}

int GameState::Ships(Player player) const {
    int ships = 0;
    foreach ( const PlanetPtr& p, planets_ )  {
        ships += p->TotalShips(player);
    }
    return ships;
}

std::vector<Fleet> GameState::Orders() const {
    return orders_;
}

void GameState::IssueOrder(const Fleet& order) {
    AddFleet(order);

    if ( order.owner != ME ) return;

    // only store order if it is for now
    if ( order.launch == 0 ) {
        orders_.push_back(order);
    }
    else {
        LOG( "  DELAYED ORDER: " << order.source << " " << order.dest << " " << order.ships << " | " << order.launch );
    }
}

void GameState::AddFleet(const Fleet& order) {
    if ( order.launch >= 0 ) {
        planets_[order.source]->RemoveShips( order.ships );
    }

    planets_[order.dest]->AddIncomingFleet( order );
}

GameState& GameState::operator=(const GameState& state) {
    if ( this != &state ) {
        // Copy the orders and the turns
        orders_ = state.orders_;
        turn_ = state.turn_;

        // we want to CLONE the planets because they are pointers
        CopyPlanets(state.planets_);
    }

    return *this;
}

void GameState::CopyPlanets(const std::vector<PlanetPtr>& planets) {
    planets_.clear();
    foreach ( const PlanetPtr& p, planets ) {
        planets_.push_back(p->Clone());
    }
}

PlanetPtr GameState::ClosestPlanetByOwner(PlanetPtr planet, Player player) const {
    const std::vector<int>& sorted = Map::PlanetsByDistance( planet->id );

    foreach ( int p, sorted ) {
        if ( planets_[p]->Owner() == player ) {
            return planets_[p];
        }
    }

    return PlanetPtr();
}

int GameState::ShipsWithinRange(PlanetPtr planet, int distance, Player owner) const {
    const std::vector<int>& sorted = Map::PlanetsByDistance(planet->id);

    int ships = 0;

    foreach ( int i, sorted ) {
        int helper_distance = Map::Distance(planet->id, i);
        if ( helper_distance >= distance ) break;

        if ( planets_[i]->Owner() != owner ) continue;
        ships += planets_[i]->Ships();
    }

    return ships;
}

// Find planets closest to the opponent
std::map<int,bool> GameState::FrontierPlanets(Player player) const {
    // TODO: use bitmap
    std::map<int,bool> frontier_planets;
    std::vector<PlanetPtr> opponent_planets = PlanetsOwnedBy(-player);

    foreach ( const PlanetPtr& p, opponent_planets ) {
        PlanetPtr closest = ClosestPlanetByOwner(p,player);
        if ( closest ) {
            frontier_planets[closest->id] = true;
        }
    }
    return frontier_planets;
}

// Find planets that will be closest to the opponent
std::map<int,bool> GameState::FutureFrontierPlanets(Player player) const {
    std::map<int,bool> frontier_planets;

    std::vector<PlanetPtr> opponent_planets = PlanetsOwnedBy(-player);
    foreach ( const PlanetPtr& opponent_planet, opponent_planets ) {
        const std::vector<int>& sorted = Map::PlanetsByDistance( opponent_planet->id );
        PlanetPtr closest;
        foreach ( int p, sorted ) {
            if ( planets_[p]->FutureOwner() == player ) {
                closest = planets_[p];
                break;
            }
        }
        if ( closest ) {
            frontier_planets[closest->id] = true;
        }
    }

    return frontier_planets;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 boost_math toolbox - expint - unit/simd Mode"

#include <nt2/toolbox/boost_math/include/expint.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/simd/native.hpp>
#include <../unit/sdk/simd/types.hpp>
#include <nt2/sdk/memory/is_aligned.hpp>
#include <nt2/sdk/memory/aligned_type.hpp>
#include <nt2/sdk/memory/load.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/type_traits/is_same.hpp>

//////////////////////////////////////////////////////////////////////////////
// Test behavior of boost_math component expint using NT2_TEST_CASE
//////////////////////////////////////////////////////////////////////////////
//NT2_TEST_CASE_TPL(expint, NT2_SIMD_TYPES )
//{
//  using nt2::expint;
//  using nt2::functors::expint;
//
//  using nt2::simd::native;
//  using nt2::meta::cardinal_of;
//
//  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
//  typedef native<T,ext_t>             n_t;
//
//  NT2_TEST( (boost::is_same<typename nt2::meta::call<boost_math_(n_t, n_t)>::type,
//                            typename std::tr1::result_of<nt2::meta::floating<($self.const_n_t_type_list$)>::type
//                            >::value)
//          );
//  NT2_ALIGNED_TYPE(T) data[2*cardinal_of<n_t>::value];
//  for(std::size_t i=0;i<2*cardinal_of<n_t>::value;++i)
//    data[i] = i; // good value here for expint
//
//    n_t a0 = load<n_t>(&data[0],0);
//    n_t a1 = load<n_t>(&data[0],1);
//    n_t v  = expint(a0, a1);
//    for(std::size_t j=0;j<cardinal_of<n_t>::value;++j)
//    {
//      NT2_TEST_EQUAL( v[j], expint(a0[j], a1[j]) );
//    }
//  }
//}


#include "hsm/details/collect_initial_states.h"
#include "hsm/details/idx.h"
#include "hsm/details/transition_table.h"
#include "hsm/front/transition_dsl.h"

#include <boost/hana.hpp>
#include <boost/hana/experimental/printable.hpp>
#include <gtest/gtest.h>

using namespace ::testing;
namespace bh = boost::hana;

class CollectInitialStatesTests : public Test {
};

namespace {
class E1 {
};
class S1 {
};
class S2 {
};

struct S {
    static constexpr auto make_transition_table()
    {
        // clang-format off
            return hsm::transition_table(
                * hsm::state<S1> {} = hsm::state<S1> {},
                * hsm::state<S2> {} = hsm::state<S2> {}
            );
        // clang-format on
    }
};
}

TEST_F(CollectInitialStatesTests, should_collect_initial_states)
{
    constexpr auto collectedInitialStates
        = bh::transform(hsm::collect_initial_states(hsm::state<S> {}), bh::typeid_);

    ASSERT_EQ(bh::size_c<2>, bh::size(collectedInitialStates));
    ASSERT_EQ(bh::typeid_(hsm::state<S1> {}), bh::at_c<0>(collectedInitialStates));
    ASSERT_EQ(bh::typeid_(hsm::state<S2> {}), bh::at_c<1>(collectedInitialStates));
}

TEST_F(CollectInitialStatesTests, should_make_initial_state_map)
{
    constexpr auto initialStateMap = hsm::make_initial_state_map(hsm::state<S> {});

    ASSERT_EQ(bh::size_c<1>, bh::size(initialStateMap));

    constexpr auto initialStates = bh::find(initialStateMap, bh::typeid_(hsm::state<S> {})).value();

    ASSERT_EQ(bh::size_c<2>, bh::size(initialStates));
    ASSERT_EQ(bh::size_c<0>, bh::at_c<0>(initialStates));
    ASSERT_EQ(bh::size_c<1>, bh::at_c<1>(initialStates));
}

TEST_F(CollectInitialStatesTests, should_count_max_initial_states)
{
    ASSERT_EQ(bh::size_c<2>, hsm::maxInitialStates(hsm::state<S> {}));
}

TEST_F(CollectInitialStatesTests, should_fill_initial_state_table)
{
    std::array<std::vector<std::size_t>, hsm::maxInitialStates(hsm::state<S> {})> initialStateTable;
    hsm::fill_initial_state_table(hsm::state<S> {}, initialStateTable);
    ASSERT_EQ(2, initialStateTable.size());

    ASSERT_EQ(2, initialStateTable[0].size());

    ASSERT_EQ(0, initialStateTable[0][0]);
    ASSERT_EQ(1, initialStateTable[0][1]);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_TRAITS_DIFFERENT_NON_COMPLEX_INCLUDE
#define MTL_TRAITS_DIFFERENT_NON_COMPLEX_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/type_traits/is_scalar.hpp>

namespace mtl { namespace traits {

/// Type trait for different non-complex scalars, i.e. pairs of scalars whose complex equivalents are not supported in binary operations
template <typename T, typename U>
struct different_non_complex
  : boost::mpl::bool_< !boost::is_same<T, U>::value && !boost::is_complex<T>::value && !boost::is_complex<U>::value
                       && boost::is_scalar<T>::value &&  boost::is_scalar<U>::value>
{};

}} // namespace mtl::traits

#endif // MTL_TRAITS_DIFFERENT_NON_COMPLEX_INCLUDE


#include <memory>
#include <random>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/unique-id.h>
#include <posegraph/edge.h>
#include <posegraph/unique-id.h>
#include <posegraph/vertex.h>
#include <sensors/sensor-factory.h>

#include "vi-map/mission.h"
#include "vi-map/test/vi-map-test-helpers.h"
#include "vi-map/transformation-edge.h"
#include "vi-map/vi-map.h"
#include "vi-map/viwls-edge.h"

namespace vi_map {

class VIMapEdgeMergingTest : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    vi_map::test::generateMap<ViwlsEdge>(kTestDataVertexCount, &map_);
    CHECK_EQ(map_.numMissions(), 1u);
    CHECK_EQ(map_.numVertices(), kTestDataVertexCount);
    mission_id_ = map_.getIdOfFirstMission();
  }

  TransformationEdge getOutgoingTransformationEdge(
      const pose_graph::VertexId& vertex_id) const;

  ViwlsEdge getOutgoingViwlsEdge(const pose_graph::VertexId& vertex_id) const;

  void mergeRandomVertex(
      const pose_graph::VertexId& vertex_id_from,
      const bool has_wheel_odometry_edge);

  void addWheelOdometryEdges();

  vi_map::VIMap map_;
  vi_map::MissionId mission_id_;

 private:
  // Modify if test dataset with different vertex count used.
  static constexpr size_t kTestDataVertexCount = 405u;
};

void VIMapEdgeMergingTest::addWheelOdometryEdges() {
  CHECK(mission_id_.isValid());

  Sensor::UniquePtr wheel_odometry_sensor =
      createTestSensor(SensorType::kRelative6DoFPose);
  CHECK(wheel_odometry_sensor);
  const SensorId wheel_odometry_sensor_id = wheel_odometry_sensor->getId();
  map_.getSensorManager().addSensor(
      std::move(wheel_odometry_sensor), mission_id_);
  CHECK(wheel_odometry_sensor_id.isValid());

  const pose_graph::Edge::EdgeType traversal_edge_type =
      map_.getGraphTraversalEdgeType(mission_id_);

  pose_graph::VertexId vertex_id_k =
      map_.getMission(mission_id_).getRootVertexId();
  CHECK(vertex_id_k.isValid());

  aslam::Transformation T_G_Ik = map_.getVertex_T_G_I(vertex_id_k);

  pose_graph::VertexId vertex_id_kp1;
  while (map_.getNextVertex(vertex_id_k, traversal_edge_type, &vertex_id_kp1)) {
    CHECK(vertex_id_kp1.isValid());

    const aslam::Transformation T_G_Ikp1 = map_.getVertex_T_G_I(vertex_id_kp1);

    const aslam::Transformation T_Ik_Ikp1 = T_G_Ik.inverse() * T_G_Ikp1;
    TransformationEdge::UniquePtr transformation_edge =
        aligned_unique<TransformationEdge>(
            Edge::EdgeType::kOdometry,
            common::createRandomId<pose_graph::EdgeId>(), vertex_id_k,
            vertex_id_kp1, T_Ik_Ikp1,
            aslam::TransformationCovariance::Identity(),
            wheel_odometry_sensor_id);

    map_.addEdge(std::move(transformation_edge));

    T_G_Ik = T_G_Ikp1;
    vertex_id_k = vertex_id_kp1;
  }
}

TransformationEdge VIMapEdgeMergingTest::getOutgoingTransformationEdge(
    const pose_graph::VertexId& vertex_id) const {
  CHECK(vertex_id.isValid());
  pose_graph::EdgeIdSet outgoing_edge_ids;
  map_.getVertex(vertex_id).getOutgoingEdges(&outgoing_edge_ids);

  CHECK(!outgoing_edge_ids.empty());

  pose_graph::EdgeId outgoing_transformation_edge_id;
  outgoing_transformation_edge_id.setInvalid();
  for (const pose_graph::EdgeId& edge_id : outgoing_edge_ids) {
    CHECK(edge_id.isValid());
    if (map_.getEdgeType(edge_id) == Edge::EdgeType::kOdometry) {
      outgoing_transformation_edge_id = edge_id;
    }
  }
  CHECK(outgoing_transformation_edge_id.isValid());

  return map_.getEdgeAs<TransformationEdge>(outgoing_transformation_edge_id);
}

ViwlsEdge VIMapEdgeMergingTest::getOutgoingViwlsEdge(
    const pose_graph::VertexId& vertex_id) const {
  CHECK(vertex_id.isValid());
  pose_graph::EdgeIdSet outgoing_edge_ids;
  map_.getVertex(vertex_id).getOutgoingEdges(&outgoing_edge_ids);

  CHECK(!outgoing_edge_ids.empty());

  pose_graph::EdgeId outgoing_viwls_edge_id;
  outgoing_viwls_edge_id.setInvalid();
  for (const pose_graph::EdgeId& edge_id : outgoing_edge_ids) {
    CHECK(edge_id.isValid());
    if (map_.getEdgeType(edge_id) == Edge::EdgeType::kViwls) {
      outgoing_viwls_edge_id = edge_id;
    }
  }
  CHECK(outgoing_viwls_edge_id.isValid());

  return map_.getEdgeAs<ViwlsEdge>(outgoing_viwls_edge_id);
}

void VIMapEdgeMergingTest::mergeRandomVertex(
    const pose_graph::VertexId& vertex_id_from,
    const bool has_wheel_odometry_edge) {
  const pose_graph::Edge::EdgeType traversal_edge_type =
      map_.getGraphTraversalEdgeType(mission_id_);
  pose_graph::VertexId next_vertex_id;
  const bool from_has_next_vertex =
      map_.getNextVertex(vertex_id_from, traversal_edge_type, &next_vertex_id);
  if (!from_has_next_vertex) {
    return;
  }

  const ViwlsEdge viwls_edge_1 = getOutgoingViwlsEdge(vertex_id_from);
  const pose_graph::VertexId& in_between_vertex_id = viwls_edge_1.to();
  ASSERT_TRUE(in_between_vertex_id.isValid());

  pose_graph::EdgeId wheel_odometry_edge_1_id;
  aslam::Transformation wheel_odometry_edge_1_T_A_B;
  if (has_wheel_odometry_edge) {
    const TransformationEdge wheel_odometry_edge_1 =
        getOutgoingTransformationEdge(vertex_id_from);
    wheel_odometry_edge_1_id = wheel_odometry_edge_1.id();
    wheel_odometry_edge_1_T_A_B = wheel_odometry_edge_1.getT_A_B();
    ASSERT_EQ(wheel_odometry_edge_1.to(), in_between_vertex_id);
  }

  const bool in_between_has_next_vertex = map_.getNextVertex(
      in_between_vertex_id, traversal_edge_type, &next_vertex_id);

  if (in_between_has_next_vertex) {
    const ViwlsEdge viwls_edge_2 = getOutgoingViwlsEdge(in_between_vertex_id);
    ASSERT_TRUE(viwls_edge_2.to().isValid());

    pose_graph::EdgeId wheel_odometry_edge_2_id;
    aslam::Transformation wheel_odometry_edge_2_T_A_B;
    if (has_wheel_odometry_edge) {
      const TransformationEdge wheel_odometry_edge_2 =
          getOutgoingTransformationEdge(in_between_vertex_id);
      wheel_odometry_edge_2_id = wheel_odometry_edge_2.id();
      wheel_odometry_edge_2_T_A_B = wheel_odometry_edge_2.getT_A_B();
      ASSERT_EQ(wheel_odometry_edge_2.to(), viwls_edge_2.to());
    }

    map_.mergeNeighboringVertices(vertex_id_from, in_between_vertex_id);
    ASSERT_TRUE(map_.hasVertex(vertex_id_from));
    ASSERT_FALSE(map_.hasVertex(in_between_vertex_id));
    ASSERT_FALSE(map_.hasEdge(viwls_edge_1.id()));
    ASSERT_FALSE(map_.hasEdge(viwls_edge_2.id()));

    const ViwlsEdge merged_vilws_edge = getOutgoingViwlsEdge(vertex_id_from);
    EXPECT_EQ(merged_vilws_edge.to(), viwls_edge_2.to());

    if (has_wheel_odometry_edge) {
      const aslam::Transformation T_A_B_merged =
          wheel_odometry_edge_1_T_A_B * wheel_odometry_edge_2_T_A_B;

      const TransformationEdge merged_wheel_odometry_edge =
          getOutgoingTransformationEdge(vertex_id_from);
      EXPECT_EQ(merged_wheel_odometry_edge.getT_A_B(), T_A_B_merged);
      EXPECT_EQ(merged_wheel_odometry_edge.from(), vertex_id_from);
      EXPECT_EQ(merged_wheel_odometry_edge.to(), viwls_edge_2.to());

      ASSERT_FALSE(map_.hasEdge(wheel_odometry_edge_1_id));
      ASSERT_FALSE(map_.hasEdge(wheel_odometry_edge_2_id));
    }
  } else {
    map_.mergeNeighboringVertices(vertex_id_from, in_between_vertex_id);
    ASSERT_TRUE(map_.hasVertex(vertex_id_from));
    ASSERT_FALSE(map_.hasVertex(in_between_vertex_id));
    ASSERT_FALSE(map_.hasEdge(viwls_edge_1.id()));
    if (has_wheel_odometry_edge) {
      ASSERT_FALSE(map_.hasEdge(wheel_odometry_edge_1_id));
    }
  }
}

TEST_F(VIMapEdgeMergingTest, EdgeMergingTestViwls) {
  CHECK_GT(map_.numVertices(), 0u);

  constexpr size_t kSeed = 42u;
  std::srand(kSeed);
  std::mt19937 integer_generator(kSeed);
  constexpr size_t kLowerBound = 0u;

  constexpr size_t kMaxIterations = 1000u;
  size_t iteration = 0u;
  do {
    pose_graph::VertexIdList vertex_ids;
    map_.getAllVertexIds(&vertex_ids);

    if (vertex_ids.size() < 2u) {
      break;
    }
    const size_t upper_bound = vertex_ids.size() - 1u;
    std::uniform_int_distribution<> vertex_index_uniform_distribution(
        kLowerBound, upper_bound);

    const size_t vertex_idx_from =
        vertex_index_uniform_distribution(integer_generator);

    const pose_graph::VertexId& vertex_id_from = vertex_ids[vertex_idx_from];
    constexpr bool kHasWheelOdometryEdges = false;
    mergeRandomVertex(vertex_id_from, kHasWheelOdometryEdges);

    ++iteration;
  } while (iteration < kMaxIterations);
}

TEST_F(VIMapEdgeMergingTest, EdgeMergingTestViwlsOdometry) {
  addWheelOdometryEdges();

  CHECK_GT(map_.numVertices(), 0u);

  constexpr size_t kSeed = 42u;
  std::srand(kSeed);
  std::mt19937 integer_generator(kSeed);
  constexpr size_t kLowerBound = 0u;

  constexpr size_t kMaxIterations = 1000u;
  size_t iteration = 0u;
  do {
    pose_graph::VertexIdList vertex_ids;
    map_.getAllVertexIds(&vertex_ids);

    if (vertex_ids.size() < 2u) {
      break;
    }
    const size_t upper_bound = vertex_ids.size() - 1u;
    std::uniform_int_distribution<> vertex_index_uniform_distribution(
        kLowerBound, upper_bound);

    const size_t vertex_idx_from =
        vertex_index_uniform_distribution(integer_generator);

    const pose_graph::VertexId& vertex_id_from = vertex_ids[vertex_idx_from];
    constexpr bool kHasWheelOdometryEdges = true;
    mergeRandomVertex(vertex_id_from, kHasWheelOdometryEdges);

    ++iteration;
  } while (iteration < kMaxIterations);
}

}  // namespace vi_map

MAPLAB_UNITTEST_ENTRYPOINT