#include "m3.h"
#include "global.h"
#include "StreetsDatabaseAPI.h"
#include "OSMDatabaseAPI.h"
#include "m1.h"
#include <math.h>
#include <iostream>
#include <algorithm>
#include <vector>
#include "graphics.h"
#include "iterator"
#include "m1.h"
#include <boost/heap/fibonacci_heap.hpp>
#define likely(x)      __builtin_expect(!!(x), 1)
#define unlikely(x)    __builtin_expect(!!(x), 0)

using namespace std;
const double INF = 99999;
vector<unsigned> reconstruct(unsigned start, unsigned end);
extern global globalVar;

void find_shortest_dij(unsigned start, unsigned end, double penalty);
void get_segment_array();

extern void highlight_intersection(unsigned intersection_id);
unsigned get_turn(unsigned sid1, unsigned sid2);

bool turned(unsigned int1, unsigned int2, unsigned int3);

vector<unsigned> get_poi_from_name(const string name);

unsigned get_dongnanxibei(unsigned sid1, unsigned sid2);

int find_segment_ID_from_intersections(unsigned int1, unsigned int2, int prevStreetID, double turn_penalty);
int find_selected_segment_ID_from_intersections(unsigned int1, unsigned index, int prevStreetID, double turn_penalty);

std::vector<unsigned> Dijkstra(const unsigned intersect_id_start,
        const unsigned intersect_id_end, const double turn_penalty);

double compute_path_travel_time(const std::vector<unsigned>& path,
        const double turn_penalty) {
    // if the route is empty
    if (path.empty() == true)
        return -1;
    double travel_time = 0;
    // loop through all segments and get travel total time
    for (unsigned segment = 0; segment < path.size(); segment++) {
        travel_time += find_street_segment_travel_time(path[segment]);
        if (segment != 0) {
            // add turn penalty
            if (getStreetSegmentInfo(path[segment]).streetID != getStreetSegmentInfo(path[segment - 1]).streetID)
                travel_time += turn_penalty;
        }
    }

    return travel_time;
}

std::vector<unsigned> find_path_between_intersections(const unsigned intersect_id_start,
        const unsigned intersect_id_end, const double turn_penalty) {

    globalVar.naviRoute_seg.clear();

    //to order the intersection and pick the one with smallest
    std::priority_queue<Node, vector<Node>, cmp> openQueue;

    //boost::heap::fibonacci_heap< Node, boost::heap::compare <cmp> > openQueue;

    //Vector to store the current intersection's parent intersection
    std::vector<int> parentInt(getNumberOfIntersections(), -1);

    parentInt[intersect_id_start] = -100;

    std::vector<double> F_Vector(getNumberOfIntersections(), -1);

    std::vector<double> G_Vector(getNumberOfIntersections(), -1);

    G_Vector[intersect_id_start] = 0;

    std::vector<int> parentSeg(getNumberOfIntersections(), -1);

    parentSeg[intersect_id_start] = -100;

    std::vector<bool> closeVector(getNumberOfIntersections(), false);

    //put the starting point position into the open list
    openQueue.push(Node(intersect_id_start, 0));

    while(!openQueue.empty()){

            IntersectionIndex current_intersection_ID = openQueue.top().intersection_ID;

            if(closeVector[current_intersection_ID] == true){
                openQueue.pop();
                continue;
            }

        //end condition, if next point to be explored is the end intersection ID
        if (current_intersection_ID == intersect_id_end) {

            //If the end intersection is found
            int result = intersect_id_end;

            while (parentSeg[result] >= 0) {
                globalVar.naviRoute_seg.insert(globalVar.naviRoute_seg.begin(), parentSeg[result]);
                result = parentInt[result];
            }

            return globalVar.naviRoute_seg;
        }

        //Delete the intersection ID with lowest F
        openQueue.pop();

        //put the intersection into close Vector
        closeVector[current_intersection_ID] = true;

        //highlight_intersection(current_intersection_ID);

         //traverse all the valid intersection
        for (unsigned index = 0; index < globalVar.int_valid_seg[current_intersection_ID].size(); ++index) {

            unsigned selected_segment_ID = globalVar.int_valid_seg[current_intersection_ID][index];

            IntersectionIndex potential_intersection_ID = globalVar.int_intids[current_intersection_ID][index];

            if(closeVector[potential_intersection_ID]){
                continue;
            }

         //value that will be iterated

          //If the current intersection's parent is starting point, then choose the shortest segment,
                //no need to decide turn penalty

          double candidate_G = 0, candidate_F = 0, candidate_H = 0;

          /*******Here is to calculate Heuristic value*/

          double H_landmark_to_end = 0;
          double H_landmark_to_potential = 0;
          double H_potential_to_landmark = 0;
          double H_end_to_landmark = 0;
          double temp_H = 100000;

          //Loop all landmarks to intersection cost
          for(int index = 0; index < globalVar.landmarks_to_int.size(); index++){

               H_landmark_to_potential = globalVar.landmarks_to_int[index][potential_intersection_ID];

               H_landmark_to_end = globalVar.landmarks_to_int[index][intersect_id_end];

               H_potential_to_landmark = globalVar.int_to_landmarks[index][potential_intersection_ID];

               H_end_to_landmark = globalVar.int_to_landmarks[index][intersect_id_end];

               double H1 = H_potential_to_landmark - H_end_to_landmark;

               double H2 = H_landmark_to_end - H_landmark_to_potential;

               temp_H = std::max(H1, H2);

               if(temp_H > candidate_H)
                   candidate_H = temp_H;
          }

          //Then it is necessary to calculate turn penalty

          if (parentInt[current_intersection_ID] != -100) {

              candidate_G = G_Vector[current_intersection_ID] + globalVar.ssArray[selected_segment_ID].weight;

              if(globalVar.seg_street_ID[parentSeg[current_intersection_ID]] !=
                      globalVar.int_seg_street_ID[current_intersection_ID][index])
                  candidate_G = candidate_G + turn_penalty;

              candidate_F = candidate_G + candidate_H;
            }

          else{

           selected_segment_ID = globalVar.int_shortest_segment[current_intersection_ID][index];

          candidate_G = G_Vector[current_intersection_ID] + globalVar.ssArray[selected_segment_ID].weight;

          candidate_F = candidate_G + candidate_H;
          }

          if (candidate_F >= F_Vector[potential_intersection_ID] && F_Vector[potential_intersection_ID] != -1)
                    continue;

          openQueue.push(Node(potential_intersection_ID, candidate_F));

          //Finally store all the iterative values
          parentInt[potential_intersection_ID] = current_intersection_ID;
          parentSeg[potential_intersection_ID] = selected_segment_ID;
          G_Vector[potential_intersection_ID] = candidate_G;
          F_Vector[potential_intersection_ID] = candidate_F;

        }
    }

        std::vector<unsigned> no_path;
        return no_path;
}

void get_segment_array() {
    globalVar.dongnanxibei.clear();
    globalVar.turn_by_turn.clear();

    if (globalVar.naviRoute_seg.size() == 0) return;

    // loop along all the segments to get navigation information
    for (unsigned i = 0; i < globalVar.naviRoute_seg.size() - 1; i++) {
        //To record turn information
        if (getStreetSegmentInfo(globalVar.naviRoute_seg[i]).streetID !=
                getStreetSegmentInfo(globalVar.naviRoute_seg[i + 1]).streetID) {
            globalVar.turn_by_turn.push_back(get_turn(globalVar.naviRoute_seg[i],
                    globalVar.naviRoute_seg[i + 1]));

        } else {
            globalVar.turn_by_turn.push_back(0);

        }
        globalVar.dongnanxibei.push_back(get_dongnanxibei(globalVar.naviRoute_seg[i], globalVar.naviRoute_seg[i + 1]));

    }
    globalVar.turn_by_turn.push_back(0);
    if (globalVar.naviRoute_seg.size() == 1)
        globalVar.dongnanxibei.push_back(get_dongnanxibei(globalVar.naviRoute_seg[0], globalVar.naviRoute_seg[0]));
    globalVar.dongnanxibei.push_back(globalVar.dongnanxibei[globalVar.dongnanxibei.size()-1]);
}

// function to calculate the turn direction of a turn

unsigned get_turn(unsigned sid1, unsigned sid2) {
    unsigned id1 = 0, id2 = 0, id3 = 0;
    // conversion from 2 segments to 3 points
    if (getStreetSegmentInfo(sid1).from == getStreetSegmentInfo(sid2).to ||
            getStreetSegmentInfo(sid1).from == getStreetSegmentInfo(sid2).from) {
        id1 = getStreetSegmentInfo(sid1).to;
        id2 = getStreetSegmentInfo(sid1).from;
        if (id2 == getStreetSegmentInfo(sid2).from)
            id3 = getStreetSegmentInfo(sid2).to;
        else id3 = getStreetSegmentInfo(sid2).from;
   // Determine ther second point
    } else if (getStreetSegmentInfo(sid1).to == getStreetSegmentInfo(sid2).to ||
            getStreetSegmentInfo(sid1).to == getStreetSegmentInfo(sid2).from) {
        id1 = getStreetSegmentInfo(sid1).from;
        id2 = getStreetSegmentInfo(sid1).to;
        if (id2 == getStreetSegmentInfo(sid2).from)
            id3 = getStreetSegmentInfo(sid2).to;
        else id3 = getStreetSegmentInfo(sid2).from;

    }

    t_point p1, p2, p3;
    double y_calc;
    double slope = 0;
    p1 = globalVar.LatLon_to_real_XY2(getIntersectionPosition(id1));
    p2 = globalVar.LatLon_to_real_XY2(getIntersectionPosition(id2));
    p3 = globalVar.LatLon_to_real_XY2(getIntersectionPosition(id3));

    // use slope and expect growth to calculate turn
    if (p2.x == p1.x)
        slope = (p2.y - p1.y) * 999999;
    else
        slope = (p2.y - p1.y) / (p2.x - p1.x);

    y_calc = p2.y + slope * (p3.x - p2.x);
    if (p2.x >= p1.x) {
        //        if (slope > 0) {
        if (p3.y > y_calc)
            return 1;
        else if (p3.y < y_calc) return 2;
        else return 0;
    // If x1 is > x2
    } else {
        //        if (slope > 0) {
        if (p3.y > y_calc)
            return 2;
        else if (p3.y < y_calc) return 1;
        else return 0;
    }
    return 0;
}

// get heading direction for turn by turn navigation

unsigned get_dongnanxibei(unsigned sid1, unsigned sid2) {
    // conversion from 2 segments to 3 intersections

    unsigned id2 = 0;
    unsigned id3 = 0;

    if(sid2 != sid1){
    if (getStreetSegmentInfo(sid1).from == getStreetSegmentInfo(sid2).to ||
            getStreetSegmentInfo(sid1).from == getStreetSegmentInfo(sid2).from) {

        id2 = getStreetSegmentInfo(sid1).from;
        if (id2 == getStreetSegmentInfo(sid2).from)
            id3 = getStreetSegmentInfo(sid2).to;
        else id3 = getStreetSegmentInfo(sid2).from;

    } else if (getStreetSegmentInfo(sid1).to == getStreetSegmentInfo(sid2).to ||
            getStreetSegmentInfo(sid1).to == getStreetSegmentInfo(sid2).from) {

        id2 = getStreetSegmentInfo(sid1).to;
        if (id2 == getStreetSegmentInfo(sid2).from)
            id3 = getStreetSegmentInfo(sid2).to;
        else id3 = getStreetSegmentInfo(sid2).from;

    }
    }

    else{

        id2 = globalVar.start_int;
        id3 = globalVar.end_point;

    }

    t_point p2 = globalVar.LatLon_to_real_XY2(getIntersectionPosition(id2));
    t_point p3 = globalVar.LatLon_to_real_XY2(getIntersectionPosition(id3));

    // Use slope to calculate direction
    double slope = 0;
    if (p3.x > p2.x) { // east side
        slope = (p3.y - p2.y) / (p3.x - p2.x);
        if (slope >= 1.732)
            return 0; //north
        else if (slope < 1.732 && slope >= 0.577)
            return 1; // north-east
        else if (slope < 0.577 && slope >= 0)
            return 2; // east
        else if (slope < 0 && slope >= -0.577)
            return 2; // east
        else if (slope < -0.577 && slope >= -1.732)
            return 3; // south-east
        else if (slope < -1.732)
            return 4; //south

      // if its going west worlds
    } else if (p3.x < p2.x) { // west side
        slope = (p3.y - p2.y) / (p3.x - p2.x);
        if (slope >= 1.732)
            return 4; //south
        else if (slope < 1.732 && slope >= 0.577)
            return 5; // south-west
        else if (slope < 0.577 && slope >= 0)
            return 6; // west
        else if (slope < 0 && slope >= -0.577)
            return 6; // west
        else if (slope < -0.577 && slope >= -1.732)
            return 7; // north-west
        else if (slope < -1.732)
            return 0; // north
    } else if (p3.y > p2.y)
        return 0;
    else return 4;

    return 0;

}

// Get a POI's nearest nearest intersection from poi id

vector<unsigned> get_poi_from_name(const string name) {

    vector<unsigned> tempID;
    globalVar.find_interest.clear();
    multimap<string, vector<unsigned>>::iterator start, end, it;
    start = globalVar.POIName.lower_bound(name);
    end = globalVar.POIName.upper_bound(name);
    for (it = start; it != end; it++) {
        tempID.push_back(it->second[1]);
        globalVar.find_interest.push_back(it -> second[0]);

    }
    return tempID;
}

// reconstruct turn-by-turn navigation information to prevent large quantities of go straight

void navi_info_reconstruct() {
    unsigned i = 0;
    globalVar.draw_turn.clear();
    globalVar.draw_dongnanxibei.clear();
    globalVar.draw_street_seg.clear();
    globalVar.move_distance.clear();
    double length = 0;
    while (i < globalVar.turn_by_turn.size()) {
        unsigned k = 0;
        // put the straight segments together
        if (globalVar.turn_by_turn[i] != 0) {
            globalVar.draw_turn.push_back(globalVar.turn_by_turn[i]);
            globalVar.draw_street_seg.push_back(globalVar.naviRoute_seg[i+1]);
            globalVar.draw_dongnanxibei.push_back(globalVar.dongnanxibei[i]);
            length += (globalVar.ssArray[globalVar.naviRoute_seg[i]]).length;
            globalVar.move_distance.push_back(length);
            //cout << "turn:  " << globalVar.turn_by_turn[i]
            //      << endl;
            i = i + 1;
            length = 0;
        } else {

            while (globalVar.turn_by_turn[i + k] == 0 && (i+k)< globalVar.turn_by_turn.size()) {
                length = length + (globalVar.ssArray[globalVar.naviRoute_seg[i + k]]).length;
                k = k + 1;
    }
            i = i + k;
}

    }
    globalVar.draw_turn.push_back(0);
    globalVar.move_distance.push_back(length);
    globalVar.draw_dongnanxibei.push_back(globalVar.dongnanxibei[globalVar.dongnanxibei.size() - 1]);
    globalVar.draw_street_seg.push_back(globalVar.naviRoute_seg[globalVar.naviRoute_seg.size()-1]);

}


/*
 * Copyright 2018,
 * Julian Viereck
 *
 * CNRS/AIST
 *
 */

#include <boost/function.hpp>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>

#include <sot/core/factory.hh>
#include <sot/core/gradient-ascent.hh>

namespace dg = ::dynamicgraph;

/* ---------------------------------------------------------------------------*/
/* ------- GENERIC HELPERS -------------------------------------------------- */
/* ---------------------------------------------------------------------------*/

namespace dynamicgraph {
namespace sot {

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GradientAscent, "GradientAscent");

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

GradientAscent::GradientAscent(const std::string &n)
    : Entity(n),
      gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient"),
      learningRateSIN(NULL,
                      "GradientAscent(" + n + ")::input(double)::learningRate"),
      refresherSINTERN("GradientAscent(" + n + ")::intern(dummy)::refresher"),
      valueSOUT(boost::bind(&GradientAscent::update, this, _1, _2),
                gradientSIN << refresherSINTERN,
                "GradientAscent(" + n + ")::output(vector)::value"),
      init(false) {
  // Register signals into the entity.
  signalRegistration(gradientSIN << learningRateSIN << valueSOUT);
  refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY);
}

GradientAscent::~GradientAscent() {}

/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */

dynamicgraph::Vector &GradientAscent::update(dynamicgraph::Vector &res,
                                             const int &inTime) {
  const dynamicgraph::Vector &gradient = gradientSIN(inTime);
  const double &learningRate = learningRateSIN(inTime);

  if (init == false) {
    init = true;
    value = gradient;
    value.setZero();
    res.resize(value.size());
  }

  value += learningRate * gradient;
  res = value;
  return res;
}

} /* namespace sot */
} /* namespace dynamicgraph */


#ifndef CBCRECOURSE_HPP
#define CBCRECOURSE_HPP

#include "RecourseSubproblemInterface.hpp"
#include "OsiClpSolverInterface.hpp"
#include "CbcModel.hpp"
#include <boost/scoped_ptr.hpp>

class CbcRecourseSolver : public RecourseSubproblemInterface {
public:
	CbcRecourseSolver(stochasticInput &input, int scenarioNumber, const std::vector<double> &firstStageSolution);

	void go();
	double getObjective() const;
	solverState getStatus() const;

	// for setting states to warm-start the solve
	void setSecondStageColState(int idx,variableState);
	void setSecondStageRowState(int idx,variableState);
	void commitStates() {}

	variableState getSecondStageColState(int idx) const;
	variableState getSecondStageRowState(int idx) const;

	void setDualObjectiveLimit(double d);

protected:
	OsiClpSolverInterface model;
	boost::scoped_ptr<CbcModel> cbcm;
};

#endif


 /*
landmarks in images read from a webcam and points that drew by the program.
 */
#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <stdio.h>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace dlib;

void print_usage() {
    std::cout << "Usage:" << std::endl;
    std::cout << "./face_landmark_detection [path/to/shape_predictor_68_face_landmarks.dat]" << std::endl;
}

// Shows the results that for Optical Flow, Kalman Filter based on dlib separately
int main(int argc, char** argv) {
    try {
        if (argc > 2) {
            print_usage();
            return 0;
        }

        std::cout << "Press q to exit." << std::endl;

        cv::VideoCapture cap(0);

        if (!cap.isOpened()) {
            std::cerr << "Unable to connect to camera" << std::endl;
            return 1;
        }

        // Load face detection and pose estimation models.
        frontal_face_detector detector = get_frontal_face_detector();
        shape_predictor pose_model;

        if (argc == 2) {
            deserialize(argv[1]) >> pose_model;
        } else {
            deserialize("../../data/shape_predictor_68_face_landmarks.dat") >> pose_model;
        }

        // Initialize the points of last frame
        std::vector<cv::Point2f> last_object;
        for (int i = 0; i < 68; ++i) {
            last_object.push_back(cv::Point2f(0.0, 0.0));
        }

        double scaling = 0.5;
        int flag = -1;
        int count = 0;

        // Initialize measurement points
        std::vector<cv::Point2f> kalman_points;
        for (int i = 0; i < 68; i++) {
            kalman_points.push_back(cv::Point2f(0.0, 0.0));
        }

        // Initialize prediction points
        std::vector<cv::Point2f> predict_points;
        for (int i = 0; i < 68; i++) {
            predict_points.push_back(cv::Point2f(0.0, 0.0));
        }

        // Kalman Filter Setup (68 Points Test)
        const int stateNum = 272;
        const int measureNum = 136;

        KalmanFilter KF(stateNum, measureNum, 0);
        Mat state(stateNum, 1, CV_32FC1);
        Mat processNoise(stateNum, 1, CV_32F);
        Mat measurement = Mat::zeros(measureNum, 1, CV_32F);

        // Generate a matrix randomly
        randn(state, Scalar::all(0), Scalar::all(0.0));

        // Generate the Measurement Matrix
        KF.transitionMatrix = Mat::zeros(272, 272, CV_32F);
        for (int i = 0; i < 272; i++) {
            for (int j = 0; j < 272; j++) {
                if (i == j || (j - 136) == i) {
                    KF.transitionMatrix.at<float>(i, j) = 1.0;
                } else {
                    KF.transitionMatrix.at<float>(i, j) = 0.0;
                }
            }
        }

        //!< measurement matrix (H) Measurement Model
        setIdentity(KF.measurementMatrix);

        //!< process noise covariance matrix (Q)
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));

        //!< measurement noise covariance matrix (R)
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));

        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  A represents F: transitionMatrix
        setIdentity(KF.errorCovPost, Scalar::all(1));

        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

        cv::Mat prevgray, gray;

        std::vector<cv::Point2f> prevTrackPts;
        std::vector<cv::Point2f> nextTrackPts;
        for (int i = 0; i < 68; i++) {
            prevTrackPts.push_back(cv::Point2f(0, 0));
        }

        // Grab and process frames until the main window is closed by the user.
        while(true) {
            // Grab a frame
            cv::Mat raw;
            cap >> raw;

            // Resize
            cv::Mat tmp;
            cv::resize(raw, tmp, cv::Size(), scaling, scaling);

            //Flip
            cv::Mat temp;
            cv::flip(tmp, temp, 1);

            // Turn OpenCV's Mat into something dlib can deal with.  Note that this just
            // wraps the Mat object, it doesn't copy anything.  So cimg is only valid as
            // long as temp is valid.  Also don't do anything to temp that would cause it
            // to reallocate the memory which stores the image as that will make cimg
            // contain dangling pointers.  This basically means you shouldn't modify temp
            // while using cimg.
            cv_image<bgr_pixel> cimg(temp);

            // Detect faces, load the vertexes as vector
            std::vector<dlib::rectangle> faces = detector(cimg);

            // Find the pose of each face.
            std::vector<full_object_detection> shapes;
            for (unsigned long i = 0; i < faces.size(); ++i) {
                shapes.push_back(pose_model(cimg, faces[i]));
            }

            // We cannot modify temp so we clone a new one
            cv::Mat face = temp.clone();
            cv::Mat face_2 = temp.clone();
            cv::Mat face_3 = temp.clone();
            cv::Mat frame = temp.clone();

            if (count == 10) {
                if (shapes.size() == 1) {
                    const full_object_detection& d = shapes[0];
                    for (int i = 0; i < d.num_parts(); i++) {
                        prevTrackPts[i].x = d.part(i).x();
                        prevTrackPts[i].y = d.part(i).y();
                    }
                }
                count = 0;
            } else {
                count += 1;
            }

            // Optical Flow Detection
            cvtColor(frame, gray, CV_BGR2GRAY);
            if (prevgray.data && !prevTrackPts.empty()) {
                std::vector<uchar> status;
                std::vector<float> err;
                calcOpticalFlowPyrLK(prevgray, gray, prevTrackPts, nextTrackPts, status, err);
                for (int i = 0; i < prevTrackPts.size(); i++) {
                    cv::circle(frame, prevTrackPts[i], 2, cv::Scalar(0, 0, 255), -1);
                }
            }
            std::swap(prevTrackPts, nextTrackPts);
            std::swap(prevgray, gray);

            cv::imshow("OpticalFlow", frame);

            // Simple Filter
            if (shapes.size() == 1) {
                const full_object_detection& d = shapes[0];
                if (flag == -1) {
                    for (int i = 0; i < d.num_parts(); i++) {
                        cv::circle(face, cv::Point(d.part(i).x(), d.part(i).y()), 2, cv::Scalar(0, 0, 255), -1);
                        std::cout << i << ": " << d.part(i) << std::endl;
                    }
                    flag = 1;
                } else {
                     for (int i = 0; i < d.num_parts(); i++) {
                        cv::circle(face, cv::Point2f(d.part(i).x() * 0.5 + last_object[i].x * 0.5, d.part(i).y() * 0.5 + last_object[i].y * 0.5), 2, cv::Scalar(0, 0, 255), -1);
                        std::cout << i << ": " << d.part(i) << std::endl;
                    }
                }
                for (int i = 0; i < d.num_parts(); i++) {
                    last_object[i].x = d.part(i).x();
                    last_object[i].y = d.part(i).y();
                }
            }

            // No Filter
            if (shapes.size() == 1) {
                const full_object_detection& d = shapes[0];
                for (int i = 0; i < d.num_parts(); i++) {
                    cv::circle(face_2, cv::Point2f(int(d.part(i).x()), int(d.part(i).y())), 2, cv::Scalar(0, 255, 255), -1);
                    std::cout << i << ": " << d.part(i) << std::endl;
                }
                for (int i = 0; i < d.num_parts(); i++) {
                    kalman_points[i].x = d.part(i).x();
                    kalman_points[i].y = d.part(i).y();
                }
            }

            // Kalman Prediction
            Mat prediction = KF.predict();
            for (int i = 0; i < 68; i++) {
                predict_points[i].x = prediction.at<float>(i * 2);
                predict_points[i].y = prediction.at<float>(i * 2 + 1);
            }

            // Update Measurement
            for (int i = 0; i < 136; i++) {
                if (i % 2 == 0) {
                    measurement.at<float>(i) = (float)kalman_points[i / 2].x;
                } else {
                    measurement.at<float>(i) = (float)kalman_points[(i - 1) / 2].y;
                }
            }

            measurement += KF.measurementMatrix * state;

            // Correct Measurement
            KF.correct(measurement);

            // Show 68-points utilizing kalman filter
            for (int i = 0; i < 68; i++) {
                cv::circle(face_3, predict_points[i], 2, cv::Scalar(255, 0, 0), -1);
            }

            // Display the frame with landmarks
            cv::imshow("Mean Filter", face);
            cv::imshow("No Filter", face_2);
            cv::imshow("Kalman Filter", face_3);

            char key = cv::waitKey(1);
            if (key == 'q') {
                break;
            }
        }
    } catch(serialization_error& e) {
        print_usage();
    } catch(std::exception& e) {
        std::cout << e.what() << std::endl;
    }
    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/kernel.hpp>
#include <boost/compute/program.hpp>
#include <boost/compute/source.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/container/vector.hpp>

namespace compute = boost::compute;

// this example shows how to use the static c++ kernel language
// extension (currently only supported by AMD) to compile and
// execute a templated c++ kernel.
// Using platform vendor info to decide if this is AMD platform
int main()
{
    // get default device and setup context
    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);
    compute::platform platform(device.get_info<CL_DEVICE_PLATFORM>());

    // check the platform vendor string
    if(platform.vendor() != "Advanced Micro Devices, Inc."){
        std::cerr << "error: static C++ kernel language is only "
                  << "supported on AMD devices."
                  << std::endl;
        return 0;
    }

    // create input int values and copy them to the device
    int int_data[] = { 1, 2, 3, 4};
    compute::vector<int> int_vector(int_data, int_data + 4, queue);

    // create input float values and copy them to the device
    float float_data[] = { 2.0f, 4.0f, 6.0f, 8.0f };
    compute::vector<float> float_vector(float_data, float_data + 4, queue);

    // create kernel source with a templated function and templated kernel
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        // define our templated function which returns the square of its input
        template<typename T>
        inline T square(const T x)
        {
            return x * x;
        }

        // define our templated kernel which calls square on each value in data
        template<typename T>
        __kernel void square_kernel(__global T *data)
        {
            const uint i = get_global_id(0);
            data[i] = square(data[i]);
        }

        // explicitly instantiate the square kernel for int's. this allows
        // for it to be called from the host with the given mangled name.
        template __attribute__((mangled_name(square_kernel_int)))
        __kernel void square_kernel(__global int *data);

        // also instantiate the square kernel for float's.
        template __attribute__((mangled_name(square_kernel_float)))
        __kernel void square_kernel(__global float *data);
    );

    // build the program. must enable the c++ static kernel language
    // by passing the "-x clc++" compile option.
    compute::program square_program =
        compute::program::build_with_source(source, context, "-x clc++");

    // create the square kernel for int's by using its mangled name declared
    // in the explicit template instantiation.
    compute::kernel square_int_kernel(square_program, "square_kernel_int");
    square_int_kernel.set_arg(0, int_vector);

    // execute the square int kernel
    queue.enqueue_1d_range_kernel(square_int_kernel, 0, int_vector.size(), 4);

    // print out the squared int values
    std::cout << "int's: ";
    compute::copy(
        int_vector.begin(), int_vector.end(),
        std::ostream_iterator<int>(std::cout, " "),
        queue
    );
    std::cout << std::endl;

    // now create the square kernel for float's
    compute::kernel square_float_kernel(square_program, "square_kernel_float");
    square_float_kernel.set_arg(0, float_vector);

    // execute the square int kernel
    queue.enqueue_1d_range_kernel(square_float_kernel, 0, float_vector.size(), 4);

    // print out the squared float values
    std::cout << "float's: ";
    compute::copy(
        float_vector.begin(), float_vector.end(),
        std::ostream_iterator<float>(std::cout, " "),
        queue
    );
    std::cout << std::endl;

    return 0;
}


//==============================================================================
//         Copyright 2014 - 2014 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/minus.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <complex>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/timing.hpp>

using nt2::tag::minus_;

namespace c1
{
  typedef float             sT;
  typedef std::complex<sT>  T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(sT)>::type vcT;

  NT2_TIMING( minus_
            , ((vcT,T(-100,-100),T(100,100)))
              ((vcT,T(-100,-100),T(100,100)))
            )
}

namespace n2
{
  typedef double            sT;
  typedef std::complex<sT>  T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(sT)>::type vcT;

  NT2_TIMING( minus_
            , ((vcT,T(-100,-100),T(100,100)))
              ((vcT,T(-100,-100),T(100,100)))
            )
}


//==================================================================================================
/*!
  @file

  @copyright 2015 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_BOOLEAN_HPP_INCLUDED
#define BOOST_SIMD_BOOLEAN_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-boolean Boolean functions

      Those functions provides scalar and SIMD algorithms for testing values properties.
      Tests like checks for validity, infinity, sign and others are provided. As for @ref
      group-boolean, those functions handles and returns typed logical types.
  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-boolean Boolean Callable Objects
    Callable objects version of @ref group-boolean

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-boolean section.
  **/
} }

#include <boost/simd/function/if_add.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/if_dec.hpp>
#include <boost/simd/function/if_else_allbits.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/ifnotadd.hpp>
#include <boost/simd/function/ifnotdec.hpp>
#include <boost/simd/function/ifnotinc.hpp>
#include <boost/simd/function/ifnotsub.hpp>
#include <boost/simd/function/if_one_else_zero.hpp>
#include <boost/simd/function/if_sub.hpp>
#include <boost/simd/function/if_zero_else_allbits.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/if_zero_else_one.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_andnot.hpp>
#include <boost/simd/function/logical_notand.hpp>
#include <boost/simd/function/logical_notor.hpp>
#include <boost/simd/function/logical_ornot.hpp>
#include <boost/simd/function/logical_xor.hpp>
#include <boost/simd/function/mask2logical.hpp>
#include <boost/simd/function/negif.hpp>
#include <boost/simd/function/negifnot.hpp>
#include <boost/simd/function/seladd.hpp>
#include <boost/simd/function/seldec.hpp>
#include <boost/simd/function/select.hpp>
#include <boost/simd/function/sel.hpp>
#include <boost/simd/function/selinc.hpp>
#include <boost/simd/function/selsub.hpp>
#include <boost/simd/function/where.hpp>

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2012: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_INTERVAL_MAP_HPP_JOFA_080705
#define BOOST_ICL_INTERVAL_MAP_HPP_JOFA_080705

#include <boost/assert.hpp>
#include <boost/icl/type_traits/is_map.hpp>
#include <boost/icl/interval_set.hpp>
#include <boost/icl/interval_base_map.hpp>

namespace boost{namespace icl
{

template<class DomainT, class CodomainT, class Traits,
         ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section,
         ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
class split_interval_map;

/** \brief implements a map as a map of intervals - on insertion
    overlapping intervals are split and associated values are combined.*/
template
<
    typename DomainT,
    typename CodomainT,
    class Traits = icl::partial_absorber,
    ICL_COMPARE Compare  = ICL_COMPARE_INSTANCE(ICL_COMPARE_DEFAULT, DomainT),
    ICL_COMBINE Combine  = ICL_COMBINE_INSTANCE(icl::inplace_plus, CodomainT),
    ICL_SECTION Section  = ICL_SECTION_INSTANCE(icl::inter_section, CodomainT),
    ICL_INTERVAL(ICL_COMPARE) Interval = ICL_INTERVAL_INSTANCE(ICL_INTERVAL_DEFAULT, DomainT, Compare),
    ICL_ALLOC   Alloc    = std::allocator
>
class interval_map:

    public interval_base_map<interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc>,
                             DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc>
{
public:
    typedef Traits traits;
    typedef       interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> type;
    typedef split_interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> split_type;
    typedef type overloadable_type;
    typedef type joint_type;
    typedef interval_base_map<type,
                              DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> base_type;

    typedef ICL_INTERVAL_TYPE(Interval,DomainT,Compare) interval_type;
    typedef typename base_type::iterator      iterator;
    typedef typename base_type::value_type    value_type;
    typedef typename base_type::element_type  element_type;
    typedef typename base_type::segment_type  segment_type;
    typedef typename base_type::domain_type   domain_type;
    typedef typename base_type::codomain_type codomain_type;
    typedef typename base_type::domain_mapping_type domain_mapping_type;
    typedef typename base_type::interval_mapping_type interval_mapping_type;
    typedef typename base_type::ImplMapT ImplMapT;

    typedef typename base_type::size_type     size_type;
    typedef typename base_type::codomain_combine codomain_combine;

    typedef interval_set<DomainT,Compare,Interval,Alloc> interval_set_type;
    typedef interval_set_type set_type;
    typedef set_type          key_object_type;

    enum { fineness = 1 };

public:
    //==========================================================================
    //= Construct, copy, destruct
    //==========================================================================

    /// Default constructor for the empty object
    interval_map(): base_type() {}

    /// Copy constructor
    interval_map(const interval_map& src): base_type(src) {}

    /// Copy constructor for base_type
    template<class SubType>
    explicit interval_map
        (const interval_base_map<SubType,DomainT,CodomainT,
                                 Traits,Compare,Combine,Section,Interval,Alloc>& src)
    { this->assign(src); }

    explicit interval_map(const domain_mapping_type& base_pair): base_type()
    { this->add(base_pair); }

    explicit interval_map(const value_type& value_pair): base_type()
    { this->add(value_pair); }

    /// Assignment operator
    interval_map& operator = (const interval_map& src)
    {
        base_type::operator=(src);
        return *this;
    }

    /// Assignment operator for base type
    template<class SubType>
    interval_map& operator =
        (const interval_base_map<SubType,DomainT,CodomainT,
                                 Traits,Compare,Combine,Section,Interval,Alloc>& src)
    { this->assign(src); return *this; }

    /// Assignment from a base interval_map.
    template<class SubType>
    void assign(const interval_base_map<SubType,DomainT,CodomainT,
                                        Traits,Compare,Combine,Section,Interval,Alloc>& src)
    {
        typedef interval_base_map<SubType,DomainT,CodomainT,
                                  Traits,Compare,Combine,Section,Interval,Alloc> base_map_type;
        this->clear();
        iterator prior_ = this->_map.end();
        ICL_const_FORALL(typename base_map_type, it_, src)
            prior_ = this->add(prior_, *it_);
    }

#   ifndef BOOST_ICL_NO_CXX11_RVALUE_REFERENCES
    //==========================================================================
    //= Move semantics
    //==========================================================================

    /// Move constructor
    interval_map(interval_map&& src)
        : base_type(boost::move(src))
    {}

    /// Move assignment operator
    interval_map& operator = (interval_map&& src)
    {
        base_type::operator=(boost::move(src));
        return *this;
    }

    //==========================================================================
#   endif // BOOST_ICL_NO_CXX11_RVALUE_REFERENCES

private:
    // Private functions that shall be accessible by the baseclass:
    friend class
        interval_base_map <interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc>,
                                        DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc>;

    iterator handle_inserted(iterator it_)
    {
        return segmental::join_neighbours(*this, it_);
    }

    void handle_inserted(iterator prior_, iterator it_)
    {
        if(prior_ != this->_map.end() && segmental::joinable(*this, prior_, it_))
            segmental::join_on_right(*this, prior_, it_);
    }

    template<class Combiner>
    void handle_left_combined(iterator it_)
    {
        if(on_absorbtion<type,Combiner,Traits::absorbs_identities>::is_absorbable((*it_).second))
            this->_map.erase(it_);
        else
            segmental::join_left(*this, it_);
    }

    template<class Combiner>
    void handle_combined(iterator it_)
    {
        if(on_absorbtion<type,Combiner,Traits::absorbs_identities>::is_absorbable((*it_).second))
            this->_map.erase(it_);
        else
            segmental::join_neighbours(*this, it_);
    }

    template<class Combiner>
    void handle_preceeded_combined(iterator prior_, iterator& it_)
    {
        if(on_absorbtion<type,Combiner,Traits::absorbs_identities>::is_absorbable((*it_).second))
        {
            this->_map.erase(it_);
            it_ = prior_;
        }
        else // After a new combination (e.g. combiner=max) joining neighbours may be possible
            segmental::join_neighbours(*this, it_);
    }

    template<class Combiner>
    void handle_succeeded_combined(iterator it_, iterator next_)
    {
        if(on_absorbtion<type,Combiner,Traits::absorbs_identities>::is_absorbable((*it_).second))
        {
            this->_map.erase(it_);
            segmental::join_right(*this, next_);
        }
        else
        {
            segmental::join_left(*this, it_);
            segmental::join_neighbours(*this, next_);
        }
    }

    void handle_reinserted(iterator insertion_)
    {
        segmental::join_right(*this, insertion_);
    }

    template<class Combiner>
    void gap_insert_at(iterator& it_, iterator prior_,
                       const interval_type& end_gap, const codomain_type& co_val)
    {
        if(on_absorbtion<type,Combiner,Traits::absorbs_identities>::is_absorbable((*it_).second))
        {
            this->_map.erase(it_);
            it_ = this->template gap_insert<Combiner>(prior_, end_gap, co_val);
            segmental::join_right(*this, it_);
        }
        else
        {
            segmental::join_left(*this, it_);
            iterator inserted_ = this->template gap_insert<Combiner>(it_, end_gap, co_val);
            it_ = segmental::join_neighbours(*this, inserted_);
        }
    }

} ;

//-----------------------------------------------------------------------------
// type traits
//-----------------------------------------------------------------------------
template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_map<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    typedef is_map<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct has_inverse<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    typedef has_inverse<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = (has_inverse<CodomainT>::value));
};

template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_interval_container<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    typedef is_interval_container<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct absorbs_identities<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    typedef absorbs_identities<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = (Traits::absorbs_identities));
};

template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_total<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    typedef is_total<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = (Traits::is_total));
};

//-----------------------------------------------------------------------------
// type representation
//-----------------------------------------------------------------------------
template <class DomainT, class CodomainT, class Traits, ICL_COMPARE Compare, ICL_COMBINE Combine, ICL_SECTION Section, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct type_to_string<icl::interval_map<DomainT,CodomainT,Traits,Compare,Combine,Section,Interval,Alloc> >
{
    static std::string apply()
    {
        return "itv_map<"+ type_to_string<DomainT>::apply()   + ","
                         + type_to_string<CodomainT>::apply() + ","
                         + type_to_string<Traits>::apply()    + ">";
    }
};

}} // namespace icl boost

#endif


//CXX 11 Required.
//This code is to produce a asscociate.txt
//association2 is used to symchrom the imu from lpms, bluefox matrix version 200w, blue sonar, and the realsense tracking camera.
//synchronize imu, image and sonar depth
//format: time_stamp -> image_dir -> sonar_depth -> qw -> qx -> qy -> qz  -> ax -> ay -> az;
//lpnav: the yaw angle is in degree.

#include <iostream>
#include <string>
#include <fstream>
#include <cstdlib>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <cmath>

using namespace std;
using namespace Eigen;

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

  if (argc!=2)
  {

    cout<<"Usage: Need data_set directory."<<endl;
    return 1;
  }

  string file_dir(argv[1]);

  ifstream file_sonar(file_dir + (string) "/sonar_data/ping_sonar.txt");
  ifstream file_image(file_dir + (string) "/t265_data/images.txt");
  ifstream file_lpimu(file_dir + (string) "/lpimu_data/imu_data.txt");
  //ifstream file_lpnav(file_dir + (string) "/lpnav_data/lp_nav.txt");

  ofstream file_associate(file_dir + (string) "/associate_stereo_no_nav.txt", ios::trunc| ios::out);

  if (!file_lpimu| !file_sonar| !file_image)
  {
    cout<<"Cannot Open files. "<<endl;
    return 0;
  }

  vector<long> imu_time;

  // imu_pose (x, y, z), imu_accel (a_x, a_y, a_z)
  vector<Vector3d> imu_pose, imu_accel;

  // imu_quaternion (x, y, z, w)
  vector<Vector4d> imu_quaternion;

  string time_stamp;

  double x, y, z, a_x, a_y, a_z, q_x, q_y, q_z, q_w;

 cout<<"Start Reading StereoImages"<<endl;

 vector<long> stereo_times;
 vector<string> images_left;
 vector<string> images_right;
 string img_left;
 string img_right;
 bool odd = true;

 while(!file_image.eof()){

	 if(odd){
		 file_image >> time_stamp;
		 file_image >> img_left;
		 int n = img_left.length();
		 char char_array[n+1];
		 strcpy(char_array, img_left.c_str());
		 for(int i =0; i<3; i++){
			 char_array[n-i]=char_array[n-i-1];
		 }
		 char_array[n-3]='.';
		 string name = "";
		 for( int i =0; i<sizeof(char_array); i++){
			 name = name + char_array[i];
		 }

		 stereo_times.push_back(stol(time_stamp));
		 images_left.push_back(name);
		 odd = false;
	 }else{

		file_image >> time_stamp;
		file_image >> img_right;
		images_right.push_back(img_right);
		odd = true;
	 }
 }

 cout<<"Start Reading LPIMU_FILE"<<endl;
 vector<Vector4d> lpimu_quaternion;
 vector<long> lpimu_time;
 vector<Vector3d> lpimu_accel;
 double lq_w, lq_x, lq_y, lq_z, la_x, la_y, la_z;

 while(!file_lpimu.eof()){
   file_lpimu>>time_stamp>>lq_w>>lq_x>>lq_y>>lq_z>>la_x>>la_y>>la_z;
   lpimu_time.push_back(stol(time_stamp));
   lpimu_quaternion.push_back(Vector4d(lq_w, lq_x, lq_y, lq_z));
   lpimu_accel.push_back(Vector3d(la_x, la_y, la_z));
 }

 string depth;
 string dot1;
 string dot2;
 string confidence;
 vector<double> sonar_depth;
 vector<long> sonar_time;

 cout <<"Start Reading SONAR_FILE"<<endl;

 while(!file_sonar.eof()){

   file_sonar>>time_stamp>>depth>>confidence;

   try{
   sonar_time.push_back(stol(time_stamp));
   }catch(...)
   {
	   continue;
   }

   sonar_depth.push_back(stod(depth)/1000);
  }

 cout <<"Start Synchronization...."<<endl;

 cout <<"Image size is: "<<stereo_times.size()<<endl;

 for (int i=50; i<(stereo_times.size()-1); i++)
 {

   cout << "Time Stamp: "<<stereo_times[i]<<endl;

   file_associate << stereo_times[i] <<" "<<images_left[i]<<" "<<images_right[i]<<" ";

   int sonar_diff=10000000;
   int id_sonar=i;

   for (int j=0; j<sonar_time.size(); j++)
   {

      int sonar_diff_prev=sonar_diff;
      sonar_diff = abs(stereo_times[i] - sonar_time[j]);

      if (sonar_diff < sonar_diff_prev)
        id_sonar = j;
   }
   //check sonar.txt last line
   //cout<<"Id of sonar depth is: "<<id_sonar<<endl;
   file_associate << sonar_depth[id_sonar]<<" ";

   int id_lpimu=i;
   int lpimu_diff = 10000000;
   for(int k=0; k <lpimu_time.size(); k++)
   {
     int lpimu_diff_prev=lpimu_diff;

     lpimu_diff = abs(stereo_times[i] - lpimu_time[k]);

     if (lpimu_diff < lpimu_diff_prev)

       id_lpimu = k;

   }

   file_associate << lpimu_quaternion[id_lpimu](0) <<" "<< lpimu_quaternion[id_lpimu](1)<<" "<< lpimu_quaternion[id_lpimu](2) <<" "<< lpimu_quaternion[id_lpimu](3)<<" "<<" "
    <<lpimu_accel[id_lpimu](0)<<" "<<lpimu_accel[id_lpimu](1)<<" "<<lpimu_accel[id_lpimu](2)<<endl;

 }

 cout << "Synchronization Finished. "<<endl;
 return 0;

}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Godlewski
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file iterative_rounding.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-05-06
 */
#ifndef PAAL_ITERATIVE_ROUNDING_HPP
#define PAAL_ITERATIVE_ROUNDING_HPP

#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/lp/glp.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/optional.hpp>

#include <cstdlib>
#include <unordered_map>

namespace paal {
namespace ir {

/**
 * @brief Default Iterative Rounding visitor.
 */
struct trivial_visitor {
    /**
     * @brief Method called after (re)solving the LP.
     */
    template <typename Problem, typename LP>
    void solve_lp(Problem &problem, LP &lp) {}

    /**
     * @brief Method called after rounding a column of the LP.
     */
    template <typename Problem, typename LP>
    void round_col(Problem &problem, LP &lp, lp::col_id col, double val) {}

    /**
     * @brief Method called after relaxing a row of the LP.
     */
    template <typename Problem, typename LP>
    void relax_row(Problem &problem, LP &lp, lp::row_id row) {}
};

///default solve lp for row_generation,
///at first call PRIMAL, and DUAL on the next calls
template <typename Problem, typename LP>
class default_solve_lp_in_row_generation {
    bool m_first;
    LP & m_lp;
public:
    ///constructor
    default_solve_lp_in_row_generation(Problem &, LP & lp) : m_first(true), m_lp(lp) {}

    ///operator()
    lp::problem_type operator()()
    {
        if (m_first) {
            m_first = false;
            return m_lp.solve_simplex(lp::PRIMAL);
        }
        return m_lp.resolve_simplex(lp::DUAL);
    }
};

/// default row_generation for lp,
/// one can customize LP solving, by setting SolveLP
template <template <class, class> class SolveLP = default_solve_lp_in_row_generation>
struct row_generation_solve_lp {
///operator()
template <class Problem, class LP>
    auto operator()(Problem & problem, LP & lp) {
        return row_generation(problem.get_find_violation(lp), SolveLP<Problem, LP>(problem, lp));
    }
};

namespace detail {

    /**
     * @brief This class solves an iterative rounding problem.
 *
 * @tparam Problem
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam LP
 */
template <typename Problem, typename IRcomponents, typename Visitor = trivial_visitor, typename LP = lp::glp>
class iterative_rounding  {
    using RoundedCols = std::unordered_map<lp::col_id, std::pair<double, double>>;

    /**
     * @brief Returns the current value of the LP column.
     */
    double get_val(lp::col_id col) const {
        auto i = m_rounded.find(col);
        if (i == m_rounded.end()) {
            return m_lp.get_col_value(col);
        } else {
            return i->second.first;
        }
    }

  public:
    /**
     * @brief Constructor.
     */
    iterative_rounding(Problem &problem, IRcomponents e,
                       Visitor vis = Visitor())
        : m_ir_components(std::move(e)), m_visitor(std::move(vis)),
          m_problem(problem) {
        call<Init>(m_problem, m_lp);
    }

    /**
     * @brief Finds solution to the LP.
     *
     * @return LP solution status
     */
    lp::problem_type solve_lp() {
        auto prob_type = call<SolveLP>(m_problem, m_lp);
        assert(prob_type != lp::UNDEFINED);
        m_visitor.solve_lp(m_problem, m_lp);
        return prob_type;
    }

    /**
     * @brief Finds solution to the LP.
     *
     * @return LP solution status
     */
    lp::problem_type resolve_lp() {
        auto prob_type = call<ResolveLP>(m_problem, m_lp);
        assert(prob_type != lp::UNDEFINED);
        m_visitor.solve_lp(m_problem, m_lp);
        return prob_type;
    }

    /**
     * @brief Returns the solution cost based on the LP values.
     */
    double get_solution_cost() {
        double sol_cost(0);
        for (auto col : m_lp.get_columns()) {
            sol_cost += m_lp.get_col_value(col) * m_lp.get_col_coef(col);
        }
        for (auto rounded_col : m_rounded) {
            sol_cost += rounded_col.second.first * rounded_col.second.second;
        }
        return sol_cost;
    }

    /**
     * @brief Rounds the LP columns (independently) using the RoundCondition
     * component.
     *
     * @return true iff at least one column was rounded
     */
    bool round() {
        int deleted(0);
        auto &&cols = m_lp.get_columns();
        auto cbegin = std::begin(cols);
        auto cend = std::end(cols);

        while (cbegin != cend) {
            lp::col_id col = *cbegin;
            auto do_round = call<RoundCondition>(m_problem, m_lp, col);
            if (do_round) {
                ++deleted;
                m_rounded.insert(std::make_pair(col,
                    std::make_pair(*do_round, m_lp.get_col_coef(col))));
                m_visitor.round_col(m_problem, m_lp, col, *do_round);
                cbegin = delete_column(cbegin, *do_round);
            }
            else {
                ++cbegin;
            }
        }

        return deleted > 0;
    }

    /**
     * @brief Relaxes the LP rows using the RelaxCondition component.
     *
     * @return true iff at least one row was relaxed
     */
    bool relax() {
        int deleted(0);
        auto &&rows = m_lp.get_rows();
        auto rbegin = std::begin(rows);
        auto rend = std::end(rows);

        while (rbegin != rend) {
            lp::row_id row = *rbegin;
            if (call<RelaxCondition>(m_problem, m_lp, row)) {
                ++deleted;
                m_visitor.relax_row(m_problem, m_lp, row);
                rbegin = m_lp.delete_row(rbegin);
                if (call<RelaxationsLimit>(deleted)) {
                    break;
                }
            } else {
                ++rbegin;
            }
        }

        return deleted > 0;
    }

    /**
     * @brief Returns the LP object used to solve the IR.
     */
    LP &get_lp() { return m_lp; }

    /**
     * @brief Returns the IR components.
     */
    IRcomponents &get_ir_components() { return m_ir_components; }

    /**
     * @brief Sets the solution to the problem using SetSolution component.
     */
    void set_solution() {
        call<SetSolution>(m_problem, boost::bind(&iterative_rounding::get_val,
                                               this, _1));
    }

    /**
     * @brief Rounds the LP using the RoundCondition component.
     */
    void dependent_round() { call<RoundCondition>(m_problem, m_lp); }

    /**
     * @brief Checks if the IR problem has been solved, using the StopCondition
    * component.
     *
     * @return true iff the problem has been solved
     */
    bool stop_condition() { return call<StopCondition>(m_problem, m_lp); }

  private:
    template <typename Action, typename... Args>
    auto call(Args &&... args)
        ->decltype(std::declval<IRcomponents>().template call<Action>(
              std::forward<Args>(args)...)) {
        return m_ir_components.template call<Action>(
            std::forward<Args>(args)...);
    }

    /// Deletes a column from the LP and adjusts the row bounds.
    typename LP::ColIter
    delete_column(typename LP::ColIter col_iter, double value) {
        auto column = m_lp.get_rows_in_column(*col_iter);
        lp::row_id row;
        double coef;
        for (auto const &c : column) {
            boost::tie(row, coef) = c;
            double ub = m_lp.get_row_upper_bound(row);
            double lb = m_lp.get_row_lower_bound(row);
            double diff = coef * value;
            m_lp.set_row_upper_bound(row, ub - diff);
            m_lp.set_row_lower_bound(row, lb - diff);
        }
        return m_lp.delete_col(col_iter);
    };

    LP m_lp;
    IRcomponents m_ir_components;
    Visitor m_visitor;
    utils::compare<double> m_compare;
    RoundedCols m_rounded;
    Problem &m_problem;
};

} //detail

/// Iterative Rounding solution cost type. Solution cost only makes sense if the LP has been solved to optimal value.
using IRSolutionCost = boost::optional<double>;
/// Iterative Rounding result type: Pair consisting of LP problem type and IR
/// solution cost.
using IRResult = std::pair<lp::problem_type, IRSolutionCost>;

/**
 * @brief Solves an Iterative Rounding problem.
 *
 * @tparam Problem
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam LP
 * @param problem IR problem
 * @param components IR problem components
 * @param visitor visitor object used for logging progress of the algoithm
 */
template <typename Problem, typename IRcomponents, typename Visitor = trivial_visitor, typename LP = lp::glp>
IRResult solve_iterative_rounding(Problem & problem, IRcomponents components, Visitor visitor = Visitor()) {
    detail::iterative_rounding<Problem, IRcomponents, Visitor, LP> ir(problem, std::move(components), std::move(visitor));

    auto prob_type = ir.solve_lp();
    if (prob_type != lp::OPTIMAL) {
        return IRResult(prob_type, IRSolutionCost{});
    }

    while (!ir.stop_condition()) {
        bool rounded{ ir.round() };
        bool relaxed{ ir.relax() };
        assert(rounded || relaxed);

        prob_type = ir.resolve_lp();
        if (prob_type != lp::OPTIMAL) {
            return IRResult(prob_type, IRSolutionCost{});
        }
    }
    ir.set_solution();
    return IRResult(lp::OPTIMAL, IRSolutionCost(ir.get_solution_cost()));
}

/**
 * @brief Solves an Iterative Rounding problem with dependent rounding.
 *
 * @tparam Problem
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam LP
 * @param problem IR problem
 * @param components IR problem components
 * @param visitor visitor object used for logging progress of the algoithm
 */
template <typename Problem, typename IRcomponents, typename Visitor = trivial_visitor, typename LP = lp::glp>
IRResult solve_dependent_iterative_rounding(Problem & problem, IRcomponents components, Visitor visitor = Visitor()) {
    detail::iterative_rounding<Problem, IRcomponents, Visitor, LP> ir(problem, std::move(components), std::move(visitor));

    auto prob_type = ir.solve_lp();
    if (prob_type != lp::OPTIMAL) {
        return IRResult(prob_type, IRSolutionCost{});
    }

    while (!ir.stop_condition()) {
        ir.dependent_round();
        ir.relax();

        prob_type = ir.resolve_lp();
        if (prob_type != lp::OPTIMAL) {
            return IRResult(prob_type, IRSolutionCost{});
        }
    }
    ir.set_solution();
    return IRResult(lp::OPTIMAL, IRSolutionCost(ir.get_solution_cost()));
}

} // ir
} // paal

#endif // PAAL_ITERATIVE_ROUNDING_HPP


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;

namespace mp = boost::multiprecision;

int main(int argc, char const* argv[])
{
	mp::cpp_int a = 2;
	mp::cpp_int r = 2;

	while(1){
		cout << a << endl;
		a *= r;
	}

	exit(0);
}


/******************************************************************
 *
 * Round for C++
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <round/common/digest/MD5.h>
#include <round/common/digest/SHA.h>

using namespace Round;

BOOST_AUTO_TEST_SUITE(common)

BOOST_AUTO_TEST_CASE(MD5FuncTest) {
  const std::string TEST_SOURCE = "abcdefghijklmnopqrstuvwxyz";

  std::string hashCode;
  MD5::Hash(TEST_SOURCE, &hashCode);
  BOOST_CHECK_EQUAL(hashCode.length(), MD5::DIGEST_STRING_LENGTH);
}

BOOST_AUTO_TEST_CASE(SHA1FuncTest) {
  const std::string TEST_SOURCE = "abcdefghijklmnopqrstuvwxyz";

  std::string hashCode;
  SHA1::Hash(TEST_SOURCE, &hashCode);
  BOOST_CHECK_EQUAL(hashCode.length(), SHA1::DIGEST_STRING_LENGTH);
}

BOOST_AUTO_TEST_CASE(SHA256FuncTest) {
  const std::string TEST_SOURCE = "abcdefghijklmnopqrstuvwxyz";

  std::string hashCode;
  SHA256::Hash(TEST_SOURCE, &hashCode);
  BOOST_CHECK_EQUAL(hashCode.length(), SHA256::DIGEST_STRING_LENGTH);
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <vector>
#include <string>

#include <random/random.hpp>

#include <sframe/testing_utils.hpp>
#include <util/testing_utils.hpp>
#include <unity/toolkits/ml_data_2/ml_data.hpp>
#include <unity/toolkits/ml_data_2/sframe_index_mapping.hpp>
#include <unity/toolkits/recsys/models.hpp>

using namespace turi;

struct recsys_popularity_test  {
 public:
  void test_popularity() {

    size_t num_observations = 10000;
    size_t num_users = 1000;
    size_t num_items = 10;

    ////////////////////////////////////////////////////////////
    // Build the data
    std::vector<std::vector<flexible_type> > train_data;

    train_data.reserve(num_observations);

    random::seed(0);

    std::set<std::pair<size_t, size_t> > rated_items;

    std::vector<size_t> counts(num_items, 0);
    size_t total_counts = 0;

    // Do one run through with all users and all items;
    for(size_t uid = 0; uid < num_users; ++uid) {
      size_t user = uid;
      size_t item = uid % num_items;
      train_data.push_back( {user, item} );
      rated_items.insert( {user, item} );
      ++counts[item];
      ++total_counts;
    }

    do {
      size_t user = random::fast_uniform<size_t>(0, num_users - 1);
      size_t item = random::fast_uniform<size_t>(0, num_items - 1);

      double accept_prob = 1.0 - double(item) / num_items;

      double r = random::fast_uniform<double>(0.0, 1.0);

      if(r < accept_prob) {
        train_data.push_back( {user, item} );
        rated_items.insert( {user, item} );
        ++counts[item];
        ++total_counts;
      }
    } while(train_data.size() < num_observations);

    sframe data = make_testing_sframe({"user", "item"},
                                      {flex_type_enum::INTEGER, flex_type_enum::INTEGER},
                                      train_data);

    TS_ASSERT_EQUALS(data.size(), num_observations);

    ////////////////////////////////////////////////////////////
    // Get the model

    std::shared_ptr<recsys::recsys_popularity> model(new recsys::recsys_popularity());

    ////////////////////////////////////////////////////////////
    // Set the options

    std::map<std::string, flexible_type> opts;
    opts["item_id"] = "item";
    opts["user_id"] = "user";
    model->init_options(opts);

    ////////////////////////////////////////////////////////////
    // Train the model

    model->setup_and_train(data);

    // Instantiate some alternate versions to make sure the save and load work
    std::vector<std::shared_ptr<recsys::recsys_model_base> > all_models =
        {model,
         std::make_shared<recsys::recsys_popularity>(),
         model};

    ////////////////////////////////////////

    save_and_load_object(*all_models[1], *model);

    ////////////////////////////////////////
    // Include the ones generated from the other models.

    {
      std::map<std::string, flexible_type> mf_opts;
      mf_opts["item_id"] = "item";
      mf_opts["user_id"] = "user";
      mf_opts["num_factors"] = 4;
      mf_opts["max_iterations"] = 5;

      auto model_mf = new recsys::recsys_ranking_factorization_model();
      model_mf->init_options(mf_opts);
      model_mf->setup_and_train(data);
      all_models.push_back(model_mf->get_popularity_baseline());
    }

    {
      auto model_item_sim = new recsys::recsys_itemcf();
      model_item_sim->init_options(opts);
      model_item_sim->setup_and_train(data);
      all_models.push_back(model_item_sim->get_popularity_baseline());
    }

    ////////////////////////////////////////////////////////////
    // get a set of predictions; check all the models

    std::vector<std::vector<flexible_type> > pred_loc;

    // Here, this model completely ignores users, so it's sufficient
    // to just test all our operations on a candidate user; in this
    // case "0".
    for(size_t m = 0; m < num_items; ++m)
      pred_loc.push_back( {0, m } );

    sframe pred_sf = make_testing_sframe(
        {"user", "item"},
        {flex_type_enum::INTEGER, flex_type_enum::INTEGER},
        pred_loc);

    // Now make sure both the original model and the saved/loaded
    // model predict how we'd expect.

    for(size_t idx = 0; idx < all_models.size(); ++idx) {

      auto test_model = all_models[idx];

      v2::ml_data pred_ml = test_model->create_ml_data(pred_sf);
      sframe pred_counts_sf = test_model->predict(pred_ml);

      std::vector<double> pred_counts = testing_extract_column<double>(pred_counts_sf.select_column(0));

      for(size_t i = 0; i < num_items; ++i)
        TS_ASSERT_EQUALS(pred_counts[i], double(counts[i]));

      ////////////////////////////////////////////////////////////////////////////////
      // Tests for recommend

      // Create a dataset with half the users

      std::vector<flexible_type> some_users;
      for(size_t i = 0; i < num_users; i += 2)
        some_users.push_back(i);

      std::vector<flexible_type> all_users;
      for(size_t i = 0; i < num_users; ++i)
        all_users.push_back(i);

      for(size_t user_source_index : {0, 1} ) {

        const std::vector<flexible_type>& user_source =
            (user_source_index == 0
             ? all_users
             : some_users);

        std::shared_ptr<sarray<flexible_type> > users_sarray = make_testing_sarray(
          flex_type_enum::INTEGER, user_source);

        sframe users_query = sframe({users_sarray}, {"user"});
        sframe restriction_sf = sframe();
        sframe exclusion_sf = sframe();
        sframe new_observation_sf = sframe();
        sframe new_user_data = sframe();
        sframe new_item_data = sframe();
        bool exclude_training_interactions = false;
        sframe empty_sframe = sframe();

        ////////////////////////////////////////////////////////////
        // All training interactions are properly excluded, when called
        // with all the data
        {

          sframe all_out = test_model->recommend(users_query, num_items);

          std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);

          std::set<std::pair<size_t, size_t> > recommended_pairs;
          for(const auto& row : recommend_out) {
            size_t user = row[0];
            size_t item = row[1];
            ASSERT_EQ(recommended_pairs.count({user, item}), 0);
            ASSERT_EQ(rated_items.count({user, item}), 0);
            recommended_pairs.insert({user, item});
          }

          for(size_t user : user_source) {
            for(size_t item = 0; item < num_items; ++item) {
              if(rated_items.count({user, item}) == 0) {
                ASSERT_EQ(recommended_pairs.count({user, item}), 1);
              }
            }
          }
        }

        ////////////////////////////////////////////////////////////
        // Nothing is excluded when asked to exclude nothing
        {
          exclude_training_interactions = false;

          sframe all_out = test_model->recommend(users_query, num_items,
                                                 restriction_sf, exclusion_sf,
                                                 new_observation_sf,
                                                 new_user_data, new_item_data,
                                                 exclude_training_interactions);

          std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);

          std::multiset<std::pair<size_t, size_t> > recommended_pairs;
          for(const auto& row : recommend_out) {
            size_t user = row[0];
            size_t item = row[1];
            ASSERT_EQ(recommended_pairs.count({user, item}), 0);
            recommended_pairs.insert({user, item});
          }

          for(size_t user : user_source) {
            for(size_t item = 0; item < num_items; ++item) {
              ASSERT_EQ(recommended_pairs.count({user, item}), 1);
            }
          }
        }

        ////////////////////////////////////////////////////////////
        // Everything is excluded, as expected, when we pass in the
        // training data as an exclusion list.
        {
          sframe exclusion_sf = data;

          sframe all_out = test_model->recommend(users_query, num_items,
                                                 restriction_sf, exclusion_sf,
                                                 new_observation_sf,
                                                 new_user_data, new_item_data,
                                                 exclude_training_interactions);

          std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);

          std::set<std::pair<size_t, size_t> > recommended_pairs;
          for(const auto& row : recommend_out) {
            size_t user = row[0];
            size_t item = row[1];
            ASSERT_EQ(recommended_pairs.count({user, item}), 0);
            ASSERT_EQ(rated_items.count({user, item}), 0);
            recommended_pairs.insert({user, item});
          }

          for(size_t user : user_source) {
            for(size_t item = 0; item < num_items; ++item) {
              if(rated_items.count({user, item}) == 0) {
                ASSERT_EQ(recommended_pairs.count({user, item}), 1);
              }
            }
          }
        }

        ////////////////////////////////////////////////////////////
        // Nothing is excluded when asked to exclude none of the
        // training data, but still pass in the training data as "new"
        // data
        {
          sframe new_observation_sf = data;
          exclude_training_interactions = false;

          sframe all_out = test_model->recommend(users_query, num_items,
                                                 restriction_sf, exclusion_sf, new_observation_sf,
                                                 new_user_data, new_item_data,
                                                 exclude_training_interactions);

          std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);

          std::multiset<std::pair<size_t, size_t> > recommended_pairs;
          for(const auto& row : recommend_out) {
            size_t user = row[0];
            size_t item = row[1];
            ASSERT_EQ(recommended_pairs.count({user, item}), 0);
            recommended_pairs.insert({user, item});
          }

          for(size_t user : user_source) {
            for(size_t item = 0; item < num_items; ++item) {
              ASSERT_EQ(recommended_pairs.count({user, item}), 1);
            }
          }
        }

        for(const std::vector<flexible_type>& item_inclusion_list
                : {std::vector<flexible_type>{1, 2, 5, 9},
                  std::vector<flexible_type>{0, 1, 2, 3},
                  std::vector<flexible_type>{0}} ) {

          std::set<size_t> item_inclusion_set(item_inclusion_list.begin(), item_inclusion_list.end());

          std::shared_ptr<sarray<flexible_type> > item_inclusion_sarray = make_testing_sarray(
          flex_type_enum::INTEGER, item_inclusion_list);

          sframe inclusion_sf = sframe({item_inclusion_sarray}, {"item"});

          /// Test recommend when the exclusion list is supplied explicitly
          {

            exclude_training_interactions = true;
            sframe all_out = test_model->recommend(users_query, num_items,
                                                   inclusion_sf, exclusion_sf,
                                                   new_observation_sf,
                                                   new_user_data, new_item_data,
                                                   exclude_training_interactions);

            std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);

            std::multiset<std::pair<size_t, size_t> > recommended_pairs;
            for(const auto& row : recommend_out) {
              size_t user = row[0];
              size_t item = row[1];
              ASSERT_EQ(item_inclusion_set.count(item), 1);
              ASSERT_EQ(recommended_pairs.count({user, item}), 0);
              ASSERT_EQ(rated_items.count({user, item}), 0);
              recommended_pairs.insert({user, item});
            }

            for(size_t user : user_source) {
              for(size_t item = 0; item < num_items; ++item) {
                if(item_inclusion_set.count(item) == 1) {
                  if(rated_items.count({user, item}) == 0)
                    ASSERT_EQ(recommended_pairs.count({user, item}), 1);
                } else {
                  ASSERT_EQ(recommended_pairs.count({user, item}), 0);
                }
              }
            }
          }

          /// Do it while not excluding the training data
          {
            exclude_training_interactions = false;
            sframe all_out = test_model->recommend(users_query, num_items,
                                                   inclusion_sf, exclusion_sf,
                                                   new_observation_sf,
                                                   new_user_data, new_item_data,
                                                   exclude_training_interactions);

            std::vector<std::vector<flexible_type> > recommend_out = testing_extract_sframe_data(all_out);
            std::multiset<std::pair<size_t, size_t> > recommended_pairs;
            for(const auto& row : recommend_out) {
              size_t user = row[0];
              size_t item = row[1];
              ASSERT_EQ(item_inclusion_set.count(item), 1);
              ASSERT_EQ(recommended_pairs.count({user, item}), 0);
              recommended_pairs.insert({user, item});
            }

            // Go through all (user, item) pairs and ensure that all recommended pairs
            // are only items in the inclusion set.
            for(size_t user : user_source) {
              for(size_t item = 0; item < num_items; ++item) {
                ASSERT_EQ(recommended_pairs.count({user, item}), item_inclusion_set.count(item));
              }
            }
          }
        }
      }

      ////////////////////////////////////////////////////////////
      // Test the ranking of the items.
      auto user_indexer = test_model->metadata->indexer(test_model->USER_COLUMN_INDEX);
      auto item_indexer = test_model->metadata->indexer(test_model->ITEM_COLUMN_INDEX);

      std::string user_column_name = test_model->metadata->column_name(test_model->USER_COLUMN_INDEX);
      std::string item_column_name = test_model->metadata->column_name(test_model->ITEM_COLUMN_INDEX);

      {
        auto user_sf = sframe({make_testing_sarray(flex_type_enum::INTEGER, {0, 1})},
                              {"user"});
        size_t k = 5;

        // Test the base data through the use of the most flexible option
        std::vector<std::vector<flexible_type> > skip_data =
            {{0, 0},
             {0, 2},
             {1, 2},
             {1, 3} };

        sframe restriction_sf = sframe();
        sframe exclusion_sf = make_testing_sframe(
            {"user", "item"},
            {flex_type_enum::INTEGER, flex_type_enum::INTEGER},
            skip_data);
        sframe new_observation_sf = sframe();
        sframe new_user_data = sframe();
        sframe new_item_data = sframe();
        bool exclude_training_interactions = false;

        sframe ranked_items = test_model->recommend(user_sf, k,
                                                    restriction_sf, exclusion_sf,
                                                    new_observation_sf,
                                                    new_user_data, new_item_data,
                                                    exclude_training_interactions);

        sframe unindexed_ranked_items = v2::map_from_custom_indexed_sframe(
            { {"user", user_indexer},
              {"item", item_indexer} },
            ranked_items);

        std::vector<std::vector<flexible_type> > res = testing_extract_sframe_data(unindexed_ranked_items);

        // for(size_t user : {0, 1} ) {
        //   for(size_t item = 0; item < num_items; ++item) {
        //     if(rated_items.count({user, item}))
        //       std::cout << "RATED: " << user << "," << item << std::endl;
        //   }
        // }

        // for(std::vector<flexible_type>& row : res) {
        //   std::cout << "res = " << row[0] << "," << row[1] << "," << row[2] << "," << row[3] << ")" << std::endl;
        // }

        std::vector<std::vector<flexible_type> > true_res =
            { {0, 1},
              {0, 3},
              {0, 4},
              {0, 5},
              {0, 6},
              {1, 0},
              {1, 1},
              {1, 4},
              {1, 5},
              {1, 6} };

        TS_ASSERT_EQUALS(res.size(), true_res.size());

        for(size_t i = 0; i < res.size(); ++i) {
          ASSERT_EQ(res[i][0], true_res[i][0]);
          ASSERT_EQ(res[i][1], true_res[i][1]);
        }

        std::vector<double> y_pred = testing_extract_column<double>(ranked_items.select_column("score"));
        TS_ASSERT( (y_pred == std::vector<double>{
                    double(counts[1]),
                    double(counts[3]),
                    double(counts[4]),
                    double(counts[5]),
                    double(counts[6]),
                    double(counts[0]),
                    double(counts[1]),
                    double(counts[4]),
                    double(counts[5]),
                    double(counts[6])}) );
      }
    }
  }
};

BOOST_FIXTURE_TEST_SUITE(_recsys_popularity_test, recsys_popularity_test)
BOOST_AUTO_TEST_CASE(test_popularity) {
  recsys_popularity_test::test_popularity();
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * 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 Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_MUL_IEXP_OUTER_HPP
#define AMA_TENSOR_MUL_IEXP_OUTER_HPP 1

#include <ama/tensor/iexp/index_reorder.hpp>
#include <ama/tensor/iexp/iexp_base.hpp>
#include <ama/tensor/mul/mul_calculator.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/plus.hpp>

namespace ama
{
  namespace tensor_
  {

    /* forward declaration*/
    template <typename LEFT, typename RIGHT> class mul_outer;

    /* traits definition */
    template <typename LEFT, typename RIGHT>
    struct iexp_traits< mul_outer<LEFT, RIGHT> >
    {
      typedef typename LEFT::value_type value_type;

      typedef typename LEFT::dimension_type dimension_type;

      typedef typename mul_index<LEFT, RIGHT>::controvariant_list controvariant_list;
      typedef typename mul_index<LEFT, RIGHT>::covariant_list covariant_list;

      typedef ::boost::mpl::false_ is_assignable;
    };

    /* class declaration */
    template <typename LEFT, typename RIGHT>
    class mul_outer:
        public iexp_base< mul_outer<LEFT, RIGHT> >
    {
    protected:
      typedef iexp_base< mul_outer<LEFT, RIGHT> > base_type;
      typedef mul_outer<LEFT, RIGHT> derived_type;

    protected:
      typedef LEFT left_operand_type;
      typedef RIGHT right_operand_type;

    public:
      typedef typename base_type::value_type value_type;

    public:
      /* costructor */
      mul_outer(left_operand_type const & left,
                 right_operand_type const & right)
          : m_left(left),
            m_right(right) { }

    public:
      /* retrieve the value */
      template <typename IMAP>
      value_type at() const
      {
        return (m_left.template at<IMAP>()) * (m_right.template at<IMAP>());
      }

    protected:
      /* members */
      left_operand_type const m_left;
      right_operand_type const m_right;
    };

  }
}

#endif /* AMA_TENSOR_MUL_IEXP_OUTER_HPP */


//==================================================================================================
/*!
  @file

  @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_COMMON_SCALAR_CONSTANT_CONSTANT_GENERATOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_CONSTANT_CONSTANT_GENERATOR_HPP_INCLUDED

#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/as.hpp>
#include <boost/config.hpp>
#include <boost/simd/detail/nsm.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4310) // Truncation
#pragma warning(disable: 4309) // Truncation
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace tt = nsm::type_traits;

  BOOST_DISPATCH_OVERLOAD_FALLBACK( (typename X, typename V)
                                  , boost::dispatch::constant_value_<tag::constant_>
                                  , boost::dispatch::cpu_
                                  , bd::scalar_<bd::unspecified_<V>>
                                  , bd::target_< bd::unspecified_<X> >
                                  )
  {
    template<typename T, T N>
    static BOOST_FORCEINLINE T impl(tt::integral_constant<T,N> const&) BOOST_NOEXCEPT
    {
      return tt::integral_constant<T,N>{};
    }

    template<typename R, typename T, T N>
    static BOOST_FORCEINLINE R impl(nsm::real_<R,T,N> const& ) BOOST_NOEXCEPT
    {
      return static_cast<R>(nsm::real_<R,T,N>{});
    }

    template<typename T, std::intmax_t N, std::intmax_t D>
    static BOOST_FORCEINLINE T impl(detail::ratio<T,N,D> const&) BOOST_NOEXCEPT
    {
      return T(double(N)/double(D));
    }

    BOOST_FORCEINLINE
    auto operator()(V const& v, X const&) const BOOST_NOEXCEPT -> decltype(impl(v))
    {
      return impl( v );
    }
  };
} } }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif