/*

Project: icp alignment for localization
Date: 20/08/20
@Author: ALL
Detail: align two clouds in the localization,
        so as to get the global pose of the current frame
Scenario: localization

*/
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/common/transforms.h>
#include <vector>

#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/sem.h>

#include <string.h>
#include <dirent.h>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <fstream>
#include <iostream>

#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>

#include "../include/Lidar.h"
#include "../include/ipc.h"
#include "../include/QuadTree.h" //structure define and the other shits
#include "../include/Localizate.h"
#include "../include/ProcessCom.h"

using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormal;

struct information_of_barrier
{
    float centerx;
    float centery;
};
int num_of_barrier;

struct Posemat
{
    Eigen::Matrix4f global_transform;
};

//uniform sampling func
//data acquired: Cloud's ptr for processing.
void uniform_sampling (PointCloud::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::UniformSampling<pcl::PointXYZ> US;
    US.setInputCloud(cloud);
    US.setRadiusSearch(0.2f);
    US.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
}

//icp point align algorithm
//data acquired: 1 & 2 is the Cloud's ptr for aligning, 3 is the Cloud's ptr for output, 4 is transformation matrix, 5 is unclear.
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f&final_transform)
{
    int iterations = 100;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_tgt);
    icp.setTransformationEpsilon(1e-15);
    icp.setMaxCorrespondenceDistance(2);
    icp.setEuclideanFitnessEpsilon(0.001);
    icp.setMaximumIterations(iterations);
    icp.align(*output);
    final_transform = icp.getFinalTransformation();
}

// func from raw pose values to the eigen transformation mat
void six2Trans(float roll, float pitch, float yaw, float x, float y, float z, Eigen::Matrix4f& transform)
{
    Eigen::Vector3f eulerAngle(yaw, pitch, roll);
    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(eulerAngle(2),Eigen::Vector3f::UnitX()));
    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(eulerAngle(1),Eigen::Vector3f::UnitY()));
    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(eulerAngle(0),Eigen::Vector3f::UnitZ()));

    Eigen::Matrix3f rotation;
    rotation = yawAngle * pitchAngle * rollAngle;

    Eigen::Translation3f translation(x, y, z);
    transform = (translation * rotation).matrix();
}

void quaterniond2eulerangle(Eigen::Quaternionf& q, float& roll, float& pitch, float& yaw)
{
    double sinr_cosp = +2.0*(q.w()*q.x() + q.y()*q.z());
    double cosr_cosp = +1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
    roll = atan2(sinr_cosp, cosr_cosp);

    double sinp = +2.0*(q.w()*q.y() - q.z()*q.x());
    if(fabs(sinp) >= 1) pitch = copysign(M_PI/2, sinp);
    else pitch = asin(sinp);

    double siny_cosp = +2.0*(q.w()*q.z() + q.x()*q.y());
    double cosy_cosp = +1.0 - 2.0*(q.y()*q.y() + q.z()*q.z());
    yaw = atan2(siny_cosp, cosy_cosp);
}

vector<information_of_barrier> remove_duplicates( vector<vector<float>> &data)
{
    for(int i = 0; i < data.size(); i++)
    {
        data[i][0] = (float)round(data[i][0]*4);
        data[i][1] = (float)round(data[i][1]*4);
    }
    sort(data.begin(),data.end());
    data.erase(unique(data.begin(), data.end()),data.end());
    std::vector<information_of_barrier> Information_of_barrier;
    for(int i = 0; i < data.size(); i++)
    {
        if(data[i][0]<=12&&data[i][0]>=0&&data[i][1]<=8&&data[i][1]>=-8){
        information_of_barrier barrier;
        barrier.centerx = data[i][0]/4;
        barrier.centery = data[i][1]/4;
        Information_of_barrier.push_back(barrier);
        }

    }
    return Information_of_barrier;
}

int main(int argc, char** argv)
{
    InitLocalizate();

    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;

    struct QuadTreeNode root;
    struct Region root_region;
    struct ElePoint ele;
    struct ElePoint old_ele;
    initRegion(&root_region, -50, 100, -50, 100);
    initNode(&root, 1, root_region);

    std::vector<std::vector<float>> data3d;
    std::vector<std::vector<float>> data_after;
    std::vector<float> data2d;
    std::vector<Posemat> Pose;
    int line = 0;

    ifstream fin("../pose.txt");
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity();
    while(!fin.eof())
    {
        fin >> line >> x >> y >> z >> yaw >> pitch >> roll;
        cout << "Checking No. " << line << " Pose." << endl;
        six2Trans(roll, pitch, yaw, x, y, z, GlobalTransform);
        Posemat a;
        a.global_transform = GlobalTransform;
        cout << "global: \n" << GlobalTransform << endl;
        Pose.push_back(a);
        ele.x = x;
        ele.y = y;
        ele.index = line;
        insertEle(&root, ele);
        if (!fin.good()) break;
    }

    pthread_mutex_init(&LidarMutex,NULL);
    lidar.InitLidar();//start get lidar data
    sleep(3);

    InitSem(LidarOccPosSemId, LidarOccPosSemKey, 1, IPC_CREAT|0777);
    MessageQueueInit(LidarOccPosMsgId, LidarOccPosMsgKey, IPC_CREAT|0777);
    InitShareMemory(LidarOccPosShmId, LidarOccPosShmKey,4096, IPC_CREAT|0777);

    V_sem(LidarOccPosSemId, 0);
    V_sem(PosSemId, 0);

    while(1)
    {
        PointCloud::Ptr source(new PointCloud);
        PointCloud::Ptr target(new PointCloud);
        PointCloud::Ptr temp(new PointCloud);
        PointCloud::Ptr Lidarcloud(new PointCloud);

        pthread_mutex_lock(&LidarMutex);
        pcl::copyPointCloud(*safecloud, *Lidarcloud);
        //end updata lidar data

        for(int i = 0; i<Lidarcloud->size(); i++)
        {
            if(Lidarcloud->points[i].x >= 0.0 && Lidarcloud->points[i].x <= 3.0 && Lidarcloud->points[i].y >= -2 && Lidarcloud->points[i].y <= 2)
            {
                data2d.push_back(Lidarcloud->points[i].x);
                data2d.push_back(Lidarcloud->points[i].y);
                data3d.push_back(data2d);
                data2d.clear();
            }
        }
        std::vector<information_of_barrier> Information_of_barrier = remove_duplicates(data3d);
        //std::cout<<"*********"<<Information_of_barrier.size()<<std::endl;

        //链接内存
        void* lidarOccPosMemory;
        lidarOccPosMemory = AttachShareMemory(LidarOccPosShmId);

        if(lidarOccPosMemory == (void*)(-1))
        {
            std::cout << "Get occPosMemoy failed" << std::endl;
            return -1;
        }

        P_sem(LidarOccPosSemId, 0);//使用互斥量
        int occNum = Information_of_barrier.size();
        //std::cout << "occNum" << occNum << std::endl;
        WriteShareMemory(lidarOccPosMemory, &occNum, sizeof(int));
        for(int loop = 0; loop < occNum; loop++)
        {
            WriteShareMemory((void*)(lidarOccPosMemory + sizeof(int) + loop * sizeof(information_of_barrier)), &Information_of_barrier[loop], sizeof(information_of_barrier));
        }
        V_sem(LidarOccPosSemId, 0);

        int ret = DisattachShareMemory(lidarOccPosMemory);
        if(ret != 0)
        {
            std::cout << "Failed detached memory" << std::endl;
        }

        //get the index from quad_tree
        float x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now;
        x_now = y_now = z_now = roll_now = pitch_now = yaw_now = v_now = w_now = 0;
        PriorLocation(x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now);
        //float d = 1;
        //float disturbance[9][2] = {{d, 0}, {d, d}, {0, d}, {0, 0}, {-d, d}, {-d, 0}, {-d, -d}, {0, -d}, {d, -d}};
        float candidate[MAX_ELE_NUM*9 + 1][3] = {0};
        float min_distance = 1000;
        float distance = 0;
        int found_index = -1;

        //find possible index
        //for(int i = 0; i < 9; i++)
        //{
            struct ElePoint test;
            test.x = x_now;// + disturbance[i][0];
            test.y = y_now; // + disturbance[i][1];
            queryEle(root, test, candidate, candidate[MAX_ELE_NUM*9][0]);
        //}

        //find closest index
        for(int k = 0; k < candidate[MAX_ELE_NUM*9][0]; k++)
        {
            distance = hypot(x_now - candidate[k][0], y_now - candidate[k][1]);
            //cout << "candidate_index: " << candidate[k][2] << " , distance: " << distance << endl;
            //1. to eradicate the keyframes on the time-related sequence
            //2. to find the closest point to the determined pose
            if(distance < min_distance)
            {
                min_distance = distance;
                found_index = candidate[k][2];
            }
        }
        cout << "found_index: " << found_index << " , mindistance: " << min_distance << endl;
        //pseudo mat
        Eigen::Matrix4f globalTransform_curr = Eigen::Matrix4f::Identity(); // for the result store
        Eigen::Matrix4f pairTransform        = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f priorTransform       = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f relateTransform      = Eigen::Matrix4f::Identity();
        float min = 0;

        cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX" << endl;
        cout << "x: " << x_now << ", y: " <<  y_now  << ", yaw: "<< yaw_now << endl;
        six2Trans(roll_now, pitch_now, yaw_now, x_now, y_now, z_now, priorTransform);

        cout << "prior: \n" << priorTransform << endl;
        cout << "keyframe: \n" << Pose[found_index].global_transform << endl;
        relateTransform = Pose[found_index].global_transform.inverse() * priorTransform;
        cout << "relate: \n" << relateTransform << endl;

        min = hypot(relateTransform(0,3), relateTransform(1,3));
        float x_new, y_new, z_new, roll_new, pitch_new, yaw_new;
        if(min < 0.4)
        {
            char namebuf[30];
            sprintf(namebuf, "../data/%d.pcd", found_index+1);
            pcl::io::loadPCDFile (namebuf, *source);    //get source cloud from keyframe inventory

            uniform_sampling(Lidarcloud);
            uniform_sampling(source);
            cout << "size after filter:" << Lidarcloud->size() << endl;

            pcl::transformPointCloud(*Lidarcloud, *target, relateTransform);
            pairAlign(target, source, temp, pairTransform);
            cout << "pair: \n" << pairTransform << endl;
            cout << "pair inverse: \n" <<  pairTransform.inverse() << endl;
            globalTransform_curr = Pose[found_index].global_transform * pairTransform * relateTransform;
            cout << "ture: \n" << globalTransform_curr<< endl;

            //sent to local
            Eigen::Matrix3f rotation_matrix = globalTransform_curr.block<3,3>(0,0);
            Eigen::Quaternionf quaternion(rotation_matrix);
            quaterniond2eulerangle(quaternion, roll_new, pitch_new, yaw_new);
            x_new = globalTransform_curr(0,3);
            y_new = globalTransform_curr(1,3);
            z_new = globalTransform_curr(2,3);
            PosteriorLocation(x_new, y_new, z_new, roll_new, pitch_new, yaw_new, v_now, w_now);
        }
        else
        {
            PosteriorLocation(x_now, y_now, z_now, roll_now, pitch_now, yaw_now, v_now, w_now);
        }
        pthread_mutex_unlock(&LidarMutex);
    }
    DestoryLocalizate();
    return 0;
}


/****************************
 * 题目：给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。
 * 使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。
 *
* 本程序学习目标：
 * 熟悉g2o库编写流程，熟悉顶点定义方法。
 *
 * 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM
 * 时间：2019.02
****************************/

#include <vector>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace Eigen;

using namespace cv;
using namespace std;

string p3d_file = "./../p3d.txt";
string p2d_file = "./../p2d.txt";

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K );

int main(int argc, char **argv) {

    vector< Point3f > p3d;//保存世界坐标系下的3D点
    vector< Point2f > p2d;//保存相机坐标系下对应的2D点

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K );
    return 0;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    //Block* solver_ptr = new Block (std::unique_ptr<Block::LinearSolverType>(linearSolver));
	    Block* solver_ptr = new Block (linearSolver);
    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

//    // old g2o version
//    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
//    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中

	// ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵

	//添加相机的位姿，这里为待优化的变量，直接设置为单位矩阵
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose，g2o内定的类型
    Eigen::Matrix3d R_mat;

    R_mat.setIdentity(3,3);
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( 0, 0, 0) )
                        );
    optimizer.addVertex ( pose );

    //g2o中世界坐标系的3维坐标才是顶点（vertex），而其对应的相机坐标系下的点是评估（estimate）点!!!!!!
    int index=1;
    for(const Point3f p:points_3d)
    {
		g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );//只有观测点
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

	// ----------------------结束你的代码

    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


/*
 * LineIterator.hpp
 *
 *  Created on: Nov 13, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"

#include <Eigen/Core>

namespace grid_map {

/*!
 * Iterator class to iterate over a line in the map.
 * Based on Bresenham Line Drawing algorithm.
 */
class LineIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting point of the line.
   * @param end the ending point of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Position& start, const Position& end);

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting index of the line.
   * @param end the ending index of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  LineIterator& operator =(const LineIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const LineIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Index& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  LineIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

private:

  /*!
   * Construct function.
   * @param gridMap the grid map to iterate on.
   * @param start the starting index of the line.
   * @param end the ending index of the line.
   * @return true if successful, false otherwise.
   */
  bool initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end);

  /*!
   * Computes the parameters requires for the line drawing algorithm.
   */
  void initializeIterationParameters();

  /*!
   * Finds the index of a position on a line within the limits of the map.
   * @param[in] gridMap the grid map that defines the map boundaries.
   * @param[in] start the position that will be limited to the map range.
   * @param[in] end the ending position of the line.
   * @param[out] index the index of the moved start position.
   * @return true if successful, false otherwise.
   */
  bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
                                 const Position& end, Index& index);

  //! Current index.
  Index index_;

  //! Starting index of the line.
  Index start_;

  //! Ending index of the line.
  Index end_;

  //! Current cell number.
  unsigned int iCell_;

  //! Number of cells in the line.
  unsigned int nCells_;

  //! Helper variables for Bresenham Line Drawing algorithm.
  Size increment1_, increment2_;
  int denominator_, numerator_, numeratorAdd_;

  //! Map information needed to get position from iterator.
  Length mapLength_;
  Position mapPosition_;
  double resolution_;
  Size bufferSize_;
  Index bufferStartIndex_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} /* namespace */


//igl
#include <igl/opengl/glfw/Viewer.h>
#include <igl/readMESH.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/per_vertex_normals.h>
#include <igl/per_corner_normals.h>
#include <igl/boundary_facets.h>
#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>
#include <igl/mat_max.h>
#include <igl/mat_min.h>
#include <igl/PI.h>
#include <igl/volume.h>
#include <igl/cat.h>
#include <igl/png/writePNG.h>

//internal
#include <json.hpp>
#include <read_data_from_json.h>
#include <lumped_mass_matrix.h>
#include <lbs_matrix.h>
#include <create_mask_matrix.h>
#include <line_search.h>
#include <util.h>

//Bartels
#include <linear_tetmesh_dphi_dX.h>
#include <linear_tetmesh_arap_dq.h>
#include <linear_tetmesh_arap_dq2.h>
#include <linear_tetmesh_arap_q.h>
#include <linear_tetmesh_neohookean_dq.h>
#include <linear_tetmesh_neohookean_dq2.h>
#include <linear_tetmesh_neohookean_q.h>
#include <linear_tetmesh_stvk_dq.h>
#include <linear_tetmesh_stvk_dq2.h>
#include <linear_tetmesh_stvk_q.h>
#include <linear_tetmesh_corotational_dq.h>
#include <linear_tetmesh_corotational_dq2.h>
#include <linear_tetmesh_corotational_q.h>
#include <simple_psd_fix.h>
#include <Eigen/Sparse>
#include <Eigen/Core>

#include <iostream>
#include <filesystem>

const Eigen::Vector3d red(255./255.,0./255.,0./255.);
int frame = 0;

void export_png_seq(igl::opengl::glfw::Viewer& viewer, const std::vector<Eigen::MatrixXd>& Vn_list,
                    const std::string& json_path, const std::string& physic_model)
{
  frame = 0;
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &) -> bool
	{
		if(viewer.core().is_animating)
		{
      Eigen::MatrixXd Vn = Vn_list[frame];
      // viewer.data().set_mesh(Vn,F);
      viewer.data().set_vertices(Vn);
      viewer.data().compute_normals();

      const std::string dir = igl::dirname(json_path) + PATH_SEPARATOR;;
      size_t dot_found = json_path.find_last_of(".");
      std::string model = std::string(json_path.begin()+dir.size(),json_path.begin()+dot_found);

      std::string output_folder = "../showcases";
      std::string output_path = output_folder+"/"+model+"_"+physic_model;
      if(!std::filesystem::exists(output_path)){
        std::filesystem::create_directory(output_path);
      }

      std::string numstr = std::to_string(frame);
      int strlen = numstr.length();

      std::string seqstr;
      for(int k=0; k<(4-strlen); k++){
          seqstr.append("0");
      }
      seqstr.append(numstr);
      std::string save_path = output_path+"/"+model+"_"+physic_model+"_"+seqstr+".png";

      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> R(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> G(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> B(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> A(1280,800);

      // Draw the scene in the buffers
      viewer.core().draw_buffer(viewer.data(),true,R,G,B,A);
      igl::png::writePNG(R,G,B,A,save_path);

			frame++;
      if(frame == Vn_list.size()){
        viewer.core().is_animating = !viewer.core().is_animating;
        frame = 0;
      }
		}
		return false;
	};
}

int main(int argc, char *argv[])
{
  Eigen::MatrixXd V,C,W,VM; //V: vertices of tet-mesh, C: joint positions, W: skinning weight, VM: lbs matrix
  Eigen::MatrixXi T,F,BE; //T: tet indices  of tet-mesh, F: face indices of tet-mesh, BE: edge indices of bone handle
  Eigen::VectorXi PI; //PI: point indices of point handle

  std::vector<Eigen::MatrixXd> TF_list; //list of transformation
  double dt, YM, pr, scale; //dt: time step, YM: young's modulus, pr: poisson ratio, scale: custom scale of mesh

  std::string json_path = argc>1?std::string(argv[1]):"../examples/sphere/sphere.json";
  std::string physic_model;
  read_json_data(json_path,V,T,F,C,PI,BE,W,TF_list,dt,YM,pr,scale,physic_model);

  std::string str = argc>2?std::string(argv[2]):"";
  bool export_objs = false;
  if(str=="export")
    export_objs=true;

  double lambda, mu;
  emu_to_lame(YM,pr,lambda,mu);

  Eigen::MatrixXd params(T.rows(),2);
  params.col(0) = 0.5*lambda*Eigen::VectorXd::Ones(T.rows());
  params.col(1) = mu*Eigen::VectorXd::Ones(T.rows());

  igl::boundary_facets(T, F);
  F = F.rowwise().reverse().eval();

	igl::lbs_matrix(V,W,VM);

  Eigen::VectorXd vol;
  igl::volume(V,T,vol);

  Eigen::MatrixXd dX;
  sim::linear_tetmesh_dphi_dX(dX,V,T);

  Eigen::SparseMatrix<double> M;
  lumped_mass_matrix(V,T,M);
  M = M*1000;

  Eigen::SparseMatrix<double> A;
  lbs_matrix_column(V,W,A);

  Eigen::SparseMatrix<double> phi;
  create_poisson_mask_matrix(V,T,phi);

  Eigen::SparseMatrix<double> Aeq = A.transpose()*M*phi;
  Eigen::VectorXd Beq = Eigen::VectorXd::Zero(A.cols());

  Eigen::MatrixXd TF = TF_list[0];

  Eigen::MatrixXd Vr = VM*TF;
  Eigen::MatrixXd U = Vr-V;

  int nc = V.rows()*V.cols();

  Eigen::VectorXd UCol = vectorize(U);

  Eigen::VectorXd VCol = vectorize(V);

  Eigen::VectorXd UdCol = Eigen::VectorXd::Zero(nc);
  Eigen::VectorXd UcCol = Eigen::VectorXd::Zero(nc);

  std::vector<Eigen::MatrixXd> Vn_list(TF_list.size());
  int max_iter = 20;
  for(int ai=0; ai<TF_list.size(); ai++){
    std::cout<<"frame: "<<ai<<std::endl;

    Eigen::MatrixXd TF = TF_list[ai];

    Eigen::VectorXd UCol0 = UCol;
    Eigen::VectorXd UdCol0 = UdCol;
    Eigen::VectorXd UcCol0 = UcCol;

    Vr = VM*TF;
    Eigen::MatrixXd Ur = Vr-V;
    Eigen::VectorXd UrCol = vectorize(Ur);

    for(int i=0; i<max_iter; i++)
    {
      Eigen::VectorXd q = VCol+UrCol+UcCol;
      Eigen::VectorXd G;
      Eigen::SparseMatrix<double> K;

      if(physic_model=="arap"){
        sim::linear_tetmesh_arap_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_arap_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="neohookean"){
        sim::linear_tetmesh_neohookean_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_neohookean_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="stvk"){
        sim::linear_tetmesh_stvk_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_stvk_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="corotational"){
        sim::linear_tetmesh_corotational_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_corotational_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      Eigen::VectorXd tmp_g = M/(dt*dt) * (UrCol+UcCol) - M*(UCol0/(dt*dt)+UdCol0/dt) + G;
      Eigen::SparseMatrix<double> tmp_H = M/(dt*dt) + K;
      tmp_H = 0.5 * (tmp_H+Eigen::SparseMatrix<double>(tmp_H.transpose()));

      Eigen::SparseMatrix<double> AR1, AR2, AA;
      igl::cat(2,tmp_H,Eigen::SparseMatrix<double>(Aeq.transpose()),AR1);
      Eigen::SparseMatrix<double> SI(Aeq.rows(), Aeq.rows());
      SI.setZero();
      igl::cat(2,Aeq,SI,AR2);
      igl::cat(1,AR1,AR2,AA);

      Eigen::VectorXd b(tmp_g.size()+Beq.size());
      b.head(tmp_g.size())=-tmp_g;
      b.tail(Beq.size())=Beq;

      Eigen::SimplicialLDLT<Eigen::SparseMatrix<double > > ldlt(AA);
      Eigen::VectorXd dUc = ldlt.solve(b).head(tmp_g.size());

      if(tmp_g.transpose()*dUc > -1e-6)
        break;

      std::function<double(const Eigen::VectorXd &, const Eigen::VectorXd &)> f;
      f = [&](const Eigen::VectorXd &UrColi, const Eigen::VectorXd &UcColi){
        double e = 0.5*(UrColi+UcColi-UCol0-dt*UdCol0).transpose()*M/(dt*dt)*(UrColi+UcColi-UCol0-dt*UdCol0);
        if(physic_model=="arap"){
          return e + sim::linear_tetmesh_arap_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="neohookean"){
          return e + sim::linear_tetmesh_neohookean_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="stvk"){
          return e + sim::linear_tetmesh_stvk_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="corotational"){
          return e + sim::linear_tetmesh_corotational_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
      };
      double alpha = line_search(f,tmp_g,dUc,UrCol,UcCol);
      UcCol = UcCol + alpha * dUc;
    }
    UCol = UrCol + UcCol;
    UdCol = (UCol-UCol0)/dt;

    Eigen::MatrixXd Vn = V + matrixize(UCol);
    Vn_list[ai] = Vn;

  }
  if(export_objs)
  {
    const std::string dir = igl::dirname(json_path) + PATH_SEPARATOR;;
    size_t dot_found = json_path.find_last_of(".");
    std::string model = std::string(json_path.begin()+dir.size(),json_path.begin()+dot_found);

    std::string output_folder = "../output";
    if(!std::filesystem::exists(output_folder)){
      std::filesystem::create_directory(output_folder);
    }
    std::string output_path = output_folder+"/"+model+"_"+physic_model;
    if(!std::filesystem::exists(output_path)){
      std::filesystem::create_directory(output_path);
    }
    for(int ai=0; ai<Vn_list.size(); ai++)
    {
      Eigen::MatrixXd Vn = Vn_list[ai];
      std::string numstr = std::to_string(ai);
      int strlen = numstr.length();

      std::string seqstr;
      for(int k=0; k<(4-strlen); k++){
          seqstr.append("0");
      }
      seqstr.append(numstr);
      std::string save_path = output_path+"/"+model+"_"+physic_model+"_"+seqstr+".obj";
      igl::writeOBJ(save_path,Vn,F);
    }
  }

	igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.core().align_camera_center(V);
  viewer.core().is_animating = true;
  viewer.core().animation_max_fps = 24.;
  viewer.data().show_lines = true;
  viewer.data().show_overlay_depth = false;
  viewer.core().background_color=Eigen::Vector4f::Ones();
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &) -> bool
	{
		if(viewer.core().is_animating)
		{
      Eigen::MatrixXd Vn = Vn_list[frame];
      viewer.data().set_vertices(Vn);
      viewer.data().compute_normals();
			frame++;
      if(frame == Vn_list.size()){
        //viewer.core().is_animating = !viewer.core().is_animating;
        frame = 0;
      }
		}
		return false;
	};

  viewer.callback_key_down = [&](igl::opengl::glfw::Viewer &, unsigned int key, int mod)
  {
    if(key==' '){
      viewer.core().is_animating = !viewer.core().is_animating;
    }
    if(key=='e' || key=='E'){
      std::cout<<"export png seq"<<std::endl;
      viewer.core().is_animating = true;
      export_png_seq(viewer,Vn_list,json_path,physic_model);
    }
    return false;
  };
  viewer.launch();
}


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.

// 2. 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.

// 3. Neither the name of the copyright holder 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 HOLDER 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.

#include "rbr_sdp_solver.h"

#include <ceres/rotation.h>

#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

#include "bcm_sdp_solver.h"
#include "math/matrix_square_root.h"

namespace gopt {
namespace solver {

RBRSDPSolver::RBRSDPSolver(const size_t n, const size_t block_dim)
    : RBRSDPSolver(n, block_dim, solver::SDPSolverOptions()) {}

RBRSDPSolver::RBRSDPSolver(
    const size_t n, const size_t block_dim,
    const solver::SDPSolverOptions& options)
    : BCMSDPSolver(n, block_dim, options){
  X_ = Eigen::MatrixXd::Identity(dim_ * n, dim_ * n);
}

void RBRSDPSolver::Solve(solver::Summary& summary) {
  double prev_func_val = std::numeric_limits<double>::max();
  double cur_func_val = this->EvaluateFuncVal();
  double duration = 0.0;
  double error = 0.0;

  summary.begin_time = std::chrono::high_resolution_clock::now();
  while (summary.total_iterations_num < sdp_solver_options_.max_iterations) {
    if (sdp_solver_options_.verbose) {
      this->LogToStd(summary.total_iterations_num, prev_func_val, cur_func_val,
                     error, duration);
    }

    if (IsConverge(prev_func_val, cur_func_val, sdp_solver_options_.tolerance, &error)) {
      break;
    }

    // convergence rate? Take it for consideration.
    for (size_t k = 0; k < n_; k++) {
      // Eliminating the k-th row and column from Y to form Bk
      Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3 * (n_ - 1), 3 * (n_ - 1));
      this->ReformingB(k, B);

      // Eliminating the k-th column and all but the k-th row from R to form Wk
      Eigen::MatrixXd W = Eigen::MatrixXd::Zero(3 * (n_ - 1), 3);
      this->ReformingW(k, W);

      Eigen::MatrixXd B_multi_W = B * W;
      Eigen::MatrixXd WtBW = W.transpose() * B_multi_W;

      // FIXME: (chenyu) Solving matrix square root with
      // SVD and LDL^T would generate different result
      Eigen::MatrixXd WtBW_sqrt = MatrixSquareRoot(WtBW);
      // Eigen::MatrixXd WtBW_sqrt =
      // MatrixSquareRootForSemidefinitePositiveMat(WtBW);

      // FIXME: (chenyu) Eigen 3.3.0 is required for the use of
      // CompleteOrthogonalDecomposition<>
      // Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> cqr(WtBW_sqrt);
      // Eigen::Matrix3d moore_penrose_pseinv = cqr.pseudoInverse();
      Eigen::Matrix3d moore_penrose_pseinv = WtBW_sqrt.inverse();

      // compute S by fixing the error of Equ.(47) in Erikson's paper
      Eigen::MatrixXd S = -B_multi_W * moore_penrose_pseinv;

      // reordering X
      this->ReorderingUnknown(k, B, S);
    }

    summary.total_iterations_num++;
    duration = summary.Duration();

    // Update function value
    prev_func_val = cur_func_val;
    cur_func_val = this->EvaluateFuncVal();
  }

  summary.total_iterations_num++;
  if (sdp_solver_options_.verbose) {
    this->LogToStd(summary.total_iterations_num, prev_func_val, cur_func_val,
                   error, duration);
  }
}

double RBRSDPSolver::EvaluateFuncVal() const {
  return EvaluateFuncVal(X_);
}

double RBRSDPSolver::EvaluateFuncVal(const Eigen::MatrixXd& Y) const {
  return (Q_ * Y).trace();
}

void RBRSDPSolver::ReformingB(const size_t k, Eigen::MatrixXd& B) {
  size_t r = 0, c = 0;  // the row and column index of matrix B

  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;
    c = 0;
    for (size_t j = 0; j < n_; j++) {
      if (j == k) continue;

      B.block(3 * r, 3 * c, 3, 3) = X_.block(3 * i, 3 * j, 3, 3);
      c++;
    }
    r++;
  }
}

void RBRSDPSolver::ReformingW(const size_t k, Eigen::MatrixXd& W) {
  size_t r = 0;  // row index of matrix W

  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;

    W.block(3 * r, 0, 3, 3) = Q_.block(3 * i, 3 * k, 3, 3);
    r++;
  }
}

void RBRSDPSolver::ReorderingUnknown(const size_t k, const Eigen::MatrixXd& B,
                                     const Eigen::MatrixXd& S) {
  // Reordering X according to [Algorithm 1] in paper:
  // - Z. Wen, D. Goldfarb, S. Ma, and K. Scheinberg.
  //   Row by row methods for semidefinite programming.
  //   Technical report, Columbia University, 2009. 7

  // Update X(k, k)
  X_.block(3 * k, 3 * k, 3, 3) = Eigen::Matrix3d::Identity();

  // Update the k-th column of Y, except Y(k, k)
  size_t j = 0;  // the j-th block of S
  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;
    X_.block(3 * i, 3 * k, 3, 3) = S.block(3 * j, 0, 3, 3);
    j++;
  }

  // Update the k-th row of Y, except Y(k, k)
  size_t i = 0;  // the i-th block of S
  for (size_t j = 0; j < n_; j++) {
    if (j == k) continue;
    X_.block(3 * k, 3 * j, 3, 3) = S.block(3 * i, 0, 3, 3).transpose();
    i++;
  }
}

}  // namespace solver
}  // namespace gopt


#include "farm_ng/calibration/visual_odometer.h"

#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/video.hpp>

#include <Eigen/Geometry>

#include <ceres/ceres.h>

#include "farm_ng/blobstore.h"
#include "farm_ng/ipc.h"
#include "farm_ng/sophus_protobuf.h"

#include "farm_ng/calibration/camera_model.h"
#include "farm_ng/calibration/eigen_cv.h"
#include "farm_ng/calibration/kinematics.h"
#include "farm_ng/calibration/local_parameterization.h"

namespace farm_ng {
namespace {

void SavePly(std::string ply_path, const std::vector<Eigen::Vector3d>& points) {
  LOG(INFO) << ply_path << " npoints: " << points.size();
  std::ofstream out(ply_path);
  out << "ply\n";
  out << "format ascii 1.0\n";
  out << "element vertex " << points.size() << "\n";
  out << "property float x\n";
  out << "property float y\n";
  out << "property float z\n";
  out << "end_header\n";
  for (auto p : points) {
    out << float(p.x()) << " " << float(p.y()) << " " << float(p.z()) << "\n";
  }
  out.close();
}
struct ProjectionCostFunctor {
  ProjectionCostFunctor(const CameraModel& camera_model,
                        const Eigen::Vector2d& point_image)
      : camera_model_(camera_model), point_image_(point_image) {}

  template <class T>
  bool operator()(T const* const raw_camera_pose_world,
                  T const* const raw_point_world, T* raw_residuals) const {
    Eigen::Map<Sophus::SE3<T> const> const camera_pose_world(
        raw_camera_pose_world);

    Eigen::Map<Eigen::Matrix<T, 3, 1> const> const point_world(raw_point_world);
    Eigen::Map<Eigen::Matrix<T, 2, 1>> residuals(raw_residuals);
    residuals =
        ProjectPointToPixel<T>(camera_model_, camera_pose_world * point_world) -
        point_image_.cast<T>();
    return true;
  }
  const CameraModel& camera_model_;
  Eigen::Vector2d point_image_;
};

struct PoseCostFunctor {
  PoseCostFunctor(const Sophus::SE3d& camera_start_pose_camera_end)
      : camera_end_pose_camera_start_(camera_start_pose_camera_end.inverse()) {}

  template <class T>
  bool operator()(T const* const raw_camera_pose_world_start,
                  T const* const raw_camera_pose_world_end,
                  T* raw_residuals) const {
    Eigen::Map<Sophus::SE3<T> const> const camera_pose_world_start(
        raw_camera_pose_world_start);

    Eigen::Map<Sophus::SE3<T> const> const camera_pose_world_end(
        raw_camera_pose_world_end);

    auto camera_start_pose_camera_end_est =
        camera_pose_world_start * camera_pose_world_end.inverse();

    auto camera_end_pose_camera_end_est =
        camera_end_pose_camera_start_.cast<T>() *
        camera_start_pose_camera_end_est;

    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(raw_residuals);
    residuals = T(100) * camera_end_pose_camera_end_est.log();
    return true;
  }
  Sophus::SE3d camera_end_pose_camera_start_;
};
}  // namespace
VisualOdometer::VisualOdometer(const CameraModel& camera_model,
                               const BaseToCameraModel& base_to_camera_model,
                               size_t max_history)
    : camera_model_(camera_model),
      flow_(camera_model, max_history),
      base_to_camera_model_(base_to_camera_model),
      odometry_pose_base_(Sophus::SE3d::rotX(0.0)) {
  ProtoToSophus(base_to_camera_model_.base_pose_camera().a_pose_b(),
                &base_pose_camera_);
}

void VisualOdometer::AddWheelMeasurements(
    const BaseToCameraModel::WheelMeasurement& measurements) {
  wheel_measurements_.insert(measurements);
}

VisualOdometerResult VisualOdometer::AddImage(
    cv::Mat image, google::protobuf::Timestamp stamp) {
  VisualOdometerResult result;
  if (const FlowImage* prev_flow_image = flow_.PreviousFlowImage()) {
    auto wheel_measurements =
        wheel_measurements_.find_range(prev_flow_image->stamp, stamp);

    Sophus::SE3d base_pose_basep = TractorStartPoseTractorEnd(
        base_to_camera_model_.wheel_radius(),
        base_to_camera_model_.wheel_baseline(), wheel_measurements.first,
        wheel_measurements.second);
    Sophus::SE3d odometry_pose_base_wheel_only =
        (base_pose_camera_ * prev_flow_image->camera_pose_world).inverse() *
        base_pose_basep;

    odometry_pose_base_ = odometry_pose_base_wheel_only;
    if ((true || base_pose_basep.log().norm() > 0.001) &&
        !wheel_measurements_.empty()) {
      auto start = MakeTimestampNow();

      flow_.AddImage(image, stamp,
                     odometry_pose_base_wheel_only * base_pose_camera_, true);
      debug_image_ = flow_.GetDebugImage();
      auto after_flow = MakeTimestampNow();

      SolvePose(true);
      odometry_pose_base_ =
          (base_pose_camera_ * flow_.PreviousFlowImage()->camera_pose_world)
              .inverse();

      wheel_measurements_.RemoveBefore(flow_.EarliestFlowImage()->stamp);
      auto after_solve = MakeTimestampNow();
      LOG_EVERY_N(INFO, 100)
          << "VO took: "
          << google::protobuf::util::TimeUtil::DurationToMilliseconds(
                 after_solve - start)
          << " ms flow: "
          << google::protobuf::util::TimeUtil::DurationToMilliseconds(
                 after_flow - start)
          << " ms solve: "
          << google::protobuf::util::TimeUtil::DurationToMilliseconds(
                 after_solve - after_flow);

      if (false && flow_.LastImageId() % 100 == 0) {
        DumpFlowPointsWorld("/tmp/flow_points_world." +
                            std::to_string(flow_.LastImageId()) + ".ply");
      }
    }

  } else {
    flow_.AddImage(image, stamp, odometry_pose_base_ * base_pose_camera_, true);
    debug_image_ = flow_.GetDebugImage();
  }

  if (goal_image_id_) {
    // TODO(ethanrublee) this is goal pose stuff here is a HACK, should be moved
    // somewhere else, tracking camera can produce some generic poses which can
    // be used for a variety of control and path following modes.
    //
    // Here the base_pose_goal is the start of the planned path (parameterized
    // as the x+ axis of this frame). For convenience downstream, we'll project
    // the tractor's goal (nearest point on this path + some reasonable
    // distance) onto this path.
    Sophus::SE3d base_pose_goal = base_pose_camera_ * camera_pose_base_goal_;
    // The path is the parameterized line passing through the origin of goal
    // frame and along the X axis.
    // This line is in the base frame.
    auto path_base = Eigen::ParametrizedLine<double, 3>::Through(
        base_pose_goal.translation(),
        base_pose_goal * Eigen::Vector3d(1.0, 0, 0));

    // Project the origin of the base frame onto the line, this is the closest
    // point on the path. Here we transform the point into the goal's reference
    // frame. We expect this point to be non zero in X, and zero in y,z as it
    // lies on the X-axis of the goal reference frame.
    Eigen::Vector3d closest_path_point_goal =
        base_pose_goal.inverse() *
        path_base.projection(Eigen::Vector3d(0, 0, 0));
    CHECK_NEAR(closest_path_point_goal.y(), 0.0, 1e-6);
    CHECK_NEAR(closest_path_point_goal.z(), 0.0, 1e-6);

    // Here we add 3 meters to the nearest point on the line.  Think of this
    // like a carrot on a stick hanging 3 meters front of the robot on the
    // planned path line.
    // NOTE this distance will effect the move to goal controller behavior.  If
    // its large, then the heading corrections will be less and the tractor will
    // gradually correct to get onto the path.  If its too small, the tractor
    // will pivot dramatically trying to stay on the line.

    Sophus::SE3d base_pose_goal_carrot =
        base_pose_goal *
        Sophus::SE3d::transX(closest_path_point_goal.x() + 10.0);
    SophusToProto(base_pose_goal_carrot,
                  result.base_pose_goal.mutable_a_pose_b());
    if (!debug_image_.empty()) {
      cv::circle(debug_image_,
                 EigenToCvPoint(ProjectPointToPixel(
                     camera_model_, base_pose_camera_.inverse() *
                                        base_pose_goal_carrot.translation())),
                 10, cv::Scalar(255, 0, 0), 3);
    }
  } else {
    SophusToProto(Sophus::SE3d::rotZ(0.0),
                  result.base_pose_goal.mutable_a_pose_b());
  }
  result.base_pose_goal.mutable_a_pose_b()->mutable_stamp()->CopyFrom(stamp);
  result.base_pose_goal.set_frame_a("tractor/base");
  result.base_pose_goal.set_frame_b("goal");

  SophusToProto(odometry_pose_base_,
                result.odometry_vo_pose_base.mutable_a_pose_b());
  result.odometry_vo_pose_base.mutable_a_pose_b()->mutable_stamp()->CopyFrom(
      stamp);
  result.odometry_vo_pose_base.set_frame_a("odometry/vo");
  result.odometry_vo_pose_base.set_frame_b("tractor/base");

  return result;
}

void VisualOdometer::AddFlowBlockToProblem(ceres::Problem* problem,
                                           const FlowBlock& flow_block) {
  ceres::CostFunction* cost_function1 =
      new ceres::AutoDiffCostFunction<ProjectionCostFunctor, 2,
                                      Sophus::SE3d::num_parameters, 3>(
          new ProjectionCostFunctor(
              camera_model_,
              flow_block.flow_point_image.point_image.cast<double>()));

  problem->AddParameterBlock(flow_block.flow_point_world->point_world.data(),
                             3);
  problem->AddResidualBlock(cost_function1, new ceres::CauchyLoss(2),
                            flow_block.flow_image->camera_pose_world.data(),
                            flow_block.flow_point_world->point_world.data());
}

void VisualOdometer::AddFlowImageToProblem(FlowImage* flow_image,
                                           ceres::Problem* problem,
                                           FlowBlocks* flow_blocks) {
  problem->AddParameterBlock(flow_image->camera_pose_world.data(),
                             Sophus::SE3d::num_parameters,
                             new LocalParameterizationSE3);

  for (const auto& id_flow_point : flow_image->flow_points) {
    const FlowPointImage& flow_point = id_flow_point.second;

    FlowPointWorld* flow_point_world =
        flow_.MutableFlowPointWorld(flow_point.id);
    if (flow_point_world->image_ids.size() < 5) {
      continue;
    }
    if (true) {  // flow_blocks->size() < 100 ||
                 // flow_blocks->count(flow_point_world->id)) {
      FlowBlock flow_block({flow_image, flow_point_world, flow_point});
      (*flow_blocks)[flow_point_world->id].push_back(flow_block);
      AddFlowBlockToProblem(problem, flow_block);
    }
  }
}
void VisualOdometer::SolvePose(bool debug) {
  ceres::Problem problem;

  std::set<uint64_t> flow_image_ids;
  FlowBlocks flow_blocks;
  {
    uint64_t image_id(flow_.LastImageId());
    if (image_id < 5) {
      return;
    }
    uint64_t begin_id =
        std::max(int(flow_.EarliestFlowImage()->id), int(image_id) - 50);
    flow_image_ids.insert(image_id);
    if (goal_image_id_) {
      if (int(image_id) - int(*goal_image_id_) < 50) {
        flow_image_ids.insert(*goal_image_id_);
      }
    }
    uint64_t skip = std::max(1, (int(image_id) - int(begin_id)) / 5);
    while (begin_id < image_id) {
      flow_image_ids.insert(begin_id);
      begin_id += skip;
    }
  }
  // debugging which images are used.
  if (false) {
    std::stringstream ss;
    for (auto id : flow_image_ids) {
      ss << " " << id;
    }
    LOG(INFO) << "Num images: " << flow_image_ids.size() << ss.str();
  }

  for (auto image_id : flow_image_ids) {
    FlowImage* flow_image = flow_.MutableFlowImage(image_id);
    AddFlowImageToProblem(flow_image, &problem, &flow_blocks);
  }

  for (auto start = flow_image_ids.begin(), end = ++flow_image_ids.begin();
       end != flow_image_ids.end(); ++start, ++end) {
    FlowImage* flow_image_start = flow_.MutableFlowImage(*start);
    FlowImage* flow_image_end = flow_.MutableFlowImage(*end);

    auto wheel_measurements = wheel_measurements_.find_range(
        flow_image_start->stamp, flow_image_end->stamp);
    VLOG(2) << std::distance(wheel_measurements.first,
                             wheel_measurements.second)
            << " Wheel measurements";

    Sophus::SE3d base_start_pose_base_end = TractorStartPoseTractorEnd(
        base_to_camera_model_.wheel_radius(),
        base_to_camera_model_.wheel_baseline(), wheel_measurements.first,
        wheel_measurements.second);

    Sophus::SE3d camera_start_pose_camera_end = base_pose_camera_.inverse() *
                                                base_start_pose_base_end *
                                                base_pose_camera_;
    ceres::CostFunction* cost_function1 =
        new ceres::AutoDiffCostFunction<PoseCostFunctor, 6,
                                        Sophus::SE3d::num_parameters,
                                        Sophus::SE3d::num_parameters>(
            new PoseCostFunctor(camera_start_pose_camera_end));

    problem.AddResidualBlock(cost_function1, new ceres::CauchyLoss(1.0),
                             flow_image_start->camera_pose_world.data(),
                             flow_image_end->camera_pose_world.data());
  }

  if (false && goal_image_id_ && flow_image_ids.count(*goal_image_id_)) {
    problem.SetParameterBlockConstant(
        flow_.MutableFlowImage(*goal_image_id_)->camera_pose_world.data());
  }

  // Set solver options (precision / method)
  ceres::Solver::Options options;
  // options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.gradient_tolerance = 1e-4;
  options.function_tolerance = 1e-4;
  options.parameter_tolerance = 1e-4;
  // options.num_threads = 1;
  options.max_num_iterations = 30;

  // Solve
  ceres::Solver::Summary summary;
  //  options.logging_type = ceres::PER_MINIMIZER_ITERATION;
  options.minimizer_progress_to_stdout = false;
  ceres::Solve(options, &problem, &summary);
  LOG_EVERY_N(INFO, 100) << summary.BriefReport();
  if (!summary.IsSolutionUsable()) {
    LOG(INFO) << summary.FullReport();
  }

  double all_rmse = 0;
  double all_n = 0;
  for (auto id_blocks : flow_blocks) {
    uint64_t point_world_id = id_blocks.first;
    double rmse = 0;
    double n = 0;
    for (auto block : id_blocks.second) {
      Eigen::Vector3d point_camera = block.flow_image->camera_pose_world *
                                     block.flow_point_world->point_world;
      Eigen::Vector2d point_image_proj =
          ProjectPointToPixel(camera_model_, point_camera);

      if (point_camera.z() < 0) {
        flow_.RemoveBlock(block);
        continue;
      }

      if (point_camera.norm() > 5e2) {
        flow_.RemoveBlock(block);
        continue;
      }
      double err2 =
          (point_image_proj - block.flow_point_image.point_image.cast<double>())
              .squaredNorm();
      if (err2 > (4 * 4)) {
        flow_.RemoveBlock(block);
      } else {
        rmse += err2;
        n += 1;
      }
    }
    auto flow_point_world = flow_.MutableFlowPointWorld(point_world_id);
    if (flow_point_world->image_ids.size() < 5 ||
        flow_point_world->point_world.norm() > 1e3) {
      flow_.RemovePointWorld(point_world_id);
    } else {
      flow_.MutableFlowPointWorld(point_world_id)->rmse = std::sqrt(rmse / n);
      all_rmse += rmse;
      all_n += n;
    }
  }
  LOG_EVERY_N(INFO, 100) << "RMSE: "
                         << std::sqrt(all_rmse / std::max(all_n, 1.0))
                         << " N: " << all_n;

  FlowImage* flow_image = flow_.MutablePreviousFlowImage();

  if (goal_image_id_) {
    auto base_pose_world_goal =
        camera_pose_base_goal_.inverse() *
        flow_.MutableFlowImage(*goal_image_id_)->camera_pose_world;

    auto camera_pose_base_goal =
        flow_image->camera_pose_world * base_pose_world_goal.inverse();

    goal_image_id_ = flow_image->id;
    camera_pose_base_goal_ = camera_pose_base_goal;
  }

  if (debug) {
    cv::Mat reprojection_image = debug_image_;
    if (reprojection_image.empty()) {
      cv::cvtColor(*(flow_image->image), reprojection_image,
                   cv::COLOR_GRAY2BGR);
    }

    for (const auto& id_flow_point : flow_image->flow_points) {
      const auto& flow_point = id_flow_point.second;
      FlowPointWorld* flow_point_world =
          flow_.MutableFlowPointWorld(flow_point.id);
      if (flow_point_world->rmse == 0.0) {
        continue;
      }
      Eigen::Vector2d point_image_proj =
          ProjectPointToPixel(camera_model_, flow_image->camera_pose_world *
                                                 flow_point_world->point_world);
      cv::line(reprojection_image, EigenToCvPoint(flow_point.point_image),
               EigenToCvPoint(point_image_proj), flow_.Color(flow_point.id));

      cv::circle(reprojection_image, EigenToCvPoint(point_image_proj), 2,
                 flow_.Color(flow_point.id), -1);
      cv::circle(reprojection_image, EigenToCvPoint(flow_point.point_image), 5,
                 flow_.Color(flow_point.id));
    }
    if (goal_image_id_) {
      for (int i = 0; i < 1000; ++i) {
        Eigen::Vector3d p1 =
            camera_pose_base_goal_ * Eigen::Vector3d(0.1 * i, 0.0, 0.0);
        Eigen::Vector3d p2 =
            camera_pose_base_goal_ * Eigen::Vector3d(0.1 * (i + 1), 0.0, 0.0);
        if (p1.z() > 0.0 && p2.z() > 0.00) {
          cv::line(reprojection_image,
                   EigenToCvPoint(ProjectPointToPixel(camera_model_, p1)),
                   EigenToCvPoint(ProjectPointToPixel(camera_model_, p2)),
                   cv::Scalar(0, 255, 0), 3);
        }
      }
    }
    auto camera_pose_base = base_pose_camera_.inverse();
    for (int i = 0; i < 1000; ++i) {
      Eigen::Vector3d ap1 =
          camera_pose_base * Eigen::Vector3d(0.1 * i, 0.0, 0.0);
      Eigen::Vector3d ap2 =
          camera_pose_base * Eigen::Vector3d(0.1 * (i + 1), 0.0, 0.0);
      cv::line(reprojection_image,
               EigenToCvPoint(ProjectPointToPixel(camera_model_, ap1)),
               EigenToCvPoint(ProjectPointToPixel(camera_model_, ap2)),
               cv::Scalar(0, 0, 255), 1);
    }
    debug_image_ = reprojection_image;
  }
}

void VisualOdometer::DumpFlowPointsWorld(std::string ply_path) {
  std::vector<Eigen::Vector3d> points;
  for (auto it : flow_.FlowPointsWorld()) {
    if (it.second.image_ids.size() >= 5) {
      points.push_back(it.second.point_world);
    }
  }
  SavePly(ply_path, points);
}

void VisualOdometer::SetGoal() {
  if (flow_.LastImageId() < 10) {
    return;
  }
  goal_image_id_ = flow_.LastImageId();
  camera_pose_base_goal_ = base_pose_camera_.inverse();
}

void VisualOdometer::AdjustGoalAngle(double theta) {
  if (!goal_image_id_) {
    return;
  }
  camera_pose_base_goal_ = camera_pose_base_goal_ * Sophus::SE3d::rotZ(theta);
}

}  // namespace farm_ng


// Copyright (c) 2014 The Pebblecoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <cassert>

#include <boost/multiprecision/cpp_int.hpp>

#include "contract_grading.h"

namespace cryptonote
{
  uint64_t grade_amount(uint64_t full_amount, uint32_t grade, uint32_t fee_scale)
  {
    assert(grade <= GRADE_SCALE_MAX);
    assert(fee_scale <= GRADE_SCALE_MAX);

    // amount_left will be rounded down
    boost::multiprecision::uint128_t graded_amount = full_amount;
    graded_amount *= grade;
    graded_amount /= GRADE_SCALE_MAX;
    assert(graded_amount == (uint64_t)graded_amount);

    // calculate fee
    boost::multiprecision::uint128_t fee = graded_amount;
    fee *= fee_scale;
    fee /= GRADE_SCALE_MAX;
    assert(fee == (uint64_t)fee);
    // round fee taken up to prevent coins from being created via fee rounding
    while (fee_scale > 0 && fee * GRADE_SCALE_MAX / fee_scale < graded_amount)
    {
      fee += 1;
    }
    assert(fee <= graded_amount);

    boost::multiprecision::uint128_t amount_left = graded_amount - fee;
    assert(amount_left == amount_left.convert_to<uint64_t>());
    return amount_left.convert_to<uint64_t>();
  }

  uint64_t grade_contract_amount(uint64_t contract_amount, uint32_t grade, uint32_t fee_scale)
  {
    // coins resolve to the grade
    return grade_amount(contract_amount, grade, fee_scale);
  }

  uint64_t grade_backing_amount(uint64_t locked_amount, uint32_t grade, uint32_t fee_scale)
  {
    // the backing coins resolve to the other side of the contract
    assert(grade <= GRADE_SCALE_MAX);
    return grade_amount(locked_amount, GRADE_SCALE_MAX - grade, fee_scale);
  }

  uint64_t calculate_total_fee(uint64_t total_contract_coins, uint32_t fee_scale)
  {
    assert(fee_scale <= GRADE_SCALE_MAX);

    // round fee reward down
    boost::multiprecision::uint128_t fee = total_contract_coins;
    fee *= fee_scale;
    fee /= GRADE_SCALE_MAX;
    assert(fee == fee.convert_to<uint64_t>());

    return fee.convert_to<uint64_t>();
  }
}


#ifndef __COMMON_HPP__
#define __COMMON_HPP__

/******************************
      Author: Joel Veness
        Date: 2011
******************************/

#include <vector>
#include <cmath>

// boost includes
#include <boost/cstdint.hpp>
#include <boost/dynamic_bitset.hpp>

#include "Neighbours.hpp"

// define a bit type
typedef uint8_t bit_t;

// stores symbol occurrence counts
typedef float count_t;

// holds context weights
typedef double weight_t;

// describe a binary context
typedef std::vector<bit_t> context_t;

// describe a binary history
typedef Neighbours history_t;

/* given log(x) and log(y), compute log(x+y). uses the following identity:
   log(x + y) = log(x) + log(1 + y/x) = log(x) + log(1+exp(log(y)-log(x)))*/
inline double logAdd(double log_x, double log_y) {

    // ensure log_y >= log_x, can save some expensive log/exp calls
    if (log_x > log_y) {
        double t = log_x; log_x = log_y; log_y = t;
    }

    // only replace log(1+exp(log(y)-log(x))) with log(y)-log(x)
    // if the the difference is small enough to be meaningful
    double tmp = log_y - log_x;

    if (tmp < 100.0) {
        return std::log(1.0 + std::exp(tmp)) + log_x;
    } else {
        return log_y;
    }
}

// compressor interface
class Compressor {

    public:

        virtual ~Compressor() {}

        // the probability of seeing a particular symbol next
        virtual double prob(bit_t b) = 0;

        // the logarithm of the probability of all processed experience
        virtual double logBlockProbability() const = 0;

        // process a new piece of sensory experience
        virtual void update(bit_t b) = 0;

        // file extension
        virtual const char *fileExtension() const = 0;
};

#endif // __COMMON_HPP__


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_3DCF6B90AE0E11DE9A315BE555D89593
#define UUID_3DCF6B90AE0E11DE9A315BE555D89593

#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits_array.hpp>
#include <boost/qvm/static_assert.hpp>

namespace boost {
namespace qvm {
namespace qvm_detail {
template <int N> struct det_size {};

template <class M>
BOOST_QVM_INLINE_TRIVIAL typename mat_traits<M>::scalar_type
determinant_impl_(M const &a, det_size<2>) {
  return mat_traits<M>::template read_element<0, 0>(a) *
             mat_traits<M>::template read_element<1, 1>(a) -
         mat_traits<M>::template read_element<1, 0>(a) *
             mat_traits<M>::template read_element<0, 1>(a);
}

template <class M, int N>
BOOST_QVM_INLINE_RECURSION typename mat_traits<M>::scalar_type
determinant_impl_(M const &a, det_size<N>) {
  typedef typename mat_traits<M>::scalar_type T;
  T m[N - 1][N - 1];
  T det = T(0);
  for (int j1 = 0; j1 != N; ++j1) {
    for (int i = 1; i != N; ++i) {
      int j2 = 0;
      for (int j = 0; j != N; ++j) {
        if (j == j1)
          continue;
        m[i - 1][j2] = mat_traits<M>::read_element_idx(i, j, a);
        ++j2;
      }
    }
    T d = determinant_impl_(m, det_size<N - 1>());
    if (j1 & 1)
      d = -d;
    det += mat_traits<M>::read_element_idx(0, j1, a) * d;
  }
  return det;
}

template <class M>
BOOST_QVM_INLINE_TRIVIAL typename mat_traits<M>::scalar_type
determinant_impl(M const &a) {
  BOOST_QVM_STATIC_ASSERT(mat_traits<M>::rows == mat_traits<M>::cols);
  return determinant_impl_(a, det_size<mat_traits<M>::rows>());
}
} // namespace qvm_detail
} // namespace qvm
} // namespace boost

#endif


/*
        Program is testing out a prototype for
        game tree solvers. Tick Tack Toe is
        especially nice as it's super simple,
        but has some interesting characteristsics
           - Game tree has many nodes
             which are invarant, ie one
             node can be represented by
             a rotation or mirror of
             another node, which means
             can have a system which "merges"
             nodes
           - We know a-prori that their is
             a solved solution, which we
             can visually check
           - We can create the static game
             tree, not need to worry about
             the complexity

        One thing to consider with these sorts
        or designs, is that we done want to
        have redendant data,

        <start> -- (0,0) -- (1,1) -- ...
                \- (1,0)
                ...
                \- (3,3) -- (1,1)

        Once we have this static game tree, we
        can then work backwards to figure
        out of an optmial strategy

 */

#include <iostream>
#include <tuple>
#include <map>
#include <cassert>
#include <sstream>
#include <random>
#include <algorithm>
#include <vector>
#include <set>
#include <bitset>
#include <boost/lexical_cast.hpp>

#define PRINT(X) do{ std::cout << #X << " = " << ( X ) << "\n"; }while(0)

/*
        Hero villian is arbitary,
        just convention, with Hero
        begin player who does the first
        move
 */
enum Player{
        Player_NotAPlayer,
        Player_Hero,         // first
        Player_Villian       // second
};
inline char PlayerToken(Player p){
        switch (p) {
        case Player_NotAPlayer:
                return ' ';
        case Player_Hero:
                return 'X';
        case Player_Villian:
                return 'O';
        }
}

template<class To, class From>
inline To Cast(From ptr){
        //PRINT( ptr->GetType() );
        //PRINT( std::remove_pointer_t<To>::__type__() );
        if( ptr->GetType() == std::remove_pointer_t<To>::__type__())
                return reinterpret_cast<To>(ptr);
        std::stringstream fmt;
        fmt << "casting " << ptr->GetType() << " to " << std::remove_pointer_t<To>::__type__();
        throw std::domain_error(fmt.str());
}

enum Eval{
        Eval_Win   = 0x01,
        Eval_Draw  = 0x02,
        Eval_Lose  = 0x04,
};
std::string EvalToString(Eval e) {
        switch (e) {
        case Eval_Win:
                return "Eval_Win";
        case Eval_Draw:
                return "Eval_Draw";
        case Eval_Lose:
                return "Eval_Lose";
        }
}

namespace Detail{
        struct PictureBox{
                PictureBox(){
                        lines_.emplace_back();
                }
                void NewLine(){
                        lines_.emplace_back();
                }
                void Append(std::string const& s){
                        lines_.back() += s;
                }
                void Append(char c){
                        lines_.back() += c;
                }
                void Display(){
                        for( size_t i=lines_.size();i!=0;){
                                --i;
                                std::cout << lines_[i] << "\n";
                        }
                }
                PictureBox& operator+=(PictureBox const& that){
                       for(; lines_.size() < that.lines_.size();)
                              NewLine();
                       for(size_t i=0;i!=std::min(lines_.size(), that.lines_.size());++i){
                               lines_[i] += that.lines_[i];
                       }
                       return *this;
                }
                static PictureBox Make(std::string const& s){
                        PictureBox result;
                        for( char c : s){
                                switch(c){
                                case '\n':
                                        result.NewLine();
                                        break;
                                default:
                                        result.Append(c);
                                        break;
                                }
                        }
                        return std::move(result);
                }
        private:
                std::vector<std::string> lines_;
        };
} // Detail

struct Board{
        /*
                For each pair of bits, we have

                +----+-----------+
                |Mask|    Data   |
                +----+-----------+
                | 00 | NotAPlayer|
                | 01 |  Hero     |
                | 10 | Villian   |
                +----+-----------+
         */
        Player GetTile(int x, int y)const{
                auto offset = Map_(x,y);
                return static_cast<Player>(( mask_ & ( 0x3 << offset ) ) >> offset);
        }
        void SetTile(int x, int y, Player p){
                assert( GetTile(x,y) == 0 && "Tile already set");
                auto offset = Map_(x,y);
                mask_ |= ( p << offset );
                assert( GetTile(x,y) == p  && "post condition failed");
        }

        friend std::ostream& operator<<(std::ostream& ostr, Board const& board){
                static const char* lineBreak = "+---+---+---+";
                for(int y=3;y!=0;){
                        --y;

                        if( y == 2 ){
                                ostr << lineBreak << "\n";
                        }

                        for(int x=0;x!=3;++x){
                                ostr << "| " << PlayerToken(board.GetTile(x,y)) << " ";
                                if( x == 2 )
                                        ostr << "|\n";
                        }
                        ostr << lineBreak;
                        if( y != 0 )
                                ostr << "\n";
                }
                return ostr;
        }
        std::string ToString()const{
                std::stringstream sstr;
                sstr << *this;
                return sstr.str();
        }
        auto GetMask()const{ return mask_; }

        auto GetInvariant()const{
                /*
                        a b c
                        d e f
                        g h i
                 */
                auto a = 0;
                auto b = 1;
                auto c = 2;
                auto d = 3;
                auto e = 4;
                auto f = 5;
                auto g = 6;
                auto h = 7;
                auto i = 8;

                std::vector<std::vector<int> > perms = {
                        /*
                        Normal Case (Rotate 0)
                         */
                        { a, b, c,
                          d, e, f,
                          g, h, i },
                        /*
                        Rotate 90
                         */
                        {  g, d, a,
                           h, e, b,
                           i, f, c },

                        /*
                        Rotate 180
                         */
                        {  i, h, g,
                           f, e, d,
                           c, b, a },
                        /*
                        Rotate 270
                         */
                        {  c, f, i,
                           b, e, h,
                           a, d, g },
                        /*
                        Mirror Above
                         */
                        {  g, h, i,
                           d, e, f,
                           a, b, c },

                        /*
                        Mirror Center
                         */
                        {  c, b, a,
                           f, e, d,
                           i, h, g }
                };

                std::vector<std::uint32_t> masks;
                for( auto const& p : perms ){
                        static std::vector<std::pair<int, int> > cord = {
                                { 0, 0},
                                { 1, 0},
                                { 2, 0},
                                { 0, 1},
                                { 1, 1},
                                { 2, 1},
                                { 0, 2},
                                { 1, 2},
                                { 2, 2}
                        };
                        Board aux;
                        for(size_t idx=0;idx!=9;++idx){
                                auto tok = GetTile(cord[idx].first, cord[idx].second);
                                aux.SetTile(cord[p[idx]].first, cord[p[idx]].second, tok);
                        }
                        masks.push_back(aux.GetMask());
                }
                return *std::min_element(masks.begin(), masks.end());
        }
private:
        static int Map_(int x, int y){
                return 2 * ( x * 3 + y );
        }
        std::uint32_t mask_ = 0;
};

/*
        Only one thing in the context, whos turn is it
        Obviously the Board is part of the context,
        but we have 2-tuples
                (B,C),
        to represent this
*/

enum GameCtrl{
        GameCtrl_Continue,
        GameCtrl_HeroWins,
        GameCtrl_VillianWins,
        GameCtrl_Draw
};
const char* GameCtrlToString(GameCtrl e) {
        switch (e) {
        case GameCtrl_Continue:
                return "GameCtrl_Continue";
        case GameCtrl_HeroWins:
                return "GameCtrl_HeroWins";
        case GameCtrl_VillianWins:
                return "GameCtrl_VillianWins";
        case GameCtrl_Draw:
                return "GameCtrl_Draw";
        }
}

struct GameContext{
        explicit GameContext(Board board = Board{}):board_{std::move(board)}{}
        explicit GameContext(Player active):active_{active}{}
        auto const& GetBoard()const{ return board_; }
        auto& GetBoard(){ return board_; }
        auto ActivePlayer()const{ return active_; }
        char ActivePlayerToken()const{
                return ( ActivePlayer() == 0 ? 'x' : 'o' );
        }
        void NextPlayer(){
                switch(active_){
                case Player_Hero:
                        active_ = Player_Villian;
                        break;
                case Player_Villian:
                        active_ = Player_Hero;
                        break;
                // stop compiler warning
                case Player_NotAPlayer:
                        break;
                }
        }
        auto GetCtrl()const{ return ctrl_; }
        auto SetCtrl(GameCtrl ctrl){ ctrl_ = ctrl; }
        friend bool operator<(GameContext const& lp, GameContext const& rp){
                #if 1
                return std::make_tuple(lp.board_.GetInvariant(), lp.ctrl_, lp.active_ )
                     < std::make_tuple(rp.board_.GetInvariant(), rp.ctrl_, rp.active_ );
                #else
                return std::make_tuple(lp.board_.GetMask(), lp.ctrl_, lp.active_ )
                     < std::make_tuple(rp.board_.GetMask(), rp.ctrl_, rp.active_ );
                #endif
        }
private:
        Board board_;
        GameCtrl ctrl_{ GameCtrl_Continue };
        Player active_{ Player_Hero };
};

struct TickTackToeLogic{
        GameContext Next(GameContext ctx, size_t x, size_t y){
                assert( ctx.GetCtrl()  == GameCtrl_Continue && "precondition failed");
                assert( ctx.GetBoard().GetTile(x,y)    == 0          && "precondition failed");

                ctx.GetBoard().SetTile(x,y, ctx.ActivePlayer() );

                // First need to check to see if someone has made a line
                auto triple = [&](auto x0, auto y0,
                                 auto x1, auto y1,
                                 auto x2, auto y2){
                        auto tok = ctx.GetBoard().GetTile(x0, y0);
                        if( tok == Player_NotAPlayer )
                                return false;
                        return
                                tok == ctx.GetBoard().GetTile(x1,y1) &&
                                tok == ctx.GetBoard().GetTile(x2,y2);
                };

                static std::vector< std::vector<size_t> > protos =
                {
                        { 0, 0, 0, 1, 0, 2},
                        { 1, 0, 1, 1, 1, 2},
                        { 2, 0, 2, 1, 2, 2},
                        { 0, 0, 1, 0, 2, 0},
                        { 0, 1, 1, 1, 2, 1},
                        { 0, 2, 1, 2, 2, 2},
                        { 0, 0, 1, 1, 2, 2},
                        { 0, 2, 1, 1, 2, 0}
                };

                for( auto const& p : protos){
                        if( triple( p[0], p[1], p[2], p[3], p[4], p[5]) ){
                                if( ctx.GetBoard().GetTile(p[0], p[1]) == Player_Hero){
                                        ctx.SetCtrl( GameCtrl_HeroWins );
                                } else {
                                        ctx.SetCtrl( GameCtrl_VillianWins );
                                }
                                return std::move(ctx);
                        }
                }

                // If we get here, their isn't a winner
                auto is_full = [&](){
                        for( size_t i=0;i!=3;++i){
                                for( size_t j=0;j!=3;++j){
                                        if( ctx.GetBoard().GetTile(i,j) == 0 )
                                                return false;

                                }
                        }
                        return true;
                };

                if( is_full() )
                        ctx.SetCtrl( GameCtrl_Draw );
                else
                        ctx.NextPlayer();
                return std::move(ctx);
        }
};

struct Node{
        enum Type{
                Type_Choice,
                Type_Payoff
        };
public:
        Type GetType()const{ return type_; }
        GameContext const& GetContext()const{ return ctx_; }
protected:
        explicit Node(Type type, GameContext ctx):
                type_{type},
                ctx_{std::move(ctx)}
        {}
private:
        Type type_;
        GameContext ctx_;
};

struct ChoiceNode : Node{
        explicit ChoiceNode(GameContext ctx):
                Node{Type_Choice, std::move(ctx)}
        {}
        // only when Choice
        void RegisterChild(Node* ptr){
                next_.push_back(ptr);
        }
        auto NumChildren()const{
                return next_.size();
        }
        auto begin()const{ return next_.begin(); }
        auto end()const{ return next_.end(); }
        static Type __type__(){ return Type_Choice; }
private:
        std::vector<Node*> next_;
};

struct PayoffNode : Node{
        explicit PayoffNode(GameContext ctx, Eval eval):
                Node{Type_Payoff, std::move(ctx)}
                ,eval_{eval}
        {}
        auto GetPayoff()const{ return eval_; }
        static Type __type__(){ return Type_Payoff; }
private:
        Eval eval_;
};

// TODO, rather than create a new graph, use this
struct NodeReference{
private:
        Node* ptr_;
        std::vector<Node*> next_;
};

struct GameTree{
        GameTree(Node* root):root_{root}{
                Register(root);
        }
        void AppendTerminal(Node* ptr){
                terminals_.push_back(ptr);
        }
        auto GetRoot()const{
                return root_;
        }
        auto const& GetTerminals()const{ return terminals_; }
        void Register(Node* ptr){
                world_.emplace(ptr->GetContext(), ptr);
                aux_.push_back(ptr);
        }
        Node const* Lookup(GameContext const& ctx)const{
                auto iter = world_.find(ctx);
                if( iter == world_.end())
                        return nullptr;
                return iter->second;
        }
        auto begin()const{ return aux_.begin(); }
        auto end()const{ return aux_.end(); }
private:
        Node* root_;
        std::vector<Node*> terminals_;
        std::map< GameContext, Node* > world_;
        std::vector<Node*> aux_;
};

/*
        Idea here, is that when creating a board
        which is invariant to anoher already
        created, we can return the reference
 */
struct NodeFactory{
        std::pair<bool, Node*> Make(GameContext ctx){
                auto iter = world_.find(ctx);
                if( iter != world_.end() )
                        return std::make_pair(false, iter->second);

                Node* ptr = nullptr;
                switch(ctx.GetCtrl()){
                case GameCtrl_Continue:
                        ptr = new ChoiceNode{ctx};
                        break;
                case GameCtrl_HeroWins:
                        ptr = new PayoffNode{ctx, Eval_Win};
                        break;
                case GameCtrl_VillianWins:
                        ptr = new PayoffNode{ctx, Eval_Lose};
                        break;
                case GameCtrl_Draw:
                        ptr = new PayoffNode{ctx, Eval_Draw};
                        break;
                }
                world_.emplace(ctx, ptr);
                return std::make_pair(true, ptr);
        }

private:
        std::map< GameContext, Node* > world_;
};

/*
        this is used to abstrauct the construction of
        the game tree
*/
struct GameTreeBuilder{
        void Generate(GameTree& tree, ChoiceNode* parent, GameContext ctx){
                static TickTackToeLogic logic;
                for( int x=0;x!=3;++x){
                        for( int y=0;y!=3;++y){
                                if( ctx.GetBoard().GetTile(x,y) == 0 ){

                                        auto nextCtx = logic.Next(ctx, x,y);

                                        auto makeRet = fac_.Make(nextCtx);
                                        auto ptr = makeRet.second;
                                        if( makeRet.first){
                                                // New node

                                                // First time here
                                                tree.Register(ptr);

                                                switch(nextCtx.GetCtrl()){
                                                case GameCtrl_Continue:
                                                        Generate(tree, Cast<ChoiceNode*>(ptr), nextCtx);
                                                        break;
                                                case GameCtrl_HeroWins:
                                                case GameCtrl_VillianWins:
                                                case GameCtrl_Draw:
                                                        tree.AppendTerminal(ptr);
                                                        break;
                                                }
                                        }
                                        parent->RegisterChild(ptr);
                                }
                        }
                }
        }
        GameTree Make(GameContext ctx){
                auto ptr = new ChoiceNode{ctx};
                GameTree tree{ptr};
                // Assume board isn't finished
                Generate(tree, ptr, ctx);
                return std::move(tree);
        }
private:
        NodeFactory fac_;
};

void test0(){
        GameContext ctx;
        Board board;
        TickTackToeLogic logic;

        std::vector< std::tuple<int, int> > todo = {
                { 0, 0 },
                { 1, 1 },
                { 0, 1 },
                { 0, 2 },
                { 1, 2 },
                { 2, 0 }
        };

        for( auto const& m : todo ){
                ctx = logic.Next(ctx, std::get<0>(m), std::get<1>(m) );
                PRINT(GameCtrlToString(ctx.GetCtrl()));
                //std::cout << board;
        }

}

void DisplayImpl(std::vector<Node*> history){
        if( history.size() == 0 )
                return;
        auto target = history.back();
        size_t indent_width = ( history.size() - 1 ) * 17;
        Detail::PictureBox indent;
        for(size_t i=7;i!=0;){
                --i;
                indent.Append(std::string(indent_width, ' ') );
                if( i != 0 )
                        indent.NewLine();
        }

        if( history.back()->GetType() == Node::Type_Payoff ){
                static auto spacer = [](){
                        Detail::PictureBox proto;
                        proto.Append("    ");
                        proto.NewLine();
                        proto.Append("    ");
                        proto.NewLine();
                        proto.Append("    ");
                        proto.NewLine();
                        proto.Append(" => ");
                        proto.NewLine();
                        proto.Append("    ");
                        proto.NewLine();
                        proto.Append("    ");
                        proto.NewLine();
                        proto.Append("    ");
                        return std::move(proto);
                }();

                Detail::PictureBox picture;
                #if 0
                picture += indent;
                picture += Detail::PictureBox::Make( history.back()->GetContext().GetBoard().ToString() );
                #endif
                for( auto ptr : history){
                        picture += Detail::PictureBox::Make( ptr->GetContext().GetBoard().ToString() );
                        picture += spacer;

                }
                picture += spacer;

                Detail::PictureBox leaf;
                leaf.NewLine();
                leaf.NewLine();
                leaf.NewLine();
                leaf.Append("Payoff " + boost::lexical_cast<std::string>(Cast<PayoffNode*>(target)->GetPayoff()));

                picture += leaf;
                picture.Display();

        } else {
                #if 0
                Detail::PictureBox picture;
                picture += indent;
                picture += Detail::PictureBox::Make( history.back()->GetContext().GetBoard().ToString() );
                picture.Display();
                #endif
                for( auto ptr : *Cast<ChoiceNode*>(target)){
                        history.push_back(ptr);
                        DisplayImpl(history);
                        history.pop_back();

                }
        }

}
void Display(Node* ptr){
        std::vector<Node*> history = {ptr};
        DisplayImpl(history);
}

/*
        Need to iterate the game tree, and evaluate each node.
        For nodes where it's the player, need to decide
        which is the best move by backwards induction

        From a payoff node, We find the most recent move the
        player made, and mark is a Never.

        Idea here is that for every move we do, we try to
        take the move with is gurenteed a win, else we take
        the move with a either a win or draw (depending on opp),
        then we take move with is only draw, then we take move
        which is win or lose, then take move with is win lose draw

        Note that in the game theory sense of a strategy, we
        need a move for every possible board, so this includeds
        taking a move we know will lose, even though we won't get
        their following out path.

        On nodes where it's the opponents move, we basically
        assume that he is going to take the optimum move, if their
        is a move we can take, which results in a node with
 */

struct EvalMetric{
        struct Result{
                unsigned Mask()const{ return mask_; }
                auto begin()const{ return next_.begin(); }
                auto end()const{ return next_.end(); }
        private:
                friend struct EvalMetric;
                unsigned mask_;
                std::vector<Node const*> next_;
        };
        static std::unique_ptr<EvalMetric> MakeForHero(GameTree const& tree){
                auto ptr = std::make_unique<EvalMetric>();
                ptr->Populate_(Player_Hero, tree.GetRoot());
                return std::move(ptr);
        }
        auto const& operator()(GameContext const& ctx)const{
                auto iter = eval_.find(ctx);
                if( iter == eval_.end()){
                        throw std::domain_error("not in tree");
                }
                return iter->second;
        }
private:

        unsigned Populate_(Player p, Node const* node){
                auto ctx = node->GetContext();
                if( eval_.count(ctx) != 0 )
                        return eval_[ctx].mask_;

                if( node->GetType() == Node::Type_Payoff ){
                        static std::map<Eval,Eval> invMap = {{Eval_Lose, Eval_Win}, {Eval_Draw, Eval_Draw}, {Eval_Win, Eval_Lose}};
                        auto payoff = Cast<PayoffNode const*>(node)->GetPayoff();
                        if( p  == Player_Villian ){
                                payoff = invMap[payoff];
                        }
                        eval_[ctx].mask_ = payoff;
                        return eval_[ctx].mask_;
                }

                using aggregator_t = std::function<Eval(std::vector<Eval>const&)>;

                aggregator_t hero_agg_ = [](std::vector<Eval> const& v){
                        return v.front();
                };

                auto choicePtr = Cast<ChoiceNode const*>(node);

                if(ctx.ActivePlayer() == p ){
                        [&](){
                                /*
                                        Want to find the path in this order,
                                        first we want to find nodes where we win,
                                        then win or draw (depending on opps move),
                                        etc
                                 */
                                static std::vector<unsigned> ticker = {
                                        Eval_Win,
                                        Eval_Win | Eval_Draw,
                                        Eval_Draw,
                                        Eval_Win | Eval_Lose,
                                        Eval_Win | Eval_Draw | Eval_Lose,
                                        Eval_Draw | Eval_Lose,
                                        Eval_Lose
                                };
                                for( auto t : ticker ){
                                        bool found = false;
                                        for( auto child : *choicePtr){
                                                if( Populate_(p, child) == t ){
                                                        // Their may be more than one
                                                        found = true;
                                                        eval_[ctx].mask_ = t;
                                                        eval_[ctx].next_.push_back(child);
                                                }
                                        }
                                        if( found )
                                                return;
                                }
                                assert( choicePtr->NumChildren() == 0 );
                        }();
                        return eval_[ctx].mask_;
                } else {
                        unsigned e = 0;
                        auto& ref = eval_[ctx];
                        for( auto child : *choicePtr){
                                e |= Populate_(p, child);
                                ref.next_.push_back(child);
                        }
                        ref.mask_ = e;
                        return e;
                }
        }
        std::map<GameContext, Result> eval_;
};

struct StrategyBuilder{
        GameTree Build(GameTree const& tree){
                NodeFactory fac;

                auto rootRet = fac.Make(tree.GetRoot()->GetContext());
                auto root = rootRet.second;

                GameTree stratTree{root};

                auto metric = EvalMetric::MakeForHero(tree);

                std::vector<Node*> stack;
                stack.push_back(root);
                for(;stack.size();){
                        auto ptr = stack.back();
                        stack.pop_back();

                        if( ptr->GetType() == Node::Type_Payoff)
                                continue;

                        auto choicePtr = Cast<ChoiceNode*>(ptr);

                        switch(ptr->GetContext().ActivePlayer()){
                        case Player_Hero:{

                                auto item = (*metric)(ptr->GetContext());

                                for( auto branch : item ){
                                        auto makeRet =fac.Make(branch->GetContext());
                                        auto next = makeRet.second;
                                        if( makeRet.first ){
                                                stratTree.Register(next);
                                                stack.push_back(next);
                                        }
                                        choicePtr->RegisterChild(next);
                                        break;
                                }
                                break;
                        }
                        /*
                                For villian we just need to copy
                                every possible move
                        */
                        case Player_Villian: {
                                auto aux = tree.Lookup(ptr->GetContext());
                                if( aux == nullptr ){
                                        std::cerr << "Can't find :(\n";
                                        break;
                                }
                                for( auto child : *Cast<ChoiceNode const*>(aux)){

                                        auto makeRet = fac.Make(child->GetContext());
                                        auto next = makeRet.second;
                                        if( makeRet.first ){
                                                stratTree.Register(next);
                                                stack.push_back(next);
                                        }
                                        choicePtr->RegisterChild(next);
                                }
                                break;
                        }
                        default:
                                break;
                        }
                }

                return std::move(stratTree);
        }
};

void RenderToDot(GameTree const& tree){
        std::cout << "digraph structs{\n";
        std::cout << "node [shape=record]\n";

        unsigned terminalMask = 0;

        auto tag = [](Node const* ptr){
                return "Node" + boost::lexical_cast<std::string>(ptr);
        };

        for( auto ptr : tree){
                auto t = tag(ptr);
                auto const& b = ptr->GetContext().GetBoard();
                std::cout
                        << t << " [shape=record,label=\""
                        << "{"
                                << "{"
                                << PlayerToken(b.GetTile(0,0)) << "|"
                                << PlayerToken(b.GetTile(1,0)) << "|"
                                << PlayerToken(b.GetTile(2,0))
                                << "}"
                        << "|"
                                << "{"
                                << PlayerToken(b.GetTile(0,1)) << "|"
                                << PlayerToken(b.GetTile(1,1)) << "|"
                                << PlayerToken(b.GetTile(2,1))
                                << "}"
                        << "|"
                                << "{"
                                << PlayerToken(b.GetTile(0,2)) << "|"
                                << PlayerToken(b.GetTile(1,2)) << "|"
                                << PlayerToken(b.GetTile(2,2))
                                << "}"
                        << "}\"];\n";
                if( ptr->GetType() == Node::Type_Choice ){
                        for( auto next : *Cast<ChoiceNode*>(ptr)){
                                std::cout << t << " -> " << tag(next) << ";\n";
                        }
                }

                if( ptr->GetType() == Node::Type_Payoff ){
                        auto payoff = Cast<PayoffNode*>(ptr)->GetPayoff();
                        std::cout << t << " -> " << EvalToString(payoff) << ";\n";
                        terminalMask |= payoff;
                }
        }

        std::cout << "}";
}

enum RenderOpt{
        RenderOpt_Solve,
};
void render(Player p, RenderOpt opt){

        TickTackToeLogic logic;
        GameTreeBuilder builder;
        GameContext ctx(p);
        auto tree = builder.Make(ctx);

        switch(opt){
        case RenderOpt_Solve:{
                StrategyBuilder sb;
                auto ret = sb.Build(tree);
                RenderToDot(ret);
        }
                break;
        }
}

void test2(){
        Detail::PictureBox first, second;
        first.Append("hello");
        first.NewLine();
        first.Append("world");
        second.Append(" oh ");
        second.NewLine();
        second.Append("yeah");
        first += second;
        first.Display();
}

void driver(){
        GameContext ctx{Player_Villian};
        GameTreeBuilder builder;
        TickTackToeLogic logic;
        auto tree = builder.Make(ctx);

        StrategyBuilder sb;
        auto strat = sb.Build(tree);

        std::random_device gen;
        std::uniform_int_distribution<int> dist(0,666);

        for(;ctx.GetCtrl() == GameCtrl_Continue;){
                std::cout << ctx.GetBoard() << "\n";

                switch(ctx.ActivePlayer()){
                case Player_Hero: {
                        auto move = Cast<ChoiceNode const*>(strat.Lookup(ctx));

                        assert( !! move );

                        // dedeuce move
                        auto randomOffset = dist(gen) % move->NumChildren();
                        auto nextMove = *std::next(move->begin(), randomOffset );
                        PRINT(move->NumChildren());
                        auto t = [&](){
                                for(size_t i=0;i!=3;++i){
                                        for(size_t j=0;j!=3;++j){
                                                if(     move->GetContext().GetBoard().GetTile(i,j) !=
                                                    nextMove->GetContext().GetBoard().GetTile(i,j) ){
                                                        return std::make_tuple(i,j);
                                                }

                                        }
                                }
                                throw std::domain_error("not move");
                        }();

                        ctx = logic.Next(ctx, std::get<0>(t), std::get<1>(t));
                        break;
                }
                case Player_Villian: {
                        char x_s, y_s;
                        std::cin >> x_s >> y_s;
                        do{
                                if(! ( std::isdigit(x_s) && std::isdigit(y_s))){
                                        std::cerr << "Invalid Move\n";
                                        break;
                                }
                                int x = x_s - '0';
                                int y = y_s - '0';
                                if( ctx.GetBoard().GetTile(x,y) != Player_NotAPlayer ){
                                        std::cerr << "Invalid Move\n";
                                        break;
                                }
                                ctx = logic.Next(ctx, x - '0', y - '0');
                        }while(0);
                        break;
                }
                default:
                        break;
                }
        }
        std::cout << ctx.GetBoard() << "\n";
        switch(ctx.GetCtrl()){
        case GameCtrl_Continue:
                break;
        case GameCtrl_HeroWins:
                std::cout << "Computer Won\n";
                break;
        case GameCtrl_VillianWins:
                std::cout << "You Won\n";
                break;
        case GameCtrl_Draw:
                std::cout << "Draw\n";
                break;
        default:
                return;
        }
}

int main(int argc, char* argv[]){
        try{
                switch(argc){
                case 1:
                        driver();
                        break;
                case 2: {
                        std::string arg = argv[1];
                        if( arg == "--hero" ){
                                render(Player_Hero, RenderOpt_Solve);
                        } else if(arg == "--villian"){
                                render(Player_Villian, RenderOpt_Solve);
                        } else if(arg == "--driver"){
                                driver();
                        } else{
                        }
                        break;
                }
                default:
                        std::cerr  << "unknown args\n";
                        return EXIT_FAILURE;
                }
        } catch(std::exception const& e){
                std::cerr << e.what() << "\n";
                return EXIT_FAILURE;
        }
}