/*

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/filters/statistical_outlier_removal.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/transformation_estimation_lm.h>

#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/visualization/pcl_visualizer.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 <iostream>

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

#include "../include/Lidar.h"
#include "../include/ipc.h"

using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
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;
};

typedef struct _RecvDataPack
{
    int index;
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;
}RecvDataPack;

typedef struct _SendDataPack
{
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;
}SendDataPack;
//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;

}

//remove singel func
//data acquired: Cloud's ptr for processing.
void remove_single (PointCloud::Ptr cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> RS;
    RS.setInputCloud(cloud);
    RS.setRadiusSearch(0.5);
    RS.setMinNeighborsInRadius(4);
    RS.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
}

//another new filter to remove the point faraway from the origin
//added on 7/30 by Wang
//data acquired: pcl::PointCloud<PointNormal> PointCloudN
void remove_distant (PointCloud::Ptr cloud)
{
    PointCloud::Ptr cloud_filtered(new PointCloud);
    pcl::StatisticalOutlierRemoval<PointT> RD;
    RD.setInputCloud(cloud);
    RD.setMeanK(50);
    RD.setStddevMulThresh(1.0);
    RD.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)
{
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;

    //processInitFlag = InitSem(LidarInSemId, LidarInSemKey, 1, IPC_CREAT|0777); //初始化信号量
    //processInitFlag = InitShareMemory(LidarInShmId, LidarInShmKey,4096, IPC_CREAT|0777);//初始化共享内存
    std::vector<std::vector<float>> data3d;
    std::vector<std::vector<float>> data_after;
    std::vector<float> data2d;

    int recvfd=open("../../../index", O_RDWR); //TODO
    int sendfd=open("../../../better_location", O_RDWR); //TODO

    if(recvfd<0)
    {
        std::cout<<"recv default"<<std::endl;
        exit(1);
    }
    if(sendfd<0)
    {
        std::cout<<"send default"<<std::endl;
        exit(1);
    }

    std::vector<Posemat> Pose;

    ifstream fin("../../../pose.txt"); // TODO
    int line = 0;
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity();

    while(!fin.eof())
    {
        fin >> line >> x >> y >> z >> yaw >> pitch >> roll;
        cout << " Receving No. " << line << " Pose." << endl;
        six2Trans(roll, pitch, yaw, x, y, z, GlobalTransform);
        Posemat a;
        a.global_transform = GlobalTransform;
        Pose.push_back(a);
        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);

    while(1)
    {
    PointCloud::Ptr source(new PointCloud);
    PointCloud::Ptr target(new PointCloud);
    PointCloud::Ptr temp(new PointCloud);
    PointCloud::Ptr Lidarcloud(new PointCloud);
        //updata lidar data get current cloud now
        //memcpy((void *)&target,&LidarSendCloudData.cloud, sizeof(target));

    pthread_mutex_lock(&LidarMutex);
    pcl::copyPointCloud(*safecloud, *Lidarcloud);
        //LidarSendCloudData.CloudReadSucceesFlag=0;
        //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
        RecvDataPack recvdatapack;
        memset(&recvdatapack,0,sizeof(recvdatapack));
        read(recvfd,&recvdatapack,sizeof(recvdatapack));

    //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 << "x: " <<recvdatapack.x << ", y: " << recvdatapack.y  << ", yaw: "<< recvdatapack.yaw << endl;
    six2Trans(recvdatapack.roll, recvdatapack.pitch, recvdatapack.yaw,
              recvdatapack.x, recvdatapack.y, recvdatapack.z, priorTransform);

    cout << "prior: \n" << priorTransform << endl;
    cout << "keyframe: \n" << Pose[recvdatapack.index].global_transform << endl;
    relateTransform = Pose[recvdatapack.index].global_transform.inverse() * priorTransform;
    cout << "relate: \n" << relateTransform << endl;
    //x_diff = abs(relateTransform(0,3));
    //y_diff = abs(relateTransform(1,3));

    min = hypot(relateTransform(0,3), relateTransform(1,3));
    SendDataPack senddatapack;
    memset(&senddatapack,0,sizeof(senddatapack));
    if(min < 0.4)
    {
        //get the two clouds
        char namebuf[30];
        sprintf(namebuf, "../../../data/%d.pcd", recvdatapack.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);
        //refine needed!!!
        pairAlign(target, source, temp, pairTransform);
        cout << "pair: \n" << pairTransform << endl;
        cout << "pair inverse: \n" <<  pairTransform.inverse() << endl;
        globalTransform_curr = Pose[recvdatapack.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);
        float newr, newp, newy;
        quaterniond2eulerangle(quaternion, newr, newp, newy);

        senddatapack.x = globalTransform_curr(0,3);
        senddatapack.y = globalTransform_curr(1,3);
        senddatapack.z = globalTransform_curr(2,3);
        senddatapack.roll = newr;
        senddatapack.pitch = newp;
        senddatapack.yaw = newy;
        write(sendfd,&senddatapack,sizeof(senddatapack));
    }
    else
    {
        senddatapack.x = recvdatapack.x;
        senddatapack.y = recvdatapack.y;
        senddatapack,z = recvdatapack.z;
        senddatapack.roll = recvdatapack.roll;
        senddatapack.pitch = recvdatapack.pitch;
        senddatapack.yaw = recvdatapack.yaw;
        write(sendfd,&senddatapack,sizeof(senddatapack));
    }
    pthread_mutex_unlock(&LidarMutex);
        //close();
        //showPCD(result);

    }

}


// C++
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
/// ROS
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <tf/transform_broadcaster.h>
/// Opencv
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/highgui/highgui.hpp>
// RGBD_RTK
#include <config_loader.h>
#include <marker_finder.h>
#include <reconstruction_visualizer.h>
// Autonomous Robot
#include <handleFiles.h>
#include <structures.h>

using namespace std;
using namespace cv;
using namespace aruco;

MarkerFinder marker_finder;
Eigen::Affine3f trans_camera_pose; // Turtlebot pose
Eigen::Quaternionf q_aruco;        // Quaternion of aruco pose
tf::TransformBroadcaster *br;      // Broadcaster for tf aruco
vector<Pose> all_markers;          // List of marker struct
string aruco_poses_file;
float aruco_marker_size, aruco_max_distance;
HandleFiles handleFiles;

void imageCallback(const sensor_msgs::ImageConstPtr &msg);
void rosMarkerFinder(cv::Mat rgb);
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
void savePosesInFile(const std_msgs::String::ConstPtr &msg);
void initRos(int argc, char **argv, string rgb_topic);
void loadParams();
void publishArucoTF();

int main(int argc, char **argv) {
    string camera_calibration_file, aruco_dic, rgb_topic;
    // Load config file param
    ConfigLoader param_loader("../../../src/ros_autonomous_robot/config_files/ConfigFile.yaml");
    param_loader.checkAndGetString("camera_calibration_file", camera_calibration_file);
    param_loader.checkAndGetString("aruco_dic", aruco_dic);
    param_loader.checkAndGetString("rgb_topic", rgb_topic);
    param_loader.checkAndGetString("aruco_poses_file", aruco_poses_file);
    param_loader.checkAndGetFloat("aruco_marker_size", aruco_marker_size);
    param_loader.checkAndGetFloat("aruco_max_distance", aruco_max_distance);

    marker_finder.markerParam(camera_calibration_file, aruco_marker_size, aruco_dic);

    initRos(argc, argv, rgb_topic); // Initializing ROS

    return 0;
}
/**
 * ROS Callback listen to rgb topic
 * @param sensor_msgs::ImageConstPtr image message
 */
void imageCallback(const sensor_msgs::ImageConstPtr &msgRGB) {
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB); // Trying convert rgb
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    rosMarkerFinder(cv_ptrRGB->image); // Calling marker finder function
}

/**
 * Looks forward ARUCO markers and saves in a  list of markerFound structure
 * @param cv::Mat image
 */
void rosMarkerFinder(cv::Mat rgb) {
    // Detect and get pose of all aruco markers
    marker_finder.detectMarkersPoses(rgb, trans_camera_pose, aruco_max_distance);
    Pose aruco_pose;
    bool marker_found = false;
    for (size_t j = 0; j < marker_finder.markers_.size(); j++) {
        // If the all_markers vector is empty we add the first marker we found
        if (all_markers.size() == 0) {
            aruco_pose.id = marker_finder.markers_[j].id;
            aruco_pose.x = marker_finder.marker_poses_[j](0, 3);
            aruco_pose.y = marker_finder.marker_poses_[j](1, 3);
            aruco_pose.z = marker_finder.marker_poses_[j](2, 3);
            q_aruco = marker_finder.marker_poses_[j].rotation();
            aruco_pose.x_rotation = q_aruco.x();
            aruco_pose.y_rotation = q_aruco.y();
            aruco_pose.z_rotation = q_aruco.z();
            aruco_pose.w_rotation = q_aruco.w();
            all_markers.push_back(aruco_pose);
        } else {
            // If it is not empty, we need to be sure that the marker we have already exists
            for (auto pose : all_markers) {
                if (marker_finder.markers_[j].id == pose.id) {
                    marker_found = true;
                    break;
                }
                // If exists we do nothing
            }
            // If do not exists we add
            if (marker_found == false) {
                aruco_pose.id = marker_finder.markers_[j].id;
                aruco_pose.x = marker_finder.marker_poses_[j](0, 3);
                aruco_pose.y = marker_finder.marker_poses_[j](1, 3);
                aruco_pose.z = marker_finder.marker_poses_[j](2, 3);
                q_aruco = marker_finder.marker_poses_[j].rotation();
                aruco_pose.x_rotation = q_aruco.x();
                aruco_pose.y_rotation = q_aruco.y();
                aruco_pose.z_rotation = q_aruco.z();
                aruco_pose.w_rotation = q_aruco.w();
                all_markers.push_back(aruco_pose);
            }
        }

        marker_finder.markers_[j].draw(rgb, Scalar(0, 0, 255), 1); // Drawing markers in rgb image
        // Drawing axis on window
        CvDrawingUtils::draw3dAxis(rgb, marker_finder.markers_[j], marker_finder.camera_params_);

        publishArucoTF(); // Publishing aruco pose

        cv::imshow("OPENCV_WINDOW", rgb); // Showing rgb image
        cv::waitKey(1);
    }
}

/**
 * Listen to robot odom and calculates camera position related to 0,0 of map
 * @param nav_msgs::Odometry::ConstPtr message
 */
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) {
    // Subscribing turtlebot pose
    Eigen::Vector3f v_robot(msg->pose.pose.position.x, msg->pose.pose.position.y, 0);
    // Subscribing turtlebot orientation pose
    Eigen::Quaternionf q_robot((msg->pose.pose.orientation.w), msg->pose.pose.orientation.x,
                               msg->pose.pose.orientation.y, (msg->pose.pose.orientation.z));
    // Convert a quaternion to a 3x3 rotation matrix:
    Eigen::Matrix3f R_robot = q_robot.normalized().toRotationMatrix();

    Eigen::Matrix4f robot_pose;
    robot_pose.setIdentity(); // Set to Identity
    robot_pose.block<3, 3>(0, 0) = R_robot;
    robot_pose.block<3, 1>(0, 3) = v_robot;

    Eigen::Vector3f v_camera(-0.087, -0.0125, 0.2972); // Subscribing turtlebot pose
    Eigen::Quaternionf q_camera(0.5, -0.5, 0.5, -0.5); // Subscribing turtlebot orientation pose

    // Convert a quaternion to a 3x3 rotation matrix:
    Eigen::Matrix3f R_camera = q_camera.normalized().toRotationMatrix();

    Eigen::Matrix4f camera_pose;
    camera_pose.setIdentity(); // Set to Identity
    camera_pose.block<3, 3>(0, 0) = R_camera;
    camera_pose.block<3, 1>(0, 3) = v_camera;

    trans_camera_pose = camera_pose.inverse() * robot_pose.inverse();
    // Calculating Transformation camera pose in relation to 0,0 global

    /* ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]",
       msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);
       ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x,
       msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
       ROS_INFO("Vel-> Linear: [%f], Angular: [%f]",
       msg->twist.twist.linear.x,msg->twist.twist.angular.z);
    */
}

/**
 * ROS Callback that listen to a string in order to save aruco markers in file
 * @param String::ConstPrt message
 */
void savePosesInFile(const std_msgs::String::ConstPtr &msg) {
    string listen = msg->data.c_str(); // Reading msg that was sent by marker_goal topic
                                       // Validing if string msg is 's'
    if (listen.compare("s") == 0)
        handleFiles.savePoses(all_markers, aruco_poses_file);
    else
        ROS_INFO("[%s] is not a valid input, use 's' to save all markers", msg->data.c_str());
}

/**
 * This function publishes the aruco markers tf related to 0,0
 */
void publishArucoTF() {
    br = new tf::TransformBroadcaster();
    tf::Transform transform;
    for (auto pose : all_markers) {
        // set the xyz and rotation pose
        transform.setOrigin(tf::Vector3(pose.x, pose.y, pose.z));
        transform.setRotation(
            tf::Quaternion(pose.w_rotation, pose.x_rotation, pose.y_rotation, pose.z_rotation));
        string aruco_tf = "aruco" + to_string(pose.id); // set aruco name
        // broadcasting to tf related to odom
        br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", aruco_tf));
    }
}

/**
 * Initialize ROS
 */
void initRos(int argc, char **argv, string rgb_topic) {
    ros::init(argc, argv, "marker_finder_ros"); // Starting ros
    ros::start();

    // Subscribing to marker_goal topic
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("marker_goal", 1000, savePosesInFile);
    // Subscribing to rgb image
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Subscriber rgb_sub = it.subscribe(rgb_topic, 1, imageCallback);

    ros::NodeHandle nn;
    ros::Subscriber odom_sub = nn.subscribe("odom", 1000, odomCallback); // Subscribing to odom

    ros::spin(); // Leting ROS doing what he needs to do
}


/*
All modification made by Cambricon Corporation: © 2018-2019 Cambricon Corporation
All rights reserved.
All other contributions:
Copyright (c) 2014--2019, the respective contributors
All rights reserved.
For the list of contributors go to https://github.com/BVLC/caffe/blob/master/CONTRIBUTORS.md
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of Intel Corporation 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 OWNER 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 <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template <>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
                           const CBLAS_TRANSPOSE TransB, const int M,
                           const int N, const int K, const float alpha,
                           const float* A, const float* B, const float beta,
                           float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

template <>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
                            const CBLAS_TRANSPOSE TransB, const int M,
                            const int N, const int K, const double alpha,
                            const double* A, const double* B, const double beta,
                            double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B, ldb,
              beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
                           const int N, const float alpha, const float* A,
                           const float* x, const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
                            const int N, const double alpha, const double* A,
                            const double* x, const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
                       float* Y) {
  cblas_saxpy(N, alpha, X, 1, Y, 1);
}

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
                        double* Y) {
  cblas_daxpy(N, alpha, X, 1, Y, 1);
}

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifdef USE_CUDA
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
                                       unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float* X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double* X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b, float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
                       double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b, float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
                       double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b, float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
                       double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b, float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
                       double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b, float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
                        double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
  vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
  vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() { return (*caffe_rng())(); }

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(b, std::numeric_limits<Dtype>::max());
}

template float caffe_nextafter(const float b);

template double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_uniform<float>(const int n, const float a,
                                       const float b, float* r);

template void caffe_rng_uniform<double>(const int n, const double a,
                                        const double b, double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a, const Dtype sigma,
                        Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_gaussian<float>(const int n, const float mu,
                                        const float sigma, float* r);

template void caffe_rng_gaussian<double>(const int n, const double mu,
                                         const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template void caffe_rng_bernoulli<double>(const int n, const double p,
                                          unsigned int* r);

template void caffe_rng_bernoulli<float>(const int n, const float p,
                                         unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
                                   const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
                                     const int incx, const double* y,
                                     const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template float caffe_cpu_dot<float>(const int n, const float* x,
                                    const float* y);

template double caffe_cpu_dot<double>(const int n, const double* x,
                                      const double* y);

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float* x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double* x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

licensed under the apache license, version 2.0 (the "license");
you may not use this file except in compliance with the license.
you may obtain a copy of the license at

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

unless required by applicable law or agreed to in writing, software
distributed under the license is distributed on an "as is" basis,
without warranties or conditions of any kind, either express or implied.
see the license for the specific language governing permissions and
limitations under the license.
--------------------------------------------------------------*/

#include "sdd_vio/vo_stereo.h"

#include <iostream>
#include "sdd_vio/utils/math_utils.h"
#include <algorithm>
#include <math.h>
#include <ros/ros.h>
#include <cmath>
#include <assert.h>
#include <boost/thread.hpp>
#include "sdd_vio/utils/ros_params_helper.h"
#include "sdd_vio/utils/timer.hpp"
#include <ctime>
#include <opencv2/core/eigen.hpp>

namespace sdd_vio {

/*
 * Pipeline for tracking -
 * Take in a pyramid of images and perform coarse-to-fine optimization between
 * last keyframe and current frame. Update global pose.
 *
 * Keeping SE3 T for compatibility to the code written before. Ceres uses only R and t.
 */
    void VoStereo::oneImagePipeline(const std::vector<cv::Mat>& img0)
    {
        ROS_DEBUG_STREAM("one-ImagePipeline: ----------- ");

        /* an iterator on all layers participating in tracking, coarse to fine order */
        int itop = nlays_-1;  // top layer index
        if (layer_chop_ == true) {
            itop = nlays_chopped_-1;
        }

        v_last_ = v_;
        T_wb_last_ = T_wb_;

        // change initial guess from IMU integration
        T_wb_.linear() = T_wb_last_.rotation() * R_meas_;
        v_ = T_wb_last_.rotation() * v_meas_ + v_last_ + g_ * t_meas_;
        T_wb_.translation() = T_wb_last_.rotation() * p_meas_ + T_wb_last_.translation() +
                              v_last_ * t_meas_ + 0.5 * g_ * t_meas_ * t_meas_;
        T_ = T_bc_inv_ * T_wb_ * T_bc_;
        T_curr_ = T_.inverse()*T_kf_;

        /* Do optimization for all pyramid layers, coarse to fine */
        for (int i=itop; i>=0; --i) {

          /* enable convergence visualization only for the specific visual layer specified in config file */
          use_opt_vis_ = (use_opt_vis_gb_ == true) ? ((i==visual_image_layer_) ? true : false) : false;

          if (use_ica_) //Inverse compositional approach
          {
            optimize(img0[base_layer_+i-1], J_[i], T_curr_, error_[i], variance_[i], feat_pixels_[i], feat_3D_[i],
                intensities_[i], mask_[i], num_feat_pts_[i], max_num_iter_, i);
            updatePose();  // vision only, so need to propagate T_curr changes to T and T_wb
          }
          else  // forward compositional approach
            optimize_fca(img0[base_layer_+i-1], T_curr_, T_wb_, T_, v_, i);  // T_curr, T, T_wb all changes in optimize_fca

          // ROS_INFO_STREAM("v_optimized: " << v_.transpose());
          // v_.setZero(); // currently set to zero to prevent drifting
          // v_ = (T_wb_.block<3,1>(0,3) - T_wb_last_.block<3,1>(0,3))/t_meas_;
        }

    }

    /* Pipeline for new keyframe */
    void VoStereo::twoImagePipeline(const std::vector<cv::Mat>& img0, const std::vector<cv::Mat>& img1)
    {

        ROS_DEBUG_STREAM("two-ImagePipeline entered.");

        layer_chop_ = false;  // first assume no layer chop needed for this keyframe

        /* declaring disp related vectors */
        std::vector<cv::Mat> disp(nlays_), G_binary(nlays_), Gx_mat(nlays_), Gy_mat(nlays_);
        std::vector< std::vector<float> > disp_vec(nlays_);

        /* obtain disp, G_binary, Gx_mat, Gy_mat and perform tracking */
        if (use_thread_ == true)
        {
            disp.clear();
            G_binary.clear();
            Gx_mat.clear();
            Gy_mat.clear();

            /* temporary vectors */
            cv::Mat disp_pyr1, disp_pyr2, disp_pyr3, disp_pyr4;
            cv::Mat G_binary_pyr1, G_binary_pyr2, G_binary_pyr3, G_binary_pyr4;
            cv::Mat Gx_mat_pyr1, Gx_mat_pyr2, Gx_mat_pyr3, Gx_mat_pyr4;
            cv::Mat Gy_mat_pyr1, Gy_mat_pyr2, Gy_mat_pyr3, Gy_mat_pyr4;

            /*
             * spawning threads -
             * only the largest layer gets disparity and feature extraction separated into two threads
             * all other smaller pyramid images combine them into one thread.
             */
            assert(nlays_==1 || nlays_==2 || nlays_==3 || nlays_==4);

            switch (nlays_) {
            /* switch on the number of layers, default is 1. any upper layers will spawn all the lower layer threads */
            case 4: {
                thread_pyr4_ = boost::thread(&VoStereo::threadFuncComb, this, boost::ref(img0[base_layer_+2]), boost::ref(img1[base_layer_+2]),
                        boost::ref(G_binary_pyr4), boost::ref(disp_pyr4), boost::ref(Gx_mat_pyr4), boost::ref(Gy_mat_pyr4), 3);
            }
            case 3: {
                thread_pyr3_ = boost::thread(&VoStereo::threadFuncComb, this, boost::ref(img0[base_layer_+1]), boost::ref(img1[base_layer_+1]),
                        boost::ref(G_binary_pyr3), boost::ref(disp_pyr3), boost::ref(Gx_mat_pyr3), boost::ref(Gy_mat_pyr3), 2);
            }
            case 2: {
                thread_pyr2_ = boost::thread(&VoStereo::threadFuncComb, this, boost::cref(img0[base_layer_]), boost::cref(img1[base_layer_]),
                        boost::ref(G_binary_pyr2), boost::ref(disp_pyr2), boost::ref(Gx_mat_pyr2), boost::ref(Gy_mat_pyr2), 1);
            }
            case 1: {
                thread_pyr1_disp_ = boost::thread(&VoStereo::threadFunc_disp, this,
                        boost::ref(img0[base_layer_-1]), boost::ref(img1[base_layer_-1]), boost::ref(disp_pyr1));
                thread_pyr1_feat_ = boost::thread(&VoStereo::threadFunc_feat, this,
                        boost::ref(img0[base_layer_-1]), boost::ref(G_binary_pyr1), boost::ref(Gx_mat_pyr1), boost::ref(Gy_mat_pyr1));
                thread_tracking_ = boost::thread(&VoStereo::threadOneImagePipeline, this, boost::cref(img0));
            }
            }

            /*
             * join threads -
             */
            switch (nlays_) {
            case 4:
                thread_pyr4_.join();
            case 3:
                thread_pyr3_.join();
            case 2:
                thread_pyr2_.join();
            case 1:
                thread_pyr1_disp_.join();
                thread_pyr1_feat_.join();
                thread_tracking_.join();
            }

            /*
             * push temporary vectors to general vectors, need to push bottom up the pyramid
             */
            // TODO - in FCA we don't need to get Gx and Gy
            disp.push_back(disp_pyr1);
            G_binary.push_back(G_binary_pyr1);
            Gx_mat.push_back(Gx_mat_pyr1);
            Gy_mat.push_back(Gy_mat_pyr1);
            if (nlays_ > 1) {
                disp.push_back(disp_pyr2);
                G_binary.push_back(G_binary_pyr2);
                Gx_mat.push_back(Gx_mat_pyr2);
                Gy_mat.push_back(Gy_mat_pyr2);
            }
            if (nlays_ > 2) {
                disp.push_back(disp_pyr3);
                G_binary.push_back(G_binary_pyr3);
                Gx_mat.push_back(Gx_mat_pyr3);
                Gy_mat.push_back(Gy_mat_pyr3);
            }
            if (nlays_ > 3) {
                disp.push_back(disp_pyr4);
                G_binary.push_back(G_binary_pyr4);
                Gx_mat.push_back(Gx_mat_pyr4);
                Gy_mat.push_back(Gy_mat_pyr4);
            }

        }
        else //if (use_thread_ == false)
        {
            /* tracking */
            if (stage_!=STAGE_PAUSED) {
                ROS_DEBUG_STREAM("two-ImagePipeline: tracking...");
                oneImagePipeline(img0);
            }

            disp.clear();
            G_binary.clear();
            Gx_mat.clear();
            Gy_mat.clear();

            for (int i=0; i<nlays_; ++i)
            {
                ROS_DEBUG_STREAM("two-ImagePipeline: not using thread. getting disparity...");
                cam_->getDisparityMap(img0[base_layer_+i-1], img1[base_layer_+i-1], disp[i]);

                // showMatProp(disp_[i]);   // disp type: 8UC1
                ROS_DEBUG_STREAM("two-ImagePipeline: extracting features...");
                extractFeats(img0[base_layer_+i-1], G_binary[i], Gx_mat[i], Gy_mat[i], i);
            }

        }

        /* obtain vectors for each layer. If use inverse comositional approach, calculate Jacobian */
        for (int i=0; i<nlays_; ++i)
        {
            /* here Gx_ is the vector, Gx_mat_ is the matrix, both member variables */
            // TODO - in FCA we don't need to get Gx and Gy
            fillVectors(img0[base_layer_+i-1], G_binary[i], disp[i], Gx_mat[i], Gy_mat[i], disp_vec[i], feat_pixels_[i],
                    intensities_[i], Gx_[i], Gy_[i], num_feat_pts_[i], i);

            cam_->get3DPoints(feat_pixels_[i], disp_vec[i], feat_3D_[i], num_feat_pts_[i], i);
            ROS_INFO_STREAM("number of feature points: " << num_feat_pts_[i]);

            if (use_ica_ == true) {
                /* get jacobian J_ for feat_3D */
                getJacobian(Gx_[i], Gy_[i], focal_pyr_[i], focal_pyr_[i], feat_3D_[i], J_[i], num_feat_pts_[i]);

                /* initialize error and variance vector */
                error_[i] = Eigen::VectorXf::Constant(num_feat_pts_[i],0);
                variance_[i] = Eigen::VectorXf::Constant(num_feat_pts_[i],1);
                mask_[i] = Eigen::VectorXf::Constant(num_feat_pts_[i],1);
            }
        }

        /* getting average scene depth - use the top layer */
        float sum_depth = 0;
        for (int i=0; i<num_feat_pts_[nlays_-1]; ++i) {
            sum_depth = sum_depth + feat_3D_[nlays_-1][i](2);
        }
        avg_depth_ = sum_depth/num_feat_pts_[nlays_-1];

        /* check if number of points decrease as layer up */
        for (int i=0; i<nlays_; ++i) {
            if ((i-1)>=0) { // if there is a layer before this layer
                if (num_feat_pts_[i-1] < num_feat_pts_[i])
                    ROS_WARN_STREAM("unreasonable feature point layout in pyramid!");
            }
        }

        /* detect layer chop */
        for (int i=0; i<nlays_; ++i) {
            if (num_feat_pts_[i] < 20) { // number of points too small
                layer_chop_ = true;
                nlays_chopped_ = i;
                ROS_INFO_STREAM("Layer chop set to true. New number of layers for current frame: " << nlays_chopped_);
                if (i==0)
                    ROS_WARN_STREAM("Chopped number of layers is 0! No tracking being done.");
            }
        }

    }

    void VoStereo::threadCreatePyr(const cv::Mat& img, std::vector<cv::Mat>& img_ready)
    {
        ROS_DEBUG_STREAM("threadCreatePyr entered.");
        cv::GaussianBlur(img, img_ready[0],cv::Size(3,3),0,0);
        for (int i=1; i<full_layer_; ++i) {
            cv::pyrDown(img_ready[i-1], img_ready[i], cv::Size(int(img_ready[i-1].cols/2), int(img_ready[i-1].rows/2)));
        }
    }

    /* disparity thread - used on the largest image in the pyramid */
    void VoStereo::threadFunc_disp(const cv::Mat& img0, const cv::Mat& img1, cv::Mat& disp)
    {
        static kr::Timer<kr::us> timer("disp");
        timer.Start();
        ROS_DEBUG_STREAM("thread-Func-Disp: getting disparity maps...");
        cam_->getDisparityMap(img0, img1, disp);

        if (pub_image_debug_) {
            ROS_DEBUG_STREAM("thread-Func-Disp: setting disparity visualization...");
            cv::ximgproc::getDisparityVis(disp, visual1);
        }
        timer.Stop();
        timer.Report();
    }

    /* feature extraction thread - used on the largest image in the pyramid */
    void VoStereo::threadFunc_feat(const cv::Mat& img0, cv::Mat& G_binary, cv::Mat& Gx_mat, cv::Mat& Gy_mat)
    {
        static kr::Timer<kr::us> timer("feat");
        timer.Start();
        ROS_DEBUG_STREAM("thread-Func-Feat: extracting features...");
        extractFeats(img0, G_binary, Gx_mat, Gy_mat, 0);

        if (pub_image_debug_) {
            visual2.setTo(0);
            visual2 = G_binary;
        }
        timer.Stop();
        timer.Report();
    }

    /* having disparity and feature extraction in one thread */
    void VoStereo::threadFuncComb(const cv::Mat& img0, const cv::Mat& img1, cv::Mat& G_binary,
            cv::Mat& disp, cv::Mat& Gx_mat, cv::Mat& Gy_mat, int ilay)
    {
        cam_->getDisparityMap(img0, img1, disp);
        extractFeats(img0, G_binary, Gx_mat, Gy_mat, ilay);
    }

    void VoStereo::threadOneImagePipeline(const std::vector<cv::Mat>& img0)
    {
        static kr::Timer<kr::us> timer("tracking");
        if (stage_!=STAGE_PAUSED)
        {
            timer.Start();
            ROS_DEBUG_STREAM("thread-One-ImagePipeline: calling one-ImagePipeline...");
            oneImagePipeline(img0);
            ROS_DEBUG_STREAM("thread-One-ImagePipeline: updating keyframe poses...");
            setKfPose();
            timer.Stop();
        }
        timer.Report();
    }

}


#include "quadruped-reactive-walking/gepadd.hpp"
#include "quadruped-reactive-walking/MPC.hpp"
#include "quadruped-reactive-walking/Planner.hpp"
#include "quadruped-reactive-walking/InvKin.hpp"
#include "quadruped-reactive-walking/QPWBC.hpp"

#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>

namespace bp = boost::python;

template <typename MPC>
struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC> > {
  template <class PyClassMPC>
  void visit(PyClassMPC& cl) const {
    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
        .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"),
                                           "Constructor with parameters."))

        // Run MPC from Python
        .def("run", &MPC::run, bp::args("num_iter", "xref_in", "fsteps_in"), "Run MPC from Python.\n")
        .def("get_latest_result", &MPC::get_latest_result,
             "Get latest result (predicted trajectory + forces to apply).\n")
        .def("get_gait", &MPC::get_gait, "Get gait matrix.\n")
        .def("get_Sgait", &MPC::get_Sgait, "Get S_gait matrix.\n");
  }

  static void expose() {
    bp::class_<MPC>("MPC", bp::no_init).def(MPCPythonVisitor<MPC>());

    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
  }
};

void exposeMPC() { MPCPythonVisitor<MPC>::expose(); }

template <typename Planner>
struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planner> > {
  template <class PyClassPlanner>
  void visit(PyClassPlanner& cl) const {
    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
        .def(bp::init<double, double, double, double, int, bool, double, const Eigen::MatrixXd&>(
            bp::args("dt_in", "dt_tsid_in", "T_gait_in", "T_mpc_in", "k_mpc_in", "on_solo8_in", "h_ref_in",
                     "fsteps_in"),
            "Constructor with parameters."))

        .def("get_xref", &Planner::get_xref, "Get xref matrix.\n")
        .def("get_fsteps", &Planner::get_fsteps, "Get fsteps matrix.\n")
        .def("get_gait", &Planner::get_gait, "Get gait matrix.\n")
        .def("get_goals", &Planner::get_goals, "Get position goals matrix.\n")
        .def("get_vgoals", &Planner::get_vgoals, "Get velocity goals matrix.\n")
        .def("get_agoals", &Planner::get_agoals, "Get acceleration goals matrix.\n")
        //.add_property("xref", &Planner::get_xref)

        // Run Planner from Python
        .def("run_planner", &Planner::run_planner, bp::args("k", "q", "v", "b_vref", "h_estim", "z_average", "joystick_code"),
             "Run Planner from Python.\n");
  }

  static void expose() {
    bp::class_<Planner>("Planner", bp::no_init).def(PlannerPythonVisitor<Planner>());

    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
  }
};

void exposePlanner() { PlannerPythonVisitor<Planner>::expose(); }

template <typename InvKin>
struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin> > {
  template <class PyClassInvKin>
  void visit(PyClassInvKin& cl) const {
    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
        .def(bp::init<double>(bp::args("dt_in"), "Constructor with parameters."))

        .def("get_q_step", &InvKin::get_q_step, "Get velocity goals matrix.\n")
        .def("get_dq_cmd", &InvKin::get_dq_cmd, "Get acceleration goals matrix.\n")

        // Run InvKin from Python
        .def("refreshAndCompute", &InvKin::refreshAndCompute,
             bp::args("x_cmd", "contacts", "goals", "vgoals", "agoals", "posf", "vf", "wf", "af", "Jf",
                      "posb", "rotb", "vb", "ab", "Jb"),
             "Run InvKin from Python.\n");
  }

  static void expose() {
    bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinPythonVisitor<InvKin>());

    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
  }
};

void exposeInvKin() { InvKinPythonVisitor<InvKin>::expose(); }

template <typename QPWBC>
struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC> > {
  template <class PyClassQPWBC>
  void visit(PyClassQPWBC& cl) const {
    cl.def(bp::init<>(bp::arg(""), "Default constructor."))

        .def("get_f_res", &QPWBC::get_f_res, "Get velocity goals matrix.\n")
        .def("get_ddq_res", &QPWBC::get_ddq_res, "Get acceleration goals matrix.\n")
        .def("get_H", &QPWBC::get_H, "Get H weight matrix.\n")

        // Run QPWBC from Python
        .def("run", &QPWBC::run, bp::args("M", "Jc", "f_cmd", "RNEA", "k_contacts"), "Run QPWBC from Python.\n");
  }

  static void expose() {
    bp::class_<QPWBC>("QPWBC", bp::no_init).def(QPWBCPythonVisitor<QPWBC>());

    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
  }
};

void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }

BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
  boost::python::def("add", gepetto::example::add);
  boost::python::def("sub", gepetto::example::sub);

  eigenpy::enableEigenPy();

  exposeMPC();
  exposePlanner();
  exposeInvKin();
  exposeQPWBC();
}


#ifndef TORRENT_MEMORY_STORAGE_HPP_INCLUDED
#define TORRENT_MEMORY_STORAGE_HPP_INCLUDED

#include <math.h>
#include <memory>
#include <algorithm>
#include <iostream>

#include <boost/cstdint.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/thread/mutex.hpp>

#include <libtorrent/error_code.hpp>
#include <libtorrent/bencode.hpp>
#include <libtorrent/storage.hpp>
#include <libtorrent/storage_defs.hpp>
#include <libtorrent/block_cache.hpp>
#include <libtorrent/fwd.hpp>
#include <libtorrent/file.hpp>
#include <libtorrent/entry.hpp>
#include <libtorrent/torrent_info.hpp>
#include <libtorrent/torrent_handle.hpp>
#include <libtorrent/torrent.hpp>

typedef boost::dynamic_bitset<> Bitset;

using namespace libtorrent;

namespace libtorrent {
        boost::int64_t memory_size = 0;

        // std::chrono::milliseconds now() {
        //         return std::chrono::duration_cast< std::chrono::milliseconds >(
        //                 std::chrono::system_clock::now().time_since_epoch()
        //         );
        // }

        boost::posix_time::ptime now() {
                return boost::posix_time::microsec_clock::local_time();
        }

        struct memory_piece
        {
        public:
                int index;
                int length;

                int size;
                int bi;
                bool is_completed;
                bool is_read;

                memory_piece(int i, int length) : index(i), length(length) {
                        size = 0;
                        bi = -1;
                        is_completed = false;
                        is_read = false;
                };

                bool is_buffered() {
                        return bi != -1;
                };

                void reset() {
                        bi = -1;
                        is_completed = false;
                        is_read = false;
                        size = 0;

                        // if (is_logging) {
                        //         std::cerr << "INFO Freeing piece " << index << std::endl;
                        // };
                }
        };

        struct memory_buffer
        {
        public:
                int index;
                int length;
                std::vector<char> buffer;

                int pi;
                bool is_used;
                boost::posix_time::ptime accessed;

                memory_buffer(int index, int length) : index(index), length(length) {
                        pi = -1;
                        is_used = false;

                        buffer.resize(length);
                };

                bool is_assigned() {
                        return pi != -1;
                };

                void reset() {
                        is_used = false;
                        pi = -1;
                        accessed = now();
                        std::fill(buffer.begin(), buffer.end(), '\0');

                        // if (is_logging) {
                        //         std::cerr << "INFO Freeing buffer " << index << std::endl;
                        // };
                };
        };

        struct memory_storage : storage_interface
        {
        private:
                boost::mutex m_mutex;
                boost::mutex r_mutex;
        public:
                Bitset reader_pieces;
                Bitset reserved_pieces;

                std::string id;
                boost::int64_t capacity;

                int piece_count;
                boost::int64_t piece_length;
                std::vector<memory_piece> pieces;

                int buffer_size;
                int buffer_limit;
                int buffer_used;
                int buffer_reserved;
                std::vector<memory_buffer> buffers;

                file_storage const* m_files;
                torrent_info const* m_info;
                libtorrent::torrent_handle* m_handle;
                libtorrent::torrent* t;

                bool is_logging;
                bool is_initialized;
                bool is_reading;

                memory_storage(storage_params const& params) {
                        piece_count = 0;
                        piece_length = 0;

                        buffer_size = 0;
                        buffer_limit = 0;
                        buffer_used = 0;
                        buffer_reserved = 0;

                        is_logging = false;
                        is_initialized = false;
                        is_reading = false;

                        m_files = params.files;
                        m_info = params.info;

                        capacity = memory_size;
                        piece_count = m_info->num_pieces();
                        piece_length = m_info->piece_length();

                        std::cerr << "INFO Init with mem size " << memory_size << ", Pieces: " << piece_count <<
                                ", Piece length: " << piece_length << std::endl;

                        for (int i = 0; i < piece_count; i++) {
                                pieces.push_back(memory_piece(i, m_info->piece_size(i)));
                        }

                        // Using max possible buffers + 2
                        buffer_size = rint(ceil(capacity/piece_length) + 2);
                        if (buffer_size > piece_count) {
                                buffer_size = piece_count;
                        };
                        buffer_limit = buffer_size;
                        std::cerr << "INFO Using " << buffer_size << " buffers" << std::endl;

                        for (int i = 0; i < buffer_size; i++) {
                                buffers.push_back(memory_buffer(i, piece_length));
                        }

                        reader_pieces.resize(piece_count+10);
                        reserved_pieces.resize(piece_count+10);

                        is_initialized = true;
                };

                ~memory_storage() {};

                void initialize(storage_error& ec) {}

                boost::int64_t get_memory_size() {
                        return capacity;
                }

                void set_memory_size(boost::int64_t s) {
                        if (s <= capacity) return;

                        boost::unique_lock<boost::mutex> scoped_lock(m_mutex);

                        capacity = s;

                        int prev_buffer_size = buffer_size;

                        // Using max possible buffers + 2
                        buffer_size = rint(ceil(capacity/piece_length) + 2);
                        if (buffer_size > piece_count) {
                                buffer_size = piece_count;
                        };
                        buffer_limit = buffer_size;
                        if (prev_buffer_size == buffer_size) {
                                std::cerr << "INFO Not increasing buffer due to same size (" << buffer_size << ")" << std::endl;
                                return;
                        };

                        std::cerr << "INFO Increasing buffer to " << buffer_size << " buffers" << std::endl;

                        for (int i = prev_buffer_size; i < buffer_size; i++) {
                                buffers.push_back(memory_buffer(i, piece_length));
                        }
                }

                // bool has_any_file() {
                //         if (logging) {
                //                 std::cerr << "INFO has_any_file" << std::endl;
                //         };
                //         return false;
                // }

                int read(char* read_buf, int size, int piece, int offset) {
                        if (!is_initialized) return 0;
                        is_reading = true;

                        if (is_logging) {
                                printf("Read start: %d, off: %d, size: %d \n", piece, offset, size);
                        };

                        if (!get_read_buffer(&pieces[piece])) {
                                if (is_logging) {
                                        std::cerr << "INFO nobuffer: " << piece << ", off: " << offset << std::endl;
                                };
                                restore_piece(piece);
                                return -1;
                        };
                        if (pieces[piece].size < pieces[piece].length) {
                                if (is_logging) {
                                        std::cerr << "INFO less: " << piece << ", off: " << offset << ", size: " << pieces[piece].size << ", length: " << pieces[piece].length << std::endl;
                                };
                                restore_piece(piece);
                                return -1;
                        };

                        int available = buffers[pieces[piece].bi].buffer.size() - offset;
                        if (available <= 0) return 0;
                        if (available > size) available = size;

                        if (is_logging) {
                                printf("       pre: %d, off: %d, size: %d, available: %d, sizeof: %d \n", piece, offset, size, available, int(sizeof(read_buf)));
                        };
                        memcpy(read_buf, &buffers[pieces[piece].bi].buffer[offset], available);

                        if (pieces[piece].is_completed && offset+available >= pieces[piece].size) {
                                pieces[piece].is_read = true;
                        };

                        buffers[pieces[piece].bi].accessed = now();

                        return size;
                };

                int readv(libtorrent::file::iovec_t const* bufs, int num_bufs
                        , int piece, int offset, int flags, libtorrent::storage_error& ec)
                {
                        if (!is_initialized) return 0;

                        if (is_logging) {
                                std::cerr << "INFO readv in  p: " << piece << ", off: " << offset << std::endl;
                        };

                        if (!get_read_buffer(&pieces[piece])) {
                                if (is_logging) {
                                        std::cerr << "INFO noreadbuffer: " << piece << std::endl;
                                };
                                return 0;
                        };

                        int file_offset = offset;
                        int n = 0;
                        for (int i = 0; i < num_bufs; ++i)
                        {
                                int const to_copy = std::min(buffers[pieces[piece].bi].buffer.size() - file_offset, bufs[i].iov_len);
				memcpy(bufs[i].iov_base, &buffers[pieces[piece].bi].buffer[file_offset], to_copy);
                                file_offset += to_copy;
				n += to_copy;

				// memcpy(bufs[i].iov_base, &b.buffer[file_offset], bufs[i].iov_len);
                                // file_offset += bufs[i].iov_len;
				// n += bufs[i].iov_len;
                        };

                        if (is_logging) {
                                int size = bufs_size(bufs, num_bufs);
                                std::cerr << "INFO readv out p: " << piece << ", pl: " << pieces[piece].length
                                        << ", bufs: " << num_bufs << "/" << bufs[0].iov_len
                                        << ", off: " << offset
                                        << ", bs: " << buffers[pieces[piece].bi].buffer.size() << ", res: " << size << "=" << n << std::endl;
                        };

                        if (pieces[piece].is_completed && offset+n >= pieces[piece].size) {
                                pieces[piece].is_read = true;
                        };

                        buffers[pieces[piece].bi].accessed = now();

                        return n;
                };

                int writev(libtorrent::file::iovec_t const* bufs, int num_bufs
                        , int piece, int offset, int flags, libtorrent::storage_error& ec)
                {
                        if (is_logging) {
                                std::cerr << "INFO writev in  p: " << piece << ", off: " << offset << ", bufs: " << bufs_size(bufs, num_bufs) << std::endl;
                        };

                        if (!is_initialized) return 0;

                        if (!get_write_buffer(&pieces[piece])) {
                                if (is_logging) {
                                        std::cerr << "INFO nowritebuffer: " << piece << std::endl;
                                };
                                return 0;
                        };

                        int size = bufs_size(bufs, num_bufs);

                        int file_offset = offset;
                        int n = 0;
                        for (int i = 0; i < num_bufs; ++i)
                        {
                                int const to_copy = std::min(std::size_t(pieces[piece].length) - file_offset, bufs[i].iov_len);
                                std::memcpy(&buffers[pieces[piece].bi].buffer[file_offset], bufs[i].iov_base, to_copy);

                                file_offset += to_copy;
                                n += to_copy;
                        };

                        if (is_logging) {
                                std::cerr << "INFO writev out p: " << piece << ", pl: " << pieces[piece].length
                                        << ", bufs: " << num_bufs << " / " << bufs[0].iov_len
                                        << ", req: " << size << ", off: " << offset
                                        << ", bs: " << buffers[pieces[piece].bi].buffer.size() << ", res: " << size << "=" << n << std::endl;
                        };

                        pieces[piece].size += n;
                        buffers[pieces[piece].bi].accessed = now();

                        if (buffer_used >= buffer_limit) {
                                trim(piece);
                        }

                        return n;
                };

                void rename_file(int index, std::string const& new_filename
                        , libtorrent::storage_error& ec) {}

                bool move_storage(std::string const& save_path) {
                        return false;
                }

                bool verify_resume_data(libtorrent::bdecode_node const& rd
                        , std::vector<std::string> const* links
                        , libtorrent::storage_error& error) {
                        return false;
                }

                bool write_resume_data(libtorrent::entry& rd) const {
                        return false;
                }

                void write_resume_data(libtorrent::entry& rd, libtorrent::storage_error& ec) {
                }

                void release_files(libtorrent::storage_error& ec) {
                }

                bool delete_files() {
                        return false;
                }

                bool has_any_file(libtorrent::storage_error& ec) {
                        if (is_logging) {
                                printf("Has 2 \n");
                        };
                        return false;
                }

                void set_torrent_handle(libtorrent::torrent_handle* h) {
                        m_handle = h;
                        t = m_handle->native_handle().get();
                }

                void set_file_priority(std::vector<boost::uint8_t>& prio, libtorrent::storage_error& ec)
                {
                        if (is_logging) {
                                printf("Set prio \n");
                        };
                }

                int move_storage(std::string const& save_path, int flags, libtorrent::storage_error& ec)
                {
                        if (is_logging) {
                                printf("Move storage 2 \n");
                        };
                        return 0;
                }

                void write_resume_data(libtorrent::entry& rd, libtorrent::storage_error& ec) const
                {
                        if (is_logging) {
                                printf("Write resume 2 \n");
                        };
                }

                void delete_files(int options, libtorrent::storage_error& ec) {
                        if (is_logging) {
                                printf("Delete file 2 \n");
                        };
                };

                bool get_read_buffer(memory_piece* p) {
                        return get_buffer(p, false);
                };

                bool get_write_buffer(memory_piece* p) {
                        return get_buffer(p, true);
                };

                bool get_buffer(memory_piece *p, bool is_write) {
                        if (p->is_buffered()) {
                                return true;
                        } else if (!is_write) {
                                // Trying to lock and get to make sure we are not affected
                                // by write/read at the same time.
                                boost::unique_lock<boost::mutex> scoped_lock(m_mutex);
                                return p->is_buffered();
                        }

                        boost::unique_lock<boost::mutex> scoped_lock(m_mutex);

                        // Once again checking in case we had multiple writes in parallel
                        if (p->is_buffered()) return true;

                        // Check if piece is not in reader ranges and avoid allocation
                        if (is_reading && !is_readered(p->index)) {
                                restore_piece(p->index);
                                return false;
                        }

                        for (int i = 0; i < buffer_size; i++) {
                                if (buffers[i].is_used) {
                                        continue;
                                };

                                if (is_logging) {
                                        std::cerr << "INFO Setting buffer " << buffers[i].index << " to piece " << p->index << std::endl;
                                };

                                buffers[i].is_used = true;
                                buffers[i].pi = p->index;
                                buffers[i].accessed = now();

                                // buffers[i].buffer.clear();
                                // buffers[i].buffer.resize(p->length);

                                p->bi = buffers[i].index;

                                // If we are placing permanent buffer entry - we should reduce the limit,
                                // to propely check for the usage.
                                if (reserved_pieces.test(p->index)) {
                                        buffer_limit--;
                                } else {
                                        buffer_used++;
                                };

                                break;
                        }

                        return p->is_buffered();
                };

                void trim(int pi) {
                        if (capacity < 0 || buffer_used < buffer_limit) {
                                return;
                        };

                        boost::unique_lock<boost::mutex> scoped_lock(m_mutex);

                        while (buffer_used >= buffer_limit) {
                                if (is_logging) {
                                        std::cerr << "INFO Trimming " << buffer_used << " to " << buffer_limit << " with reserved " << buffer_reserved << ", " << get_buffer_info() << std::endl;
                                };

                                if (!reader_pieces.empty()) {
                                        int bi = find_last_buffer(pi, true);
                                        if (bi != -1) {
                                                if (is_logging) {
                                                        std::cerr << "INFO Removing non-read piece: " << buffers[bi].pi << ", buffer:" << bi << std::endl;
                                                };
                                                remove_piece(bi);
                                                continue;
                                        }
                                }

                                int bi = find_last_buffer(pi, false);
                                if (bi != -1) {
                                        if (is_logging) {
                                                std::cerr << "INFO Removing LRU piece: " << buffers[bi].pi << ", buffer:" << bi << std::endl;
                                        };
                                        remove_piece(bi);
                                        continue;
                                }
                        }
                };

                std::string get_buffer_info() {
                        std::string result = "";

                        for (int i = 0; i < buffers.size(); i++)
                        {
                                if (!result.empty()) result += " ";

                                // result += std::to_string(it->index) + ":" + std::to_string(it->pi);
                                result += boost::lexical_cast<std::string>(buffers[i].index) + ":" + boost::lexical_cast<std::string>(buffers[i].pi);
                        };

                        return result;
                };

                int find_last_buffer(int pi, bool check_read) {
                        int bi = -1;
                        boost::posix_time::ptime minTime = now();
                        boost::unique_lock<boost::mutex> scoped_lock(r_mutex);

                        for (int i = 0; i < buffers.size(); i++)
                        {
                                if (buffers[i].is_used && buffers[i].is_assigned()
                                        && !is_reserved(buffers[i].pi)
                                        && buffers[i].pi != pi
                                        && (!check_read || !is_readered(buffers[i].pi))
                                        && buffers[i].accessed < minTime) {
                                        bi = buffers[i].index;
                                        minTime = buffers[i].accessed;
                                };
                        };

                        return bi;
                }

                void remove_piece(int bi) {
                        int pi = buffers[bi].pi;

                        buffers[bi].reset();
                        buffer_used--;

                        if (pi != -1 && pi < piece_count) {
                                pieces[pi].reset();
                                restore_piece(pi);
                        }
                }

                void restore_piece(int pi) {
                        if (!m_handle || !t) return;

                        // libtorrent::torrent* t = m_handle->native_handle().get();
                        // if (!t) return;

                        if (is_logging) {
                                std::cerr << "INFO Restoring piece: " << pi << std::endl;
                        };
                        // t->picker().reset_piece(pi);
                        t->reset_piece_deadline(pi);
                        t->picker().set_piece_priority(pi, 0);
                        t->picker().we_dont_have(pi);
                }

                void enable_logging() {
                        is_logging = true;
                }

                void disable_logging() {
                        is_logging = false;
                }

                void update_reader_pieces(std::vector<int> pieces) {
                        if (!is_initialized) return;

                        boost::unique_lock<boost::mutex> scoped_lock(r_mutex);
                        reader_pieces.reset();
                        for (int i = 0; i < pieces.size(); i++) {
                                reader_pieces.set(pieces[i]);
                        };
                };

                void update_reserved_pieces(std::vector<int> pieces) {
                        if (!is_initialized) return;

                        boost::unique_lock<boost::mutex> scoped_lock(r_mutex);
                        buffer_reserved = 0;
                        reserved_pieces.reset();
                        for (int i = 0; i < pieces.size(); i++) {
                                reserved_pieces.set(pieces[i]);
                                buffer_reserved++;
                        };
                };

                bool is_reserved(int index) {
                        if (!is_initialized) return false;

                        return reserved_pieces.test(index);
                };

                bool is_readered(int index) {
                        if (!is_initialized) return false;

                        if (!m_handle) {
                                std::cerr << "INFO no handle" << std::endl;
                                return true;
                        }

                        return m_handle->piece_priority(index) != 0;
                        // return reader_pieces.test(index);
                };
        };

        storage_interface* memory_storage_constructor(storage_params const& params)
        {
                return new memory_storage(params);
        };
}

#endif // TORRENT_MEMORY_STORAGE_HPP_INCLUDED


#include "TCPServer.hpp"
#include "TCPClient.hpp"
#include <boost/thread/thread.hpp>
#include "ParamGen.hpp"

int main() {
    // generateParams();
    std::cout << "Generated params." << std::endl;

    auto ecg = GenerateECGroup();
    auto gen = GenerateECBase();
    PublicData pub(ecg, 10, 5);
    PrivateData priv(5);
    VoteData data(ecg, gen, pub, priv);

    boost::asio::io_context sio;
    boost::asio::io_context cio;
    TCPServer ser(data, sio);
    TCPClient cli(data, cio);
    boost::thread t1(boost::bind(&boost::asio::io_context::run, &sio));
    boost::thread t2(boost::bind(&boost::asio::io_context::run, &cio));
    t1.join();
    t2.join();

    return 0;
}


/*

Copyright (c) 2007-2016, Un Shyam & Arvid Norberg
All rights reserved.

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

    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in
      the documentation and/or other materials provided with the distribution.
    * Neither the name of the author 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 OWNER 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.

*/

#if !defined(TORRENT_DISABLE_ENCRYPTION) && !defined(TORRENT_DISABLE_EXTENSIONS)

#ifndef TORRENT_PE_CRYPTO_HPP_INCLUDED
#define TORRENT_PE_CRYPTO_HPP_INCLUDED

#include "libtorrent/config.hpp"

#include "libtorrent/aux_/disable_warnings_push.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include "libtorrent/aux_/disable_warnings_pop.hpp"

#include "libtorrent/receive_buffer.hpp"
#include "libtorrent/sha1_hash.hpp"
#include "libtorrent/extensions.hpp"
#include "libtorrent/assert.hpp"
#include "libtorrent/span.hpp"
#include "libtorrent/buffer.hpp"
#include "libtorrent/aux_/array.hpp"

#include <list>
#include <array>
#include <cstdint>

namespace libtorrent {

	namespace mp = boost::multiprecision;

	using key_t = mp::number<mp::cpp_int_backend<768, 768, mp::unsigned_magnitude, mp::unchecked, void>>;

	TORRENT_EXTRA_EXPORT std::array<char, 96> export_key(key_t const& k);

	// RC4 state from libtomcrypt
	struct rc4 {
		int x;
		int y;
		aux::array<std::uint8_t, 256> buf;
	};

	// TODO: 3 dh_key_exchange should probably move into its own file
	class TORRENT_EXTRA_EXPORT dh_key_exchange
	{
	public:
		dh_key_exchange();
		bool good() const { return true; }

		// Get local public key
		key_t const& get_local_key() const { return m_dh_local_key; }

		// read remote_pubkey, generate and store shared secret in
		// m_dh_shared_secret.
		void compute_secret(std::uint8_t const* remote_pubkey);
		void compute_secret(key_t const& remote_pubkey);

		key_t const& get_secret() const { return m_dh_shared_secret; }

		sha1_hash const& get_hash_xor_mask() const { return m_xor_mask; }

	private:

		key_t m_dh_local_key;
		key_t m_dh_local_secret;
		key_t m_dh_shared_secret;
		sha1_hash m_xor_mask;
	};

	struct TORRENT_EXTRA_EXPORT encryption_handler
	{
		std::tuple<int, span<span<char const>>>
		encrypt(span<span<char>> iovec);

		int decrypt(crypto_receive_buffer& recv_buffer
			, std::size_t& bytes_transferred);

		bool switch_send_crypto(std::shared_ptr<crypto_plugin> crypto
			, int pending_encryption);

		void switch_recv_crypto(std::shared_ptr<crypto_plugin> crypto
			, crypto_receive_buffer& recv_buffer);

		bool is_send_plaintext() const
		{
			return m_send_barriers.empty() || m_send_barriers.back().next != INT_MAX;
		}

		bool is_recv_plaintext() const
		{
			return m_dec_handler.get() == nullptr;
		}

	private:
		struct barrier
		{
			barrier(std::shared_ptr<crypto_plugin> plugin, int n)
				: enc_handler(plugin), next(n) {}
			std::shared_ptr<crypto_plugin> enc_handler;
			// number of bytes to next barrier
			int next;
		};
		std::list<barrier> m_send_barriers;
		std::shared_ptr<crypto_plugin> m_dec_handler;
	};

	struct TORRENT_EXTRA_EXPORT rc4_handler : crypto_plugin
	{
	public:
		rc4_handler();

		// Input keys must be 20 bytes
		void set_incoming_key(span<char const> key) override;
		void set_outgoing_key(span<char const> key) override;

		std::tuple<int, span<span<char const>>>
		encrypt(span<span<char>> buf) override;

		std::tuple<int, int, int> decrypt(span<span<char>> buf) override;

	private:
		rc4 m_rc4_incoming;
		rc4 m_rc4_outgoing;

		// determines whether or not encryption and decryption is enabled
		bool m_encrypt;
		bool m_decrypt;
	};

} // namespace libtorrent

#endif // TORRENT_PE_CRYPTO_HPP_INCLUDED
#endif // TORRENT_DISABLE_ENCRYPTION


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX_FUNCTION_BROADCAST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_FUNCTION_BROADCAST_HPP_INCLUDED

#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/dispatch/adapted/std/integral_constant.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  template<typename A0, typename A1>
  struct bc_helper
  {
    using select_t  = brigand::bool_<(A1::value >= 0 && A1::value < A0::static_size)>;
    using sel_t     = std::integral_constant<int, (A1::value >= A0::static_size/2) ? 1 : 0>;
    using idx_t     = std::integral_constant<int, A1::value - sel_t::value*(A0::static_size/2)>;
    using type      = typename A0::template resize<A0::static_size/2>;
  };

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::avx_
                          , bs::pack_<bd::single_<A0>,bs::avx_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,typename bc_helper<A0,A1>::select_t{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      using sel_t = typename bc_helper<A0,A1>::sel_t;
      using idx_t = typename bc_helper<A0,A1>::idx_t;
      using h_t   = typename bc_helper<A0,A1>::type;

      h_t half =  _mm256_extractf128_ps(a0, sel_t::value);
      return      _mm256_broadcast_ps(&broadcast<idx_t::value>(half).storage());
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::avx_
                          , bs::pack_<bd::double_<A0>,bs::avx_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,typename bc_helper<A0,A1>::select_t{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      using sel_t = typename bc_helper<A0,A1>::sel_t;
      using idx_t = typename bc_helper<A0,A1>::idx_t;
      using h_t   = typename bc_helper<A0,A1>::type;

      h_t half =  _mm256_extractf128_pd(a0, sel_t::value);
      return      _mm256_broadcast_pd(&broadcast<idx_t::value>(half).storage());
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };
} } }

#endif


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     hypothesis_factor_graph.cpp
 * \author   Collin Johnson
 *
 * Definition of convert_hypothesis_graph_to_factor_graph.
 */

#include "hssh/local_topological/area_detection/labeling/hypothesis_factor_graph.h"
#include "hssh/local_topological/area_detection/labeling/boundary.h"
#include "hssh/local_topological/area_detection/labeling/boundary_classifier.h"
#include "hssh/local_topological/area_detection/labeling/hypothesis.h"
#include "hssh/local_topological/area_detection/labeling/hypothesis_classifier.h"
#include <boost/range/iterator_range.hpp>

namespace vulcan
{
namespace hssh
{

using VarVec = std::vector<Variable::Ptr>;
using FactVec = std::vector<Factor::Ptr>;
using EdgeVec = std::vector<FactorEdge::Ptr>;

struct ConstructionState
{
    int nextId = 0;
    VarVec vars;
    FactVec factors;
    EdgeVec edges;
    std::unordered_map<int, AreaHypothesisBoundary*> varToBoundary;
    std::unordered_map<int, AreaHypothesis*> varToHyp;
    std::unordered_map<AreaHypothesis*, Variable*> hypToVar;
    std::unordered_map<AreaHypothesisBoundary*, Variable*> bndToVar;
};

void add_area_variable(AreaHypothesis* hyp, const HypothesisClassifier& classifier, ConstructionState& state);
void add_gateway_variable(AreaHypothesisBoundary* boundary, ConstructionState& state);
void add_boundary_factor(AreaHypothesisBoundary* boundary,
                         const BoundaryClassifier& classifier,
                         ConstructionState& state);

HypothesisFactorGraph convert_hypothesis_graph_to_factor_graph(HypothesisGraph& hypGraph,
                                                               const HypothesisClassifier& hypClassifier,
                                                               const BoundaryClassifier& bndClassifier)
{
    ConstructionState state;

    // Create a factor for the type distribution for each area
    for (AreaHypothesis* hyp : boost::make_iterator_range(hypGraph.beginHypothesis(), hypGraph.endHypothesis())) {
        add_area_variable(hyp, hypClassifier, state);
    }

    // Create a factor for the gateway probability for each boundary
    for (AreaHypothesisBoundary* bnd : boost::make_iterator_range(hypGraph.beginBoundary(), hypGraph.endBoundary())) {
        add_gateway_variable(bnd, state);
    }

    // Create a factor for the type distribution for each boundary and its bounding areas
    for (AreaHypothesisBoundary* bnd : boost::make_iterator_range(hypGraph.beginBoundary(), hypGraph.endBoundary())) {
        add_boundary_factor(bnd, bndClassifier, state);
    }

    HypothesisFactorGraph hypFactorGraph;
    hypFactorGraph.graph = FactorGraph(state.vars, state.factors, state.edges);
    hypFactorGraph.varToHyp = state.varToHyp;
    hypFactorGraph.varToBoundary = state.varToBoundary;

    return hypFactorGraph;
}

void add_area_variable(AreaHypothesis* hyp, const HypothesisClassifier& classifier, ConstructionState& state)
{
    int id = state.nextId++;
    state.varToHyp[id] = hyp;

    auto hypVar = std::make_shared<Variable>(id, 3);
    std::vector<Variable*> hypVars;
    hypVars.push_back(hypVar.get());

    auto dist = classifier.calculateDistribution(hyp->features());

    std::vector<FactorStateProb> hypProbs;
    FactorStateProb pathProb;
    pathProb.states.emplace_back(id, kPathIdx);
    pathProb.prob = dist.path;
    hypProbs.push_back(pathProb);

    FactorStateProb decisionProb;
    decisionProb.states.emplace_back(id, kDecisionIdx);
    decisionProb.prob = dist.decision;
    hypProbs.push_back(decisionProb);

    FactorStateProb destProb;
    destProb.states.emplace_back(id, kDestIdx);
    destProb.prob = dist.destination;
    hypProbs.push_back(destProb);

    auto hypFactor = std::make_shared<Factor>(state.nextId++, hypVars, hypProbs);
    auto hypEdge = std::make_shared<FactorEdge>(hypVar->id(), hypFactor->id(), 3);
    hypVar->addEdge(hypEdge.get());
    hypFactor->addEdge(hypEdge.get());

    state.hypToVar[hyp] = hypVar.get();
    state.vars.push_back(hypVar);
    state.factors.push_back(hypFactor);
    state.edges.push_back(hypEdge);
}

void add_gateway_variable(AreaHypothesisBoundary* boundary, ConstructionState& state)
{
    int id = state.nextId++;
    state.varToBoundary[id] = boundary;

    auto bndVar = std::make_shared<Variable>(id, 2);
    std::vector<Variable*> bndVars;
    bndVars.push_back(bndVar.get());

    std::vector<FactorStateProb> bndProbs;
    FactorStateProb onProb;
    onProb.states.emplace_back(id, 1);
    onProb.prob = boundary->getGateway().probability();
    bndProbs.push_back(onProb);

    FactorStateProb offProb;
    offProb.states.emplace_back(id, 0);
    offProb.prob = 1.0 - onProb.prob;
    bndProbs.push_back(offProb);

    auto bndFactor = std::make_shared<Factor>(state.nextId++, bndVars, bndProbs);
    auto bndEdge = std::make_shared<FactorEdge>(bndVar->id(), bndFactor->id(), 2);
    bndVar->addEdge(bndEdge.get());
    bndFactor->addEdge(bndEdge.get());

    state.bndToVar[boundary] = bndVar.get();
    state.vars.push_back(bndVar);
    state.factors.push_back(bndFactor);
    state.edges.push_back(bndEdge);
}

void add_boundary_factor(AreaHypothesisBoundary* boundary,
                         const BoundaryClassifier& classifier,
                         ConstructionState& state)
{
    auto bndVar = state.bndToVar[boundary];

    std::array<Variable*, 2> hypVars;
    hypVars[0] = state.hypToVar[boundary->getHypotheses()[0]];
    hypVars[1] = state.hypToVar[boundary->getHypotheses()[1]];

    std::vector<Variable*> allVars;
    allVars.push_back(bndVar);
    allVars.push_back(hypVars[0]);
    allVars.push_back(hypVars[1]);

    std::vector<FactorStateProb> bndProbs;
    auto dist = classifier.boundaryDistribution();

    std::array<AreaHypothesis*, 2> hyps;
    hyps[0] = boundary->getHypotheses()[0];
    hyps[1] = boundary->getHypotheses()[1];

    std::array<int, 2> pathIdx;
    for (int n = 0; n < 2; ++n) {
        pathIdx[n] = hyps[n]->isEndGateway(boundary->getGateway().id()) ? kPathEndIdx : kPathIdx;
    }

    // Create a state probability for each combination of path/dest/decision for the areas and on/off for boundary
    for (int i = 0; i < 3; ++i) {
        int idxI = i == kPathIdx ? pathIdx[0] : i;

        for (int j = 0; j < 3; ++j) {
            int idxJ = j == kPathIdx ? pathIdx[1] : j;

            FactorStateProb onProb;
            onProb.states.emplace_back(hypVars[0]->id(), i);
            onProb.states.emplace_back(hypVars[1]->id(), j);
            onProb.states.emplace_back(bndVar->id(), 1);
            onProb.prob = dist.posModel(idxI, idxJ);
            bndProbs.push_back(onProb);

            FactorStateProb offProb;
            offProb.states.emplace_back(hypVars[0]->id(), i);
            offProb.states.emplace_back(hypVars[1]->id(), j);
            offProb.states.emplace_back(bndVar->id(), 0);
            offProb.prob = dist.negModel(idxI, idxJ);
            bndProbs.push_back(offProb);
        }
    }

    auto bndFactor = std::make_shared<Factor>(state.nextId++, allVars, bndProbs);
    auto edge1 = std::make_shared<FactorEdge>(hypVars[0]->id(), bndFactor->id(), 3);
    auto edge2 = std::make_shared<FactorEdge>(hypVars[1]->id(), bndFactor->id(), 3);
    auto edge3 = std::make_shared<FactorEdge>(bndVar->id(), bndFactor->id(), 2);

    bndFactor->addEdge(edge1.get());
    bndFactor->addEdge(edge2.get());
    bndFactor->addEdge(edge3.get());
    hypVars[0]->addEdge(edge1.get());
    hypVars[1]->addEdge(edge2.get());
    bndVar->addEdge(edge3.get());

    state.factors.push_back(bndFactor);
    state.edges.push_back(edge1);
    state.edges.push_back(edge2);
    state.edges.push_back(edge3);
}

}   // namespace hssh
}   // namespace vulcan