//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
#ifndef BOOST_GRAPH_MST_PRIM_HPP
#define BOOST_GRAPH_MST_PRIM_HPP

#include <functional>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace boost {

  namespace detail {
    // this should be somewhere else in boost...
    template <class U, class V> struct _project2nd {
      V operator()(U, V v) const { return v; }
    };
  }

  namespace detail {

    // This is Prim's algorithm to calculate the Minimum Spanning Tree
    // for an undirected graph with weighted edges.

    template <class Graph, class P, class T, class R, class Weight>
    inline void
    prim_mst_impl(const Graph& G,
                  typename graph_traits<Graph>::vertex_descriptor s,
                  const bgl_named_params<P,T,R>& params,
                  Weight)
    {
      typedef typename property_traits<Weight>::value_type W;
      std::less<W> compare;
      detail::_project2nd<W,W> combine;
      dijkstra_shortest_paths(G, s, params.distance_compare(compare).
                              distance_combine(combine));
    }
  } // namespace detail

  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap>
  inline void
  prim_minimum_spanning_tree
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     DijkstraVisitor vis)
  {
    typedef typename property_traits<WeightMap>::value_type W;
    std::less<W> compare;
    detail::_project2nd<W,W> combine;
    dijkstra_shortest_paths(g, s, predecessor, distance, weight, index_map,
                            compare, combine, (std::numeric_limits<W>::max)(), 0,
                            vis);
  }

  template <class VertexListGraph, class PredecessorMap,
            class P, class T, class R>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g,
     PredecessorMap p_map,
     const bgl_named_params<P,T,R>& params)
  {
    detail::prim_mst_impl
      (g,
       choose_param(get_param(params, root_vertex_t()), *vertices(g).first),
       params.predecessor_map(p_map),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
  }

  template <class VertexListGraph, class PredecessorMap>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g, PredecessorMap p_map)
  {
    detail::prim_mst_impl
      (g, *vertices(g).first, predecessor_map(p_map).
       weight_map(get(edge_weight, g)),
       get(edge_weight, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_MST_PRIM_HPP


#ifndef utils_utils_hpp
#define utils_utils_hpp

#include <vector>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <string>
#include <regex>

#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <math.h>

#include "img_hash/AverageHash.h"
#include "img_hash/BlockMeanHash.h"
#include "img_hash/PerceptualHash.h"

//#include "img_hash/FragmentHash.h"
//#include "ShapeAndPositionInvariantImage.h"
#include "Triangle.h"
#include "mainImageProcessingFunctions.hpp"
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <iostream>
#include "utils/utils.hpp"
#include <tuple>

using boost::property_tree::ptree;
using boost::property_tree::read_json;
using boost::property_tree::write_json;

namespace pt = boost::property_tree;
using namespace std;

static unsigned long x=123456789, y=362436069, z=521288629;

void drawShape_inner(cv::Mat mat);

unsigned long xorshf96(void) {          //period 2^96-1
    unsigned long t;
    x ^= x << 16;
    x ^= x >> 5;
    x ^= x << 1;

    t = x;
    x = y;
    y = z;
    z = t ^ x ^ y;

    return z;
}

cv::Mat convertKeypointsVectorToMat(vector<Keypoint> kps)
{
    cv::Mat ret = cv::Mat::zeros(3, kps.size(), CV_64F);

    for (unsigned int i = 0; i < kps.size(); i++)
    {
        auto k = kps[i];
        ret.at<double>(0, i) = k.x;
        ret.at<double>(1, i) = k.y;
        ret.at<double>(2, i) = 1;
    }
    return ret;
}

vector<Keypoint> convertMatToKeypointsVector(cv::Mat inputPoints)
{
    vector<Keypoint> ret;
    for (signed int i = 0; i < inputPoints.cols; i++)//signed just to surpress warning
    {
        double x = inputPoints.at<double>(0, i);
        double y = inputPoints.at<double>(1, i);
        Keypoint temp(x, y);
        ret.push_back(temp);
    }
    return ret;
}

vector<Keypoint> applyTransformationMatrixToKeypointVector(vector<Keypoint> keypoints, cv::Mat transformationMat)
{
    cv::Mat keypointMat = convertKeypointsVectorToMat(keypoints);
    cv::Mat transKeypointMat = transformationMat*keypointMat;
    return convertMatToKeypointsVector(transKeypointMat);
}

void drawSingleKeypoint(Keypoint kp, cv::Mat inputImage, cv::Scalar colour=cv::Scalar(0,0,255)){
    cv::circle(inputImage, cv::Point(kp.x, kp.y), 2, colour);
}

void drawKeypoints(vector<Keypoint> keypoints, cv::Mat inputImage, cv::Scalar colour=cv::Scalar(0,0,255))
{
    for(auto kp : keypoints)
    {
        drawSingleKeypoint(kp, inputImage, colour);
    }
}

vector<Keypoint> readKeypointsFromJsonFile(string filename);

vector<Keypoint> getKeypoints(cv::Mat inputImage)
{
    //clear any ones that existed before hand!!!
    //TODO:
    //save the image
    cv::imwrite("tempImage.jpg", inputImage);
    //call the python on the image
    FILE* file = popen("python ./python_src/dumpKeypointsToJson.py ./tempImage.jpg ./tempOutputKeypoints.json", "r");
    // use fscanf to read:
    char buffer[101];
    fscanf(file, "%100s", buffer);
    pclose(file);
    //save the keypoints to the output file
    return readKeypointsFromJsonFile("./tempOutputKeypoints.json");
    //read the output file and get the keypoints (we already have a script for this!!!)
}

void util_drawShape(vector<Keypoint> points, cv::Mat inputImage, bool randomColours = true){
    auto prevPoint = points.back();

    int r = (int) xorshf96();
    int g = (int) xorshf96();
    int b = (int) xorshf96();
    for (auto currentPoint : points){

        cv::line(inputImage, cv::Point(prevPoint.x, prevPoint.y), cv::Point(currentPoint.x, currentPoint.y),
                 cv::Scalar(b,g,r));

        prevPoint = currentPoint;
    }
}

cv::Size calcBoundingRectangleOfShape(cv::Mat shape) {
    vector<cv::Point> convertedMat;
//    cout << "shape: " << shape << endl;
    for (int i = 0; i < shape.cols; i++) {
        //grab the two points
        double x = shape.at<double>(i);
        double y = shape.at<double>(shape.cols + i);
        cv::Point tempPt(x, y);
        convertedMat.push_back(tempPt);
//        cout << "X: " << x << ", Y: " << y << endl;
    }
    auto resultRect = cv::boundingRect( cv::Mat(convertedMat) );
    return resultRect.size();
}

void drawShape_inner(cv::Mat shape, cv::Mat DEBUG_IMAGE) {
    vector<Keypoint> convertedMat;
//    cout << "shape: " << shape << endl;
    for (int i = 0; i < shape.cols; i++) {
        //grab the two points
        double x = shape.at<double>(i);
        double y = shape.at<double>(shape.cols + i);
        Keypoint tempPt(x, y);
        convertedMat.push_back(tempPt);
//        cout << "X: " << x << ", Y: " << y << endl;
    }
    util_drawShape(convertedMat, DEBUG_IMAGE);
}

std::pair<cv::Mat, cv::Size> calcTransformationMatrix(cv::Size inputImageSize, double rotation, double scale, bool keepArea=true)
{

    //input image bounding rectangle
    double xlen = inputImageSize.width;
    double ylen = inputImageSize.height;
    cv::Matx34d inputImageShape(
                               0.0, xlen, xlen,  0.0,
                               0.0,  0.0, ylen, ylen,
                               1.0,  1.0,  1.0,  1.0
                            );

    //transpose center of input image to 0,0
    cv::Matx33d transpose1_m(
                              1.0, 0.0, -(inputImageSize.width/2),
                              0.0, 1.0, -(inputImageSize.height/2),
                              0.0, 0.0, 1.0
                            );
    //then rotate
    cv::Mat rotate_m3x2 = cv::getRotationMatrix2D(cv::Point2f(0, 0), rotation, 1.0);
    cv::Matx33d rotate_m(
            rotate_m3x2.at<double>(0), rotate_m3x2.at<double>(1), rotate_m3x2.at<double>(2),
            rotate_m3x2.at<double>(3), rotate_m3x2.at<double>(4), rotate_m3x2.at<double>(5),
            0.0, 0.0, 1.0
    );
    //then scale
    double scaleX = (keepArea)? sqrt(scale):     scale;
    double scaleY = (keepArea)? 1.0/sqrt(scale): 1.0;
    cv::Matx33d scale_m(scaleX, 0.0, 0.0,
                        0.0, scaleY, 0.0,
                        0.0, 0.0, 1.0);

    //get the shape of the output image
    cv::Mat temp = cv::Mat(scale_m)*cv::Mat(rotate_m)*cv::Mat(transpose1_m);
    cv::Mat outputImageShape = temp*cv::Mat(inputImageShape);
    cv::Size newImageSize = calcBoundingRectangleOfShape(outputImageShape);

    //then move the image center to the new center
    cv::Matx33d transpose2_m(  1.0, 0.0, (newImageSize.width/2),
                               0.0, 1.0, (newImageSize.height/2),
                               0.0, 0.0, 1.0 );
    cv::Mat ret = cv::Mat(transpose2_m)*cv::Mat(scale_m)*cv::Mat(rotate_m)*cv::Mat(transpose1_m);//*cv::Mat(rotate_m);
    return pair<cv::Mat, cv::Size>(ret, newImageSize);
}

void drawSingleTriangleOntoImage(Triangle tri, cv::Mat inputImage, bool setColour = false, cv::Scalar colourInput = cv::Scalar(0,0,0)){
    auto keypoints = tri.toKeypoints();
    auto prevPoint = keypoints.back();
//    for (auto currentPoint: keypoints)
//    {
    int r = (int) xorshf96();
    int g = (int) xorshf96();
    int b = (int) xorshf96();
    for (int i = 0; i < 3; i++){
        auto currentPoint = keypoints[i];
        auto colour = (setColour)? colourInput: cv::Scalar(b,g,r);

        cv::line(inputImage, cv::Point(prevPoint.x, prevPoint.y), cv::Point(currentPoint.x, currentPoint.y),
                 colour);
        //cv::imshow("something", inputImage);
        //cv::waitKey(10);
        prevPoint = currentPoint;
    }
}

void drawTrianglesOntoImage(vector<Triangle> tris, cv::Mat inputImage, bool randomColours = true)
{
    for (auto tri: tris){
        drawSingleTriangleOntoImage(tri, inputImage, randomColours);
    }
}

Triangle getTriangleFromRedisEntry(string redisEntry)
{
    pt::ptree root;
    std::stringstream ss;
    ss << redisEntry;
    pt::read_json(ss, root);

    vector<Keypoint> keypoints;
    for (auto pt_j: root.get_child("triangle"))
    {
        double x = pt_j.second.get<double>("x");
        double y = pt_j.second.get<double>("y");
        keypoints.push_back(Keypoint(x,y));
    }
    return Triangle(keypoints);
}

string getImageNameFromRedisEntry(string redisEntry)
{
    pt::ptree root;
    std::stringstream ss;
    ss << redisEntry;
    pt::read_json(ss, root);
    return root.get<string>("imageName");
}

string convertToRedisEntryJson(string imageName, Triangle tri){
    pt::ptree root;
    root.put("imageName", imageName);

    //add each point of the triangle
    pt::ptree points;
    for (auto pt : tri.toKeypoints())
    {
        pt::ptree point;
        point.put("x", pt.x);
        point.put("y", pt.y);
        points.push_back(std::make_pair("", point));
    }
    root.add_child("triangle", points);

    std::ostringstream buf;
    write_json(buf, root, false);
    return buf.str();
}

vector<Keypoint> readKeypointsFromJsonFile(std::ifstream *file)
{
    vector<Keypoint> result;
    try {
        boost::property_tree::ptree pt;
        boost::property_tree::read_json(*file, pt);

        for (auto label0 : pt) {
            if (label0.first == "output") {
                for (auto label1: label0.second) {
                    if (label1.first == "keypoints") {
                        for (auto kp : label1.second){
                            double x, y;
                            for (auto pt : kp.second){
                                if(pt.first == "x"){
                                    x = pt.second.get_value<double>();
                                }else{
                                    y = pt.second.get_value<double>();
                                }
                            }
                            result.push_back(Keypoint(x, y));
                        }
                    }
                }
            }
        }
    } catch (std::exception const &e) {
        std::cerr << e.what() << std::endl;
    }
    return result;
}

vector<Keypoint> readKeypointsFromJsonFile(string filename)
{
    std::ifstream file(filename);
    return readKeypointsFromJsonFile(&file);
}

void print(boost::property_tree::ptree const &pt) {
}

double getKeypointDistance(Keypoint one, Keypoint two)
{
	return sqrt( pow( one.x-two.x, 2.0 ) + pow( one.y-two.y, 2.0 ) );
}

vector<Keypoint> findKeypointsWithInRangeFromTwoPoints(Keypoint one, Keypoint two, vector<Keypoint> otherKeypoints, double lowerThreshold, double upperThreshold)
{
	vector<Keypoint> result;
	for (auto cmpKp : otherKeypoints)
	{
        double distanceFromPointOne = getKeypointDistance(one, cmpKp);
        double distanceFromPointTwo = getKeypointDistance(two, cmpKp);
		if(distanceFromPointOne > lowerThreshold && distanceFromPointOne < upperThreshold
                && distanceFromPointTwo > lowerThreshold && distanceFromPointTwo < upperThreshold)
		{
			result.push_back(cmpKp);
		}
	}
	return result;
}

bool isInKeypointExcludeList(Keypoint keypoint, vector<Keypoint> excludeList) {
    for (auto kp : excludeList)
    {
        if(kp.x == keypoint.x && kp.y == keypoint.y){
            return true;
        }
    }
    return false;
}

bool shouldPointBeExcluded(Keypoint pt, vector<Keypoint> previouslyProcessedPoints, vector<Keypoint> currentProcessedPoints, Keypoint currentTopLevelPoint, Keypoint currentSecondLevelPoint)
{
    return isInKeypointExcludeList(pt, previouslyProcessedPoints)
           || isInKeypointExcludeList(pt, currentProcessedPoints)
           || currentTopLevelPoint == pt
           || currentSecondLevelPoint == pt
            ;
}

//NOTE: otherKeypoints may contain centerKeypoint
vector<Triangle> buildTrianglesForSingleKeypoint(Keypoint centerKeypoint, vector<Keypoint> otherKeypoints, vector<Keypoint> previouslyProcessedPoints, double lowerThreshold, double upperThreshold)
{
	vector<Triangle> result;
    vector<Keypoint> currentProcessedPoints;//a collection of points we have processed since entering this function
	for (auto iterKeypoint: otherKeypoints)
	{
        if(isInKeypointExcludeList(iterKeypoint, previouslyProcessedPoints) || iterKeypoint == centerKeypoint) {
            continue;
        }

        double distance = getKeypointDistance(iterKeypoint, centerKeypoint);
		if(distance > lowerThreshold && distance < upperThreshold)
		{
			vector<Keypoint> finalKeypoints = findKeypointsWithInRangeFromTwoPoints(iterKeypoint, centerKeypoint, otherKeypoints, lowerThreshold, upperThreshold);
			for (auto finKp : finalKeypoints)
			{
                //check if this combination of points will make a triangle we have already created
                if(shouldPointBeExcluded(finKp, previouslyProcessedPoints, currentProcessedPoints, centerKeypoint, iterKeypoint)){
                    continue;
                }
                Triangle testingTri(centerKeypoint, iterKeypoint, finKp);
                if(testingTri.calcArea() > 200){
                    result.push_back(testingTri);
                }
			}
		}
        currentProcessedPoints.push_back(iterKeypoint);
	}
	return result;
}

vector<Triangle> buildTrianglesFromKeypoints(vector<Keypoint> keypoints, double lowerThreshold=150, double upperThreshold=500)
{
	vector<Triangle> outputTriangles;
//	for (auto keypoint: keypoints)
//	{
    //FIXME: this multi-threading needs to be improved
    #pragma omp parallel
    {
        vector<Triangle> vec_private;
        #pragma omp for nowait schedule(static)
        for (unsigned int i = 0; i < keypoints.size(); i++)
        {
            vector<Keypoint>::const_iterator first = keypoints.begin();
            vector<Keypoint>::const_iterator last = keypoints.begin() + i;
            vector<Keypoint> processedPoints(first, last);
            auto keypoint = keypoints[i];
            auto triangles = buildTrianglesForSingleKeypoint(keypoint, keypoints, processedPoints, lowerThreshold, upperThreshold);
            for (auto tri : triangles){
                vec_private.push_back(tri);
            }
            processedPoints.push_back(keypoint);
        }
        #pragma omp critical
        outputTriangles.insert(outputTriangles.end(), vec_private.begin(), vec_private.end());
    }
	return outputTriangles;
}

vector<Triangle> buildTrianglesFromKeypointJsonFile(string filename){
    vector<Keypoint> output = readKeypointsFromJsonFile(filename);
    vector<Triangle> ret = buildTrianglesFromKeypoints(output, 50, 400);
    return ret;
}

vector<Triangle> getTriangles(string filename){
    return buildTrianglesFromKeypointJsonFile(filename);
}

const pair<vector<Triangle>, vector<Triangle>> readMatchingTrianglesFromJsonFile(std::ifstream *file) {
    vector<Triangle> image1OutputTriangles;
    vector<Triangle> image2OutputTriangles;
    try {
        boost::property_tree::ptree pt;
        boost::property_tree::read_json(*file, pt);

        using boost::property_tree::ptree;
        for (auto it : pt) {
            if (it.first == "count") {
                //save count
                cout << "Number of triangles read: " << it.second.get_value<std::string>() << endl;
            }

            if (it.first == "triangles") {
                for (auto tri: it.second) {
                    vector<Keypoint> one, two;
                    for (auto tri_point : tri.second) {
                        vector<Keypoint> bothKp;
                        for (auto pt_u : tri_point.second) {
                            vector<double> tempPoint;
                            for (auto pt : pt_u.second) {
                                double coord = pt.second.get_value<double>();
                                tempPoint.push_back(coord);
                            }
                            Keypoint kp(tempPoint[0], tempPoint[1]);
                            bothKp.push_back(kp);
                        }
                        one.push_back(bothKp[0]);
                        two.push_back(bothKp[1]);
                    }
                    Triangle one_t(one);
                    Triangle two_t(two);
                    image1OutputTriangles.push_back(one_t);
                    image2OutputTriangles.push_back(two_t);
                }
            }
        }

        print(pt);
    }
    catch (std::exception const &e) {
        std::cerr << e.what() << std::endl;
    }

    return pair<vector<Triangle>, vector<Triangle>>(image1OutputTriangles, image2OutputTriangles);
}

const pair<vector<Triangle>, vector<Triangle>> readMatchingTrianglesFromJsonFile(const string filename) {
    std::ifstream file(filename);
    return readMatchingTrianglesFromJsonFile(&file);
}

template<typename T>
const vector<T> readJsonHashesFile(std::ifstream *file) {
    vector<T> ret;
    vector<Triangle> image1OutputTriangles;
    vector<Triangle> image2OutputTriangles;
    try {
        boost::property_tree::ptree pt;
        boost::property_tree::read_json(*file, pt);

        for (auto label0 : pt) {
            if (label0.first == "output") {
                for (auto label1: label0.second) {
                    if (label1.first == "imageName") {
                        //save the imageName
                    } else if (label1.first == "hashes") {
                        for (auto hash_item: label1.second) {
                            ret.push_back(T(hash_item.second.get_value<std::string>()));
                            //cout << "hash: " << hash_item.second.get_value<std::string>() << endl;
                        }
                    }
                }
            }
        }

        print(pt);
    }
    catch (std::exception const &e) {
        std::cerr << e.what() << std::endl;
    }

    return ret;
}

template<typename T>
const vector<T> readJsonHashesFile(const string filename) {
    std::ifstream file(filename);
    return readJsonHashesFile<T>(&file);
}

template<typename T>
void dumpHashesToJsonFile(std::ofstream *file, vector<T> hashes) {
    ptree outputTree, hashesTree;
    for(auto hash: hashes){
        hashesTree.push_back(std::make_pair( "", ptree(hash.toString()) ));
    }
    outputTree.put_child("output.hashes", hashesTree);
    std::stringstream ss;
    write_json(ss, outputTree);
    *file << ss.str();
}

template<typename T> void dumpHashesToJsonFile(const string filename, vector<T> hashes) {
    std::ofstream ofs;
    ofs.open (filename, std::ofstream::out);
    dumpHashesToJsonFile<T>(&ofs, hashes);
}

ShapeAndPositionInvariantImage getLoadedImage(string imageFullPath) {
    cv::Mat img = cv::imread(imageFullPath);
    return ShapeAndPositionInvariantImage("", img, std::vector<Keypoint>(), "");
}

void writeHashesToFile(string fullFilePath, vector<string> hashes) {
    std::ofstream outputFile;
    outputFile.open(fullFilePath, std::ios::out);
    for (auto hash: hashes) {
        outputFile << hash << endl;
    }
}

template<typename T>
void writeHashObjectsToFile(string fullFilePath, vector<T> hashes) {
    vector<string> hashesToString;
    for (auto hash: hashes) {
        hashesToString.push_back(hash.toString());
    }
    writeHashesToFile(fullFilePath, hashesToString);
}

vector<string> loadImageNames(string filename) {
    vector<string> filenames;

    std::ifstream file(filename);
    std::string str = "";
    while (true) {
        if (!std::getline(file, str)) {
            break;
        }
        filenames.push_back(str);
    }

    return filenames;
}

vector<string> loadExcludeList(string filename) {
    vector<string> filenames;

    std::ifstream file(filename);
    std::string str = "";
    while (true) {
        if (!std::getline(file, str)) {
            break;
        }
        filenames.push_back(str);
    }

    return filenames;
}

template<typename T>
std::vector<T> loadHashesFromFile(std::string filename) {
    std::vector<T> ret;
    std::ifstream file(filename);
    std::string str = "";
    while (true) {
        if (!std::getline(file, str)) {
            break;
        }

        T fraghash(str);
        ret.push_back(fraghash);
        //std::cout << str << " should match: " << cv::convertHashToString(fraghash) << std::endl;
    }
    return ret;
}

bool isInImageNameExcludeList(string name, vector<string> excludeList, string imageName) {
    if (name == imageName) {
        return true;
    }
    for (auto e: excludeList) {
        if (name == e) {
            return true;
        }
    }
    return false;
}

#endif//utils_utils_hpp


/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_process_mode_iid_test.cpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <memory>

#include <fl/model/process/linear_transition.hpp>
#include <fl/model/process/joint_transition.hpp>

/* ############################ */
/* # Static dimensions tests  # */
/* ############################ */

TEST(JointProcessModel_IID_Tests, fixed_fixed_static_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, 3, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    EXPECT_EQ(fl::Traits<JointModel>::State::SizeAtCompileTime, 9);
    EXPECT_EQ(fl::Traits<JointModel>::Noise::SizeAtCompileTime, 9);
    EXPECT_EQ(fl::Traits<JointModel>::Input::SizeAtCompileTime, 3);
}

TEST(JointProcessModel_IID_Tests, fixed_dynamic_static_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    EXPECT_EQ(fl::Traits<JointModel>::State::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Noise::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Input::SizeAtCompileTime, 3);
}

TEST(JointProcessModel_IID_Tests, dynamic_fixed_static_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, 3, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, -1>> JointModel;

    EXPECT_EQ(fl::Traits<JointModel>::State::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Noise::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Input::SizeAtCompileTime, -1);

    auto f = JointModel(LocalModel(3), 3);
    EXPECT_EQ(f.state_dimension(), 9);
    EXPECT_EQ(f.noise_dimension(), 9);
    EXPECT_EQ(f.input_dimension(), 3);

    auto f_mof = JointModel(
                MultipleOf<LocalModel, -1>(LocalModel(Cov::Identity()), 3));

    EXPECT_EQ(f_mof.state_dimension(), 9);
    EXPECT_EQ(f_mof.noise_dimension(), 9);
    EXPECT_EQ(f_mof.input_dimension(), 3);
}

TEST(JointProcessModel_IID_Tests, dynamic_dynamic_static_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, -1>> JointModel;

    EXPECT_EQ(fl::Traits<JointModel>::State::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Noise::SizeAtCompileTime, -1);
    EXPECT_EQ(fl::Traits<JointModel>::Input::SizeAtCompileTime, -1);
}

/* ############################ */
/* # Runtime dimensions tests # */
/* ############################ */

TEST(JointProcessModel_IID_Tests, fixed_fixed_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, 3, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    auto f = JointModel(LocalModel());
    EXPECT_EQ(f.state_dimension(), 9);
    EXPECT_EQ(f.noise_dimension(), 9);
    EXPECT_EQ(f.input_dimension(), 3);

    auto f_mof = JointModel(MultipleOf<LocalModel, 3>(LocalModel()));
    EXPECT_EQ(f_mof.state_dimension(), 9);
    EXPECT_EQ(f_mof.noise_dimension(), 9);
    EXPECT_EQ(f_mof.input_dimension(), 3);
}

TEST(JointProcessModel_IID_Tests, fixed_dynamic_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    auto f = JointModel(LocalModel(3));
    EXPECT_EQ(f.state_dimension(), 9);
    EXPECT_EQ(f.noise_dimension(), 9);
    EXPECT_EQ(f.input_dimension(), 3);

    auto f_mof = JointModel(
        MultipleOf<LocalModel, 3>(LocalModel(Cov::Identity(3,3), 3)));
    EXPECT_EQ(f_mof.state_dimension(), 9);
    EXPECT_EQ(f_mof.noise_dimension(), 9);
    EXPECT_EQ(f_mof.input_dimension(), 3);
}

TEST(JointProcessModel_IID_Tests, dynamic_dynamic_dimensions)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, -1>> JointModel;

    auto f = JointModel(LocalModel(Cov::Identity(3, 3), 3), 3);

    EXPECT_EQ(f.state_dimension(), 9);
    EXPECT_EQ(f.noise_dimension(), 9);
    EXPECT_EQ(f.input_dimension(), 3);

    auto f_mof = JointModel(
        MultipleOf<LocalModel, -1>(LocalModel(Cov::Identity(3,3), 3), 3));
    EXPECT_EQ(f_mof.state_dimension(), 9);
    EXPECT_EQ(f_mof.noise_dimension(), 9);
    EXPECT_EQ(f_mof.input_dimension(), 3);
}

/* ############################ */
/* # Prediction tests         # */
/* ############################ */

TEST(JointProcessModel_IID_Tests, fixed_fixed_prediction)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, 5, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    auto f = JointModel(LocalModel(Cov::Identity() * 9.0 ));

    auto state = Traits<JointModel>::State();
    auto noise = Traits<JointModel>::Noise();
    auto input = Traits<JointModel>::Input();

    state.setZero();
    noise.setOnes();
    input.setZero();

    auto prediction = f.predict_state(1., state, noise, input);

    auto expected = Eigen::Matrix<double, 15, 1>::Ones().eval();
    expected.topRows(5) = expected.topRows(5) * 3.;
    expected.middleRows(5, 5) = expected.middleRows(5, 5) * 3.;
    expected.bottomRows(5) = expected.bottomRows(5) * 3.;

    EXPECT_TRUE(fl::are_similar(prediction, expected));
}

TEST(JointProcessModel_IID_Tests, fixed_dynamic_prediction)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, 3>> JointModel;

    auto f = JointModel(LocalModel(Cov::Identity(5, 5) * 9.0, 5));

    auto state = Traits<JointModel>::State(f.state_dimension(), 1);
    auto noise = Traits<JointModel>::Noise(f.noise_dimension(), 1);
    auto input = Traits<JointModel>::Input(f.input_dimension(), 1);

    state.setZero();
    noise.setOnes();
    input.setZero();

    auto prediction = f.predict_state(1., state, noise, input);

    auto expected = Eigen::Matrix<double, 15, 1>::Ones().eval();
    expected.topRows(5) = expected.topRows(5) * 3.;
    expected.middleRows(5, 5) = expected.middleRows(5, 5) * 3.;
    expected.bottomRows(5) = expected.bottomRows(5) * 3.0;

    EXPECT_TRUE(fl::are_similar(prediction, expected));
}

TEST(JointProcessModel_IID_Tests, dynamic_fixed_prediction)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, 5, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, -1>> JointModel;

    auto f = JointModel(LocalModel(Cov::Identity() * 9.0), 3);

    auto state = Traits<JointModel>::State(f.state_dimension(), 1);
    auto noise = Traits<JointModel>::Noise(f.noise_dimension(), 1);
    auto input = Traits<JointModel>::Input(f.input_dimension(), 1);

    state.setZero();
    noise.setOnes();
    input.setZero();

    auto prediction = f.predict_state(1., state, noise, input);

    auto expected = Eigen::Matrix<double, 15, 1>::Ones().eval();
    expected.topRows(5) = expected.topRows(5) * 3.;
    expected.middleRows(5, 5) = expected.middleRows(5, 5) * 3.;
    expected.bottomRows(5) = expected.bottomRows(5) * 3.0;

    EXPECT_TRUE(fl::are_similar(prediction, expected));
}

TEST(JointProcessModel_IID_Tests, dynamic_dynamic_prediction)
{
    using namespace fl;

    typedef LinearGaussianProcessModel<Eigen::Matrix<double, -1, 1>> LocalModel;
    typedef typename Traits<LocalModel>::SecondMoment Cov;

    typedef JointProcessModel<MultipleOf<LocalModel, -1>> JointModel;

    auto f = JointModel(LocalModel(Cov::Identity(5,5) * 9.0, 5), 3);

    auto state = Traits<JointModel>::State(f.state_dimension(), 1);
    auto noise = Traits<JointModel>::Noise(f.noise_dimension(), 1);
    auto input = Traits<JointModel>::Input(f.input_dimension(), 1);

    state.setZero();
    noise.setOnes();
    input.setZero();

    auto prediction = f.predict_state(1., state, noise, input);

    auto expected = Eigen::Matrix<double, 15, 1>::Ones().eval();
    expected.topRows(5) = expected.topRows(5) * 3.;
    expected.middleRows(5, 5) = expected.middleRows(5, 5) * 3.;
    expected.bottomRows(5) = expected.bottomRows(5) * 3.0;

    EXPECT_TRUE(fl::are_similar(prediction, expected));
}


#include <iostream>

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Odometry.h>
#include <aruco/aruco.h>
#include <aruco/cvdrawingutils.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/SVD>
// Added dependency for inverse and cross product
#include <Eigen/Geometry>
//EIgen SVD libnary, may help you solve SVD
//JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);

#define DEBUG false

using namespace cv;
using namespace aruco;
using namespace Eigen;

//global varialbles for aruco detector
aruco::CameraParameters CamParam;
MarkerDetector MDetector;
vector<Marker> Markers;
float MarkerSize = 0.20 / 1.5 * 1.524;
float MarkerWithMargin = MarkerSize * 1.2;
BoardConfiguration TheBoardConfig;
BoardDetector TheBoardDetector;
Board TheBoardDetected;
ros::Publisher pub_odom_yourwork;
ros::Publisher pub_odom_ref;
cv::Mat K, D;

// Beck's insert for non-linear 3D-2D pose estimation
Vector3d euler_past; // Euler angle in ZYX from last frame
Vector3d T_rec_past; // recursive T from last frame

// test function, can be used to verify your estimation
void calculateReprojectionError(const vector<cv::Point3f> &pts_3, const vector<cv::Point2f> &pts_2, const cv::Mat R, const cv::Mat t)
{
    puts("calculateReprojectionError begins");
    vector<cv::Point2f> un_pts_2;
    cv::undistortPoints(pts_2, un_pts_2, K, D);
    for (unsigned int i = 0; i < pts_3.size(); i++)
    {
        cv::Mat p_mat(3, 1, CV_64FC1);
        p_mat.at<double>(0, 0) = pts_3[i].x;
        p_mat.at<double>(1, 0) = pts_3[i].y;
        p_mat.at<double>(2, 0) = pts_3[i].z;
        cv::Mat p = (R * p_mat + t);
        printf("(%f, %f, %f) -> (%f, %f) and (%f, %f)\n",
               pts_3[i].x, pts_3[i].y, pts_3[i].z,
               un_pts_2[i].x, un_pts_2[i].y,
               p.at<double>(0) / p.at<double>(2), p.at<double>(1) / p.at<double>(2));
    }
    puts("calculateReprojectionError ends");
}

// the main function you need to work with
// pts_id: id of each point
// pts_3: 3D position (x, y, z) in world frame
// pts_2: 2D position (u, v) in image frame
void process(const vector<int> &pts_id, const vector<cv::Point3f> &pts_3, const vector<cv::Point2f> &pts_2, const ros::Time& frame_time)
{
    //version 1, as reference
    cv::Mat r, rvec, t;
    cv::solvePnP(pts_3, pts_2, K, D, rvec, t);
    cv::Rodrigues(rvec, r);
    Matrix3d R_ref;
    for(int i=0;i<3;i++)
        for(int j=0;j<3;j++)
        {
            R_ref(i,j) = r.at<double>(i, j);
        }
    Quaterniond Q_ref;
    Q_ref = R_ref;
    nav_msgs::Odometry odom_ref;
    odom_ref.header.stamp = frame_time;
    odom_ref.header.frame_id = "world";
    odom_ref.pose.pose.position.x = t.at<double>(0, 0);
    odom_ref.pose.pose.position.y = t.at<double>(1, 0);
    odom_ref.pose.pose.position.z = t.at<double>(2, 0);
    odom_ref.pose.pose.orientation.w = Q_ref.w();
    odom_ref.pose.pose.orientation.x = Q_ref.x();
    odom_ref.pose.pose.orientation.y = Q_ref.y();
    odom_ref.pose.pose.orientation.z = Q_ref.z();
    pub_odom_ref.publish(odom_ref);

    // version 2, your work
    Matrix3d R;
    Vector3d T;
    R.setIdentity();
    T.setZero();
    ROS_INFO("write your code here!");
    // Version 1 Linear 3D-2D pose estimation on planar scene
    // H = K (r1, r2, t), Hf stands for H flat
    // Using four points to build the H first
    // preprocessing the points
    vector<cv::Point2f> un_pts_2;
    cv::undistortPoints(pts_2, un_pts_2, K, D);
    int pointsNumber = pts_2.size();

    MatrixXd P0(2*pointsNumber, 9);
    MatrixXd P(8,9);

    for (int i = 0; i < pointsNumber; i++) {
        float X, Y, u, v;
        X = pts_3[i].x;
        Y = pts_3[i].y;
        u = un_pts_2[i].x;
        v = un_pts_2[i].y;
        P0.row(2*i  ) << X, Y, 1, 0, 0, 0, -X*u, -Y*u, -u;
        P0.row(2*i+1) << 0, 0, 0, X, Y, 1, -X*v, -Y*v, -v;
    }
    P = P0.topRows(8);

    // Using all of the points to find the linear pose estimation
    JacobiSVD<MatrixXd> svd(P0, ComputeThinU | ComputeThinV);

    MatrixXd V(9,9);
    V = svd.matrixV();

    VectorXd Hf = V.col(8);
    Matrix3d H;

    H << Hf(0), Hf(1), Hf(2),
         Hf(3), Hf(4), Hf(5),
         Hf(6), Hf(7), Hf(8);

    // Find the orthogonal matrix R, with the estimate h1_bar and h2_bar
    Vector3d h1_bar;
    Vector3d h2_bar;
    Vector3d h3_bar;
    Vector3d h12_bar_cross;
    h1_bar = H.col(0);
    h2_bar = H.col(1);
    h3_bar = H.col(2);
    h12_bar_cross = h1_bar.cross(h2_bar);
    Matrix3d R_approx;
    R_approx << h1_bar, h2_bar, h12_bar_cross;
    JacobiSVD<MatrixXd> svd_min(R_approx, ComputeThinU | ComputeThinV);

    Matrix3d R_linear;
    Vector3d T_linear;
    R_linear = svd_min.matrixU() * svd_min.matrixV().transpose();

    double h1_norm = h1_bar.norm();
    T_linear = h3_bar / h1_norm;

    // Since the direction of the camera depends on the z axis
    // Normalize themt
    if (T_linear(2) < 0) {
        T_linear = -T_linear;
        R_linear.col(0) = -R_linear.col(0);
        R_linear.col(1) = -R_linear.col(1);
    }

    // Second phase: using nonlinear 3D-2D Newton Method
    Vector2d b_sum;
    MatrixXd A_sum(2,6);

    Vector3d euler_init;// Euler angle in ZYX, initial
    Vector3d T_init;  // initial T
    Vector3d euler;// Euler angle in ZYX, initial
    Vector3d T_rec;  // recursive T

    euler_init = R_linear.eulerAngle(2,1,0); // Euler angle in ZYX
    T_init = T_linear；
    // Step 1: compute the initial Euler angle and T
    for (int i = 0; i < pointsNumber; i++) {
        Vector2d f0;
        Vector3d p0 << pts_3[i].x, pts_3[i].y, pts_3[i].z;
        Vector3d pos3d_rot = R_linear * p0 + T_linear;
        Vector2d pos3d_rot_2d;
        pos3d_rot_2d << pos3d_rot(0) / pos3d_rot(2), pos3d_rot(1) / pos3d_rot(2); // x/z, y/z
        f0 << un_pts_2[i].x - pos3d_rot_2d(0), un_pts_2[i].y - pos3d_rot_2d(1);
        b_sum += f0;
    }

    // Step 2: get the derivative matrix from Matlab, and get the A
    for (int i = 0; i < pointsNumber; i++) {
        MatrixXd Jacobian(2,6);

        // Jacobian Matrix from Matlab
        /*
        R =

        [ cos(a)*cos(b), cos(a)*sin(b)*sin(y) - sin(a)*cos(y), sin(a)*sin(y) + cos(a)*sin(b)*cos(y)]
        [ cos(b)*sin(a), cos(a)*cos(y) + sin(a)*sin(b)*sin(y), sin(a)*sin(b)*cos(y) - cos(a)*sin(y)]
        [       -sin(b),                        cos(b)*sin(y),                        cos(b)*cos(y)]

        dRda =

        [ -cos(b)*sin(a), - cos(a)*cos(y) - sin(a)*sin(b)*sin(y), cos(a)*sin(y) - sin(a)*sin(b)*cos(y)]
        [  cos(a)*cos(b),   cos(a)*sin(b)*sin(y) - sin(a)*cos(y), sin(a)*sin(y) + cos(a)*sin(b)*cos(y)]
        [              0,                                      0,                                    0]

        dRdb =

        [ -cos(a)*sin(b), cos(a)*cos(b)*sin(y), cos(a)*cos(b)*cos(y)]
        [ -sin(a)*sin(b), cos(b)*sin(a)*sin(y), cos(b)*sin(a)*cos(y)]
        [        -cos(b),       -sin(b)*sin(y),       -sin(b)*cos(y)]

        dRdy =

        [ 0, sin(a)*sin(y) + cos(a)*sin(b)*cos(y),   sin(a)*cos(y) - cos(a)*sin(b)*sin(y)]
        [ 0, sin(a)*sin(b)*cos(y) - cos(a)*sin(y), - cos(a)*cos(y) - sin(a)*sin(b)*sin(y)]
        [ 0,                        cos(b)*cos(y),                         -cos(b)*sin(y)]
        */
    }
    // Step 3: calculate the increment
    VectorXd increment(6,1);
    increment = b_sum / A_sum;
    euler += increment.segment(0,2);
    T_rec += increment.segment(3,5);

    // Step 4: convert the incremented euler angle to rotation matrix
    double roll = euler(0);
    double yaw  = euler(1);
    double pitch= euler(2);
    AngleAxisd rollAngle(roll, Vector3d::UnitZ());
    AngleAxisd yawAngle(yaw, Vector3d::UnitY());
    AngleAxisd pitchAngle(pitch, Vector3d::UnitX());

    Quaternion<double> quaternion = rollAngle * yawAngle * pitchAngle;

    Matrix3d rotationMatrix = quaternion.matrix();

    R = rotationMatrix;
    T = T_rec;

    Quaterniond Q_yourwork;
    Q_yourwork = R;
    nav_msgs::Odometry odom_yourwork;
    odom_yourwork.header.stamp = frame_time;
    odom_yourwork.header.frame_id = "world";
    odom_yourwork.pose.pose.position.x = T(0);
    odom_yourwork.pose.pose.position.y = T(1);
    odom_yourwork.pose.pose.position.z = T(2);
    odom_yourwork.pose.pose.orientation.w = Q_yourwork.w();
    odom_yourwork.pose.pose.orientation.x = Q_yourwork.x();
    odom_yourwork.pose.pose.orientation.y = Q_yourwork.y();
    odom_yourwork.pose.pose.orientation.z = Q_yourwork.z();
    pub_odom_yourwork.publish(odom_yourwork);
}

cv::Point3f getPositionFromIndex(int idx, int nth)
{
    int idx_x = idx % 6, idx_y = idx / 6;
    double p_x = idx_x * MarkerWithMargin - (3 + 2.5 * 0.2) * MarkerSize;
    double p_y = idx_y * MarkerWithMargin - (12 + 11.5 * 0.2) * MarkerSize;
    return cv::Point3f(p_x + (nth == 1 || nth == 2) * MarkerSize, p_y + (nth == 2 || nth == 3) * MarkerSize, 0.0);
}

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    double t = clock();
    cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
    MDetector.detect(bridge_ptr->image, Markers);
    float probDetect = TheBoardDetector.detect(Markers, TheBoardConfig, TheBoardDetected, CamParam, MarkerSize);
    ROS_DEBUG("p: %f, time cost: %f\n", probDetect, (clock() - t) / CLOCKS_PER_SEC);

    vector<int> pts_id;
    vector<cv::Point3f> pts_3;
    vector<cv::Point2f> pts_2;
    for (unsigned int i = 0; i < Markers.size(); i++)
    {
        int idx = TheBoardConfig.getIndexOfMarkerId(Markers[i].id);

        char str[100];
        sprintf(str, "%d", idx);
        cv::putText(bridge_ptr->image, str, Markers[i].getCenter(), CV_FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(-1));
        for (unsigned int j = 0; j < 4; j++)
        {
            sprintf(str, "%d", j);
            cv::putText(bridge_ptr->image, str, Markers[i][j], CV_FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(-1));
        }

        for (unsigned int j = 0; j < 4; j++)
        {
            pts_id.push_back(Markers[i].id * 4 + j);
            pts_3.push_back(getPositionFromIndex(idx, j));
            pts_2.push_back(Markers[i][j]);
        }
    }

    //begin your function
    if (pts_id.size() > 5)
        process(pts_id, pts_3, pts_2, img_msg->header.stamp);

    cv::imshow("in", bridge_ptr->image);
    cv::waitKey(10);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tag_detector");
    ros::NodeHandle n("~");

    ros::Subscriber sub_img = n.subscribe("image_raw", 100, img_callback);
    pub_odom_yourwork = n.advertise<nav_msgs::Odometry>("odom_yourwork",10);
    pub_odom_ref = n.advertise<nav_msgs::Odometry>("odom_ref",10);
    //init aruco detector
    string cam_cal, board_config;
    n.getParam("cam_cal_file", cam_cal);
    n.getParam("board_config_file", board_config);
    CamParam.readFromXMLFile(cam_cal);
    TheBoardConfig.readFromFile(board_config);

    //init intrinsic parameters
    cv::FileStorage param_reader(cam_cal, cv::FileStorage::READ);
    param_reader["camera_matrix"] >> K;
    param_reader["distortion_coefficients"] >> D;

    //init window for visualization
    cv::namedWindow("in", 1);

    ros::spin();
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: BLAS level 3 functionality (blas3.cpp and blas3.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

//disable debug mechanisms to have a fair comparison with ublas:
#ifndef NDEBUG
 #define NDEBUG
#endif

//
// include necessary system headers
//
#include <iostream>

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/prod.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

#include "../benchmarks/benchmark-utils.hpp"

#define BLAS3_MATRIX_SIZE   400

using namespace boost::numeric;

#ifndef VIENNACL_WITH_OPENCL
  struct dummy
  {
    std::size_t size() const { return 1; }
  };
#endif

int main()
{
  typedef float     ScalarType;

  Timer timer;
  double exec_time;

  //
  // Set up some ublas objects
  //
  ublas::matrix<ScalarType> ublas_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType, ublas::column_major> ublas_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C1(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  //
  // Fill the matrix
  //
  for (unsigned int i = 0; i < ublas_A.size1(); ++i)
    for (unsigned int j = 0; j < ublas_A.size2(); ++j)
    {
      ublas_A(i,j) = random<ScalarType>();
    }

  for (unsigned int i = 0; i < ublas_B.size1(); ++i)
    for (unsigned int j = 0; j < ublas_B.size2(); ++j)
    {
      ublas_B(i,j) = random<ScalarType>();
    }

  //
  // Set up some ViennaCL objects
  //
  //viennacl::ocl::set_context_device_type(0, viennacl::ocl::gpu_tag());  //uncomment this is you wish to use GPUs only
  viennacl::matrix<ScalarType> vcl_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType, viennacl::column_major> vcl_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType> vcl_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  /////////////////////////////////////////////////
  //////////// Matrix-matrix products /////////////
  /////////////////////////////////////////////////

  //
  // Compute reference product using ublas:
  //
  std::cout << "--- Computing matrix-matrix product using ublas ---" << std::endl;
  timer.start();
  ublas_C = ublas::prod(ublas_A, ublas_B);
  exec_time = timer.get();
  std::cout << " - Execution time: " << exec_time << std::endl;

  //
  // Now iterate over all OpenCL devices in the context and compute the matrix-matrix product
  //
  std::cout << std::endl << "--- Computing matrix-matrix product on each available compute device using ViennaCL ---" << std::endl;
#ifdef VIENNACL_WITH_OPENCL
  std::vector<viennacl::ocl::device> devices = viennacl::ocl::current_context().devices();
#else
  dummy devices;
#endif

  for (std::size_t i=0; i<devices.size(); ++i)
  {
#ifdef VIENNACL_WITH_OPENCL
    viennacl::ocl::current_context().switch_device(devices[i]);
    std::cout << " - Device Name: " << viennacl::ocl::current_device().name() << std::endl;
#endif

    viennacl::copy(ublas_A, vcl_A);
    viennacl::copy(ublas_B, vcl_B);
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    timer.start();
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    exec_time = timer.get();
    std::cout << " - Execution time on device (no setup time included): " << exec_time << std::endl;

    //
    // Verify the result
    //
    viennacl::copy(vcl_C, ublas_C1);

    std::cout << " - Checking result... ";
    bool check_ok = true;
    for (std::size_t i = 0; i < ublas_A.size1(); ++i)
    {
      for (std::size_t j = 0; j < ublas_A.size2(); ++j)
      {
        if ( std::fabs(ublas_C1(i,j) - ublas_C(i,j)) / ublas_C(i,j) > 1e-4 )
        {
          check_ok = false;
          break;
        }
      }
      if (!check_ok)
        break;
    }
    if (check_ok)
      std::cout << "[OK]" << std::endl << std::endl;
    else
      std::cout << "[FAILED]" << std::endl << std::endl;

  }

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  return EXIT_SUCCESS;
}


// ======================================================================
/*!
 * \file
 * \brief Regression tests for Fmi::NearTreeLatLon
 */
// ======================================================================

#include "NearTree.h"
#include "NearTreeLatLon.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <regression/tframe.h>
#include <iostream>
#include <sstream>
#include <string>

using namespace std;
using namespace Fmi;

using NearTreeType = NearTreeLatLon<int>;

NearTree<NearTreeType, NearTreeLatLonDistance<NearTreeType>> tree;

const int Helsinki = 1;
const int Tampere = 2;
const int Turku = 3;
const int Oulu = 4;
const int Kuopio = 5;
const int Rovaniemi = 6;
const int Hanko = 7;
const int Utsjoki = 8;

map<int, string> names = {make_pair(Helsinki, "Helsinki"),
                          make_pair(Tampere, "Tampere"),
                          make_pair(Turku, "Turku"),
                          make_pair(Oulu, "Oulu"),
                          make_pair(Kuopio, "Kuopio"),
                          make_pair(Rovaniemi, "Rovaniemi"),
                          make_pair(Hanko, "Hanko"),
                          make_pair(Utsjoki, "Utsjoki")};

void prepare()
{
  tree.insert(NearTreeType(24.9354, 60.1695, Helsinki));
  tree.insert(NearTreeType(23.8702, 61.6074, Tampere));
  tree.insert(NearTreeType(22.3333, 60.5333, Turku));
  tree.insert(NearTreeType(25.9694, 65.2173, Oulu));
  tree.insert(NearTreeType(27.8957, 62.9472, Kuopio));
  tree.insert(NearTreeType(25.9525, 66.60, Rovaniemi));
  tree.insert(NearTreeType(22.95, 59.833, Hanko));
  tree.insert(NearTreeType(27.0284, 69.908, Utsjoki));
  tree.flush();
}

namespace TreeTest
{
// ----------------------------------------------------------------------

void size()
{
  if (tree.size() != 8)
    TEST_FAILED("Test tree size should be 8, not " + boost::lexical_cast<string>(tree.size()));
  TEST_PASSED();
}

// ----------------------------------------------------------------------

void empty()
{
  if (tree.empty())
    TEST_FAILED("Test tree should not be empty");
  TEST_PASSED();
}

// ----------------------------------------------------------------------

void nearest()
{
  {
    auto ret = tree.nearest(NearTreeType(25, 60));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 25,60");
    if (names.at(ret->ID()) != "Helsinki")
      TEST_FAILED("Closest point to 25,60 should be Helsinki, not " + names.at(ret->ID()));
  }

  {
    auto ret = tree.nearest(NearTreeType(0, 0));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 0,0");
    if (names.at(ret->ID()) != "Hanko")
      TEST_FAILED("Closest point to 0,0 should be Hanko, not " + names.at(ret->ID()));
  }

  {
    auto ret = tree.nearest(NearTreeType(25, 80));
    if (!ret)
      TEST_FAILED("Failed to find nearest point for 25,80");
    if (names.at(ret->ID()) != "Utsjoki")
      TEST_FAILED("Closest point to 25,80 should be Utsjoki, not " + names.at(ret->ID()));
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------

void farthest()
{
  {
    auto ret = tree.farthest(NearTreeType(0, 0));
    if (!ret)
      TEST_FAILED("Failed to find farthest point for 0,0");
    if (names.at(ret->ID()) != "Utsjoki")
      TEST_FAILED("Farthest point to 0,0 should be Utsjoki, not " + names.at(ret->ID()));
  }

  {
    auto ret = tree.farthest(NearTreeType(25, 80));
    if (!ret)
      TEST_FAILED("Failed to find farthest point for 25,80");
    if (names.at(ret->ID()) != "Hanko")
      TEST_FAILED("Farthest point to 25,80 should be Hanko, not " + names.at(ret->ID()));
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------

void nearest_many()
{
  auto ret = tree.nearestones(NearTreeType(25, 60), NearTreeType::ChordLength(200));
  if (ret.size() != 4)
  {
    // print(ret);
    TEST_FAILED("Failed to find 4 nearest points for 25,60 with radius 200 km");
  }

  ret = tree.nearestones(NearTreeType(25, 70), NearTreeType::ChordLength(400));
  if (ret.size() != 2)
  {
    // print(ret);
    TEST_FAILED("Failed to find 2 nearest points for 25,70 with radius 400 km");
  }

  TEST_PASSED();
}

// ----------------------------------------------------------------------
/*!
 * The actual test suite
 */
// ----------------------------------------------------------------------

class tests : public tframe::tests
{
  virtual const char* error_message_prefix() const { return "\n\t"; }
  void test(void)
  {
    TEST(size);
    TEST(empty);
    TEST(nearest);
    TEST(farthest);
    TEST(nearest_many);
  }
};

}  // namespace TreeTest

//! The main program
int main(void)
{
  prepare();

  using namespace std;
  cout << endl << "Tree" << endl << "==========" << endl;
  TreeTest::tests t;
  return t.run();
}

// ======================================================================


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Stanford University
*  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 Rice University 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.
*********************************************************************/

/* Author: Javier V. Gomez */
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/fmt/FMT.h>
#include <ompl/util/PPM.h>
#include <ompl/base/PredefinedStateSampler.h>

#include <ompl/config.h>
#include <../tests/resources/config.h>

#include <boost/filesystem.hpp>
#include <iostream>
#include <fstream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

bool heuristics = false;
bool nearestK = true;
bool cacheCC = true;

void preSampleFree(const ob::SpaceInformationPtr si, const size_t n, std::vector<const ob::State*> &states)
{
    size_t statesSampled = 0;
    ob::StateSamplerPtr sampler = si->allocStateSampler();
    states.reserve(n);
    ob::State* s = si->allocState();

    while (statesSampled < n)
    {
        sampler->sampleUniform(s);
        bool collision_free = si->isValid(s);
        if (collision_free)
        {
            ++statesSampled;
            states.push_back(s);
            s = si->allocState();
        } // If collision free
    } // While statesSampled < n
}

struct PredefinedStateSamplerAllocator
{
    PredefinedStateSamplerAllocator(const std::vector<const ob::State*> &states) : states_(states) {}

    ob::StateSamplerPtr operator()(const ob::StateSpace* space)
    {
        return ob::StateSamplerPtr(new ob::PredefinedStateSampler(space, states_));
    }

    void saveSamples(const char * filename)
    {
        std::ofstream ofs;
        ofs.open(filename, std::ofstream::trunc);
        ofs << std::fixed;

        for (std::size_t i = 0; i < states_.size(); ++i)
        {
            ofs << states_[i]->as<ob::RealVectorStateSpace::StateType>()->values[0] << " "
                << states_[i]->as<ob::RealVectorStateSpace::StateType>()->values[1];

            if (i != states_.size()-1)
                ofs << std::endl;
        }
        ofs.close();
    }

    const std::vector<const ob::State*>& states_;
};

template<typename planner_t>
class Test2DDataGenerator
{
public:

    Test2DDataGenerator(const char *ppm_file)
    {
        bool ok = false;
        try
        {
            ppm_.loadFile(ppm_file);
            ok = true;
        }
        catch(ompl::Exception &ex)
        {
            OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
        }
        if (ok)
        {
            ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
            space->addDimension(0.0, ppm_.getWidth());
            space->addDimension(0.0, ppm_.getHeight());
            maxWidth_ = ppm_.getWidth() - 1;
            maxHeight_ = ppm_.getHeight() - 1;
            ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));

            // set state validity checking for this space
            ss_->setStateValidityChecker(boost::bind(&Test2DDataGenerator::isStateValid, this, _1));
            space->setup();
            ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());

            // configuring underlying planner
            ss_->setPlanner(ob::PlannerPtr( new planner_t(ss_->getSpaceInformation()) ));
        }
    }

    bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col, double max_time)
    {
        if (!ss_)
            return false;
        ob::ScopedState<> start(ss_->getStateSpace());
        start[0] = start_row;
        start[1] = start_col;
        ob::ScopedState<> goal(ss_->getStateSpace());
        goal[0] = goal_row;
        goal[1] = goal_col;
        ss_->setStartAndGoalStates(start, goal);
        ss_->solve(max_time);

        if (ss_->haveSolutionPath())
            return true;
        else
            return false;
    }

    const ob::SpaceInformationPtr getSpaceInformation() const
   {
       return ss_->getSpaceInformation();
   }

    const og::SimpleSetupPtr getSetup() const
   {
       return ss_;
   }

    const ob::PlannerPtr getPlanner() const
    {
        return ss_->getPlanner();
    }

    void setStateSamplerAllocator(const ob::StateSamplerAllocator ssa)
    {
        ss_->getStateSpace()->setStateSamplerAllocator(ssa);
    }

private:

    bool isStateValid(const ob::State *state) const
    {
        const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
        const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);

        const ompl::PPM::Color &c = ppm_.getPixel(h, w);
        return c.red > 127 && c.green > 127 && c.blue > 127;
    }

    og::SimpleSetupPtr ss_;
    int maxWidth_;
    int maxHeight_;
    ompl::PPM ppm_;
};

int main(int argc, char ** argv)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);

    size_t exp = 3, n_samples[] = {1000, 5000, 10000};
    double max_time = 20.0;

    // Local scope to load the state sampler
    std::vector<const ob::State*> states;
    {
        Test2DDataGenerator<og::FMT> aux((path / "ppm/floor.ppm").string().c_str());
        preSampleFree(aux.getSpaceInformation(), n_samples[exp-1], states);
    }
    PredefinedStateSamplerAllocator pssa(states);
    pssa.saveSamples("fmttest_samples.txt");

    // Saving header of the results file.
    std::stringstream fmtss;
    fmtss << std::fixed;
    fmtss << exp << " " << max_time;
    for (size_t i = 0; i < exp; ++i)
        fmtss << " " << n_samples[i];
    fmtss << std::endl;

    for (size_t i = 0; i < exp; ++i)
    {
        Test2DDataGenerator<og::FMT> test_planner((path / "ppm/floor.ppm").string().c_str());
        test_planner.getPlanner()->as<og::FMT>()->setNumSamples(n_samples[i]);
        test_planner.setStateSamplerAllocator(pssa);

        test_planner.getPlanner()->as<og::FMT>()->setNearestK(nearestK);
        //test_planner.getPlanner()->as<og::FMT>()->setHeuristics(heuristics);
        //test_planner.getPlanner()->as<og::FMT>()->setCacheCC(cacheCC);

        if (test_planner.plan(0,0, 1859, 1568, max_time)
            && test_planner.getSetup()->haveExactSolutionPath())
        {
            const double cost = test_planner.getSetup()->getSolutionPath().length();
            const double time = test_planner.getSetup()->getLastPlanComputationTime();
            const unsigned int n_states = test_planner.getSetup()->getSolutionPath().getStateCount();
            fmtss << cost << " " << time << " " << n_states << std::endl;
            test_planner.getSetup()->getSolutionPath().printAsMatrix(fmtss);
        }
        else
        {
            std::cout << "***** PLANNING FAILED ***** Please run again." << std::endl;
            return 1;
        }
    }

    std::ofstream fmtssofs;
    fmtssofs.open("fmttest_paths.txt", std::ofstream::trunc);
    fmtssofs << fmtss.rdbuf();
    fmtssofs.close();

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTION_SCALAR_FAST_SINCOSD_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTION_SCALAR_FAST_SINCOSD_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>

#include <nt2/toolbox/trigonometric/function/scalar/impl/trigo.hpp>
#include <nt2/include/functions/tofloat.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fast_sincosd_, tag::cpu_,
                               (A0),
                               (real_<A0>)
                              )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fast_sincosd_(tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::strip<A0>::type                      etype;
      typedef boost::fusion::tuple<etype, etype>                   type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      typedef typename std::tr1::result_of<meta::floating(A0)>::type etype;
      impl::trig_base < A0,degree_tag
                      , fast_tag, tag::not_simd_type
                      >::sincosa( etype(a0)
                                , boost::fusion::at_c<0>(res)
                                , boost::fusion::at_c<1>(res)
                                );
      return res;
    }

  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::fast_sincosd_, tag::cpu_,
                               (A0),
                               (arithmetic_<A0>)
                              )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::fast_sincosd_(tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type etype;
      typedef boost::fusion::tuple<etype, etype>                   type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      typedef typename std::tr1::result_of<meta::floating(A0)>::type etype;
      return nt2::fast_sincosd(tofloat(a0));
    }

  };
} }

#endif
// modified by jt the 26/12/2010


//   Copyright (c) 2001-2009 Hartmut Kaiser
//   Copyright (c) 2001-2009 Joel de Guzman
//
//   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)

///////////////////////////////////////////////////////////////////////////////
//
//  A complex number micro generator - take 2. Look'ma no semantic actions!
//
//  [ HK July 26, 2009 ]  spirit2
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/fusion/include/std_pair.hpp>

#include <iostream>
#include <string>
#include <complex>

namespace client
{
    ///////////////////////////////////////////////////////////////////////////
    //  Our complex number parser/compiler (that's just a copy of the complex
    //  number example from Qi (see examples/qi/complex_number.cpp)
    ///////////////////////////////////////////////////////////////////////////
    template <typename Iterator>
    bool parse_complex(Iterator first, Iterator last, std::complex<double>& c)
    {
        using boost::spirit::qi::double_;
        using boost::spirit::qi::_1;
        using boost::spirit::qi::phrase_parse;
        using boost::spirit::ascii::space;
        using boost::phoenix::ref;

        double rN = 0.0;
        double iN = 0.0;
        bool r = phrase_parse(first, last,
            (
                    '(' >> double_[ref(rN) = _1]
                        >> -(',' >> double_[ref(iN) = _1]) >> ')'
                |   double_[ref(rN) = _1]
            ),
            space);

        if (!r || first != last) // fail if we did not get a full match
            return false;
        c = std::complex<double>(rN, iN);
        return r;
    }

    ///////////////////////////////////////////////////////////////////////////
    //  Our complex number generator
    ///////////////////////////////////////////////////////////////////////////
    //[tutorial_karma_complex_number_easier
    template <typename OutputIterator>
    bool generate_complex(OutputIterator sink, std::complex<double> const& c)
    {
        using boost::spirit::karma::double_;
        using boost::spirit::karma::omit;
        using boost::spirit::karma::generate;

        return generate(sink,

            //  Begin grammar
            (
               !double_(0.0) << '(' << double_ << ", " << double_ << ')'
            |   omit[double_] << double_ << omit[double_]
            ),
            //  End grammar

            c.imag(), c.real(), c.imag()     //  Data to output
        );
    }
    //]
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "\t\tA complex number micro generator for Spirit...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";

    std::cout << "Give me a complex number of the form r or (r) or (r,i) \n";
    std::cout << "Type [q or Q] to quit\n\n";

    std::string str;
    while (getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        std::complex<double> c;
        if (client::parse_complex(str.begin(), str.end(), c))
        {
            std::cout << "-------------------------\n";

            std::string generated;
            std::back_insert_iterator<std::string> sink(generated);
            if (!client::generate_complex(sink, c))
            {
                std::cout << "-------------------------\n";
                std::cout << "Generating failed\n";
                std::cout << "-------------------------\n";
            }
            else
            {
                std::cout << "-------------------------\n";
                std::cout << "Generated: " << generated << "\n";
                std::cout << "-------------------------\n";
            }
        }
        else
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "-------------------------\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/geometries/point.hpp>

template <std::size_t D, typename P, typename T>
void test_get(T const& c, T const& e)
{
    //std::cout << "get_as_radian " << typeid(P).name() << std::endl;

    typedef typename bg::coordinate_type<P>::type coordinate_type;

    P p;
    bg::set<D>(p, coordinate_type(c));

    coordinate_type g = bg::get_as_radian<D>(p);

    BOOST_CHECK_CLOSE(double(g), double(e), 0.0001);
}

template <std::size_t D, typename P, typename T>
void test_set(T const& c, T const& e)
{
    //std::cout << "set_from_radian " << typeid(P).name() << std::endl;

    typedef typename bg::coordinate_type<P>::type coordinate_type;

    P p;
    bg::set_from_radian<D>(p, coordinate_type(c));

    coordinate_type g = bg::get<D>(p);

    BOOST_CHECK_CLOSE(double(g), double(e), 0.0001);
}

template <typename T>
void test()
{
    double d2r = 3.1415926535897932384626433832795 / 180.0;

    // Degree
    test_get<0, bg::model::point<T, 2, bg::cs::spherical<bg::degree> > >
        (1.0, 1.0 * d2r);
    test_get<1, bg::model::point<T, 2, bg::cs::spherical<bg::degree> > >
        (2.0, 2.0 * d2r);

    test_set<0, bg::model::point<T, 2, bg::cs::spherical<bg::degree> > >
        (1.0, 1.0 / d2r);
    test_set<1, bg::model::point<T, 2, bg::cs::spherical<bg::degree> > >
        (2.0, 2.0 / d2r);

    // Radian
    test_get<0, bg::model::point<T, 2, bg::cs::spherical<bg::radian> > >
        (1.0, 1.0);
    test_get<1, bg::model::point<T, 2, bg::cs::spherical<bg::radian> > >
        (1.0, 1.0);

    test_set<0, bg::model::point<T, 2, bg::cs::spherical<bg::radian> > >
        (1.0, 1.0);
    test_set<1, bg::model::point<T, 2, bg::cs::spherical<bg::radian> > >
        (1.0, 1.0);

    // Cartesian (== untouched, no conversion)
    test_get<0, bg::model::point<T, 2, bg::cs::cartesian> >
        (1.0, 1.0);
    test_get<1, bg::model::point<T, 2, bg::cs::cartesian> >
        (1.0, 1.0);

    // Dimension >=2, should always be untouched, even for degree
    // (assuming lat/lon + height)
    test_set<2, bg::model::point<T, 3, bg::cs::spherical<bg::radian> > >
        (1.0, 1.0);
    test_set<2, bg::model::point<T, 3, bg::cs::spherical<bg::degree> > >
        (1.0, 1.0);

}

int test_main(int, char* [])
{
    //test<float>();
    test<double>();

    return 0;
}


#ifndef LVR2_IO_YAML_MATRIX_IO_HPP
#define LVR2_IO_YAML_MATRIX_IO_HPP

#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

namespace YAML
{

template <class Scalar_, int A_, int B_, int C_, int D_, int E_>
struct convert<Eigen::Matrix<Scalar_, A_, B_, C_, D_, E_> >
{

    /**
     * Encode Eigen matrix to yaml.
     */
    template <class Scalar, int A, int B, int C, int D, int E>
    static Node encode(const Eigen::Matrix<Scalar, A, B, C, D, E>& M)
    {
        typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;
        IndexType rows = M.rows();
        IndexType cols = M.cols();

        Node node;

        node["rows"] = rows;
        node["cols"] = cols;
        node["data"] = Load("[]");

        for (IndexType i = 0; i < rows; ++i) {
            for (IndexType j = 0; j < cols; ++j) {
                node["data"].push_back(M.coeff(i, j));
            }
        }
        return node;
    }

    template <class Scalar, int A, int B, int C, int D, int E>
    static bool decode(const Node& node, Eigen::Matrix<Scalar, A, B, C, D, E>& M)
    {
        typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;

        IndexType rows = node["rows"].as<IndexType>();
        IndexType cols = node["cols"].as<IndexType>();

        size_t expected_size = M.rows() * M.cols();
        if (!node["data"].IsSequence() || node["data"].size() != expected_size) {
            return false;
        }

        YAML::const_iterator it = node["data"].begin();
        YAML::const_iterator it_end = node["data"].end();
        if (rows > 0 && cols > 0) {
            for (IndexType i = 0; i < rows; ++i) {
                for (IndexType j = 0; j < cols; ++j) {
                    M.coeffRef(i, j) = it->as<Scalar>();
                    ++it;
                }
            }
        }
        return true;
    }

    template <class Scalar, int B, int C, int D, int E>
    static bool decode(const Node& node, Eigen::Matrix<Scalar, Eigen::Dynamic, B, C, D, E>& M)
    {

        typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, B, C, D, E>::Index IndexType;
        IndexType rows = node["rows"].as<IndexType>();
        IndexType cols = node["cols"].as<IndexType>();

        M.resize(rows, Eigen::NoChange);

        size_t expected_size = M.rows() * M.cols();
        if (!node["data"].IsSequence() || node["data"].size() != expected_size) {
        return false;
        }

        YAML::const_iterator it = node["data"].begin();
        YAML::const_iterator it_end = node["data"].end();
        if (rows > 0 && cols > 0) {
            for (IndexType i = 0; i < rows; ++i) {
                for (IndexType j = 0; j < cols; ++j) {
                    M.coeffRef(i, j) = it->as<Scalar>();
                    ++it;
                }
            }
        }
        return true;
    }

    /**
     * Specialization for Eigen matrices of dynamic sized columns
     */
    template <class Scalar, int A, int C, int D, int E>
    static bool decode(const Node& node, Eigen::Matrix<Scalar, A, Eigen::Dynamic, C, D, E>& M)
    {
        typedef typename Eigen::Matrix<Scalar, A, Eigen::Dynamic, C, D, E>::Index IndexType;
        IndexType rows = node["rows"].as<IndexType>();
        IndexType cols = node["cols"].as<IndexType>();

        M.resize(Eigen::NoChange, cols);

        size_t expected_size = M.rows() * M.cols();
        if (!node["data"].IsSequence() || node["data"].size() != expected_size) {
            return false;
        }

        YAML::const_iterator it = node["data"].begin();
        YAML::const_iterator it_end = node["data"].end();
        if (rows > 0 && cols > 0) {
            for (IndexType i = 0; i < rows; ++i) {
                for (IndexType j = 0; j < cols; ++j) {
                    M.coeffRef(i, j) = it->as<Scalar>();
                    ++it;
                }
            }
        }
        return true;
    }

    /**
     * Specialization for Eigen matrices of dynamic sized columns and rows
     */
    template <class Scalar, int C, int D, int E>
    static bool decode(const Node& node, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, C, D, E>& M)
    {
        typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, C, D, E>::Index IndexType;
        IndexType rows = node["rows"].as<IndexType>();
        IndexType cols = node["cols"].as<IndexType>();

        M.resize(rows, cols);

        size_t expected_size = M.rows() * M.cols();
        if (!node["data"].IsSequence() || node["data"].size() != expected_size) {
            return false;
        }

        YAML::const_iterator it = node["data"].begin();
        YAML::const_iterator it_end = node["data"].end();
        if (rows > 0 && cols > 0) {
            for (IndexType i = 0; i < rows; ++i) {
                for (IndexType j = 0; j < cols; ++j) {
                    M.coeffRef(i, j) = it->as<Scalar>();
                    ++it;
                }
            }
        }
        return true;
    }
};
}  // namespace YAML

#endif // LVR2_IO_YAML_MATRIX_IO_HPP


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct MultiInputsOutputsFixture : public ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    MultiInputsOutputsFixture()
    {
        // input1 = tf.placeholder(tf.float32, shape=[], name = "input1")
        // input2 = tf.placeholder(tf.float32, shape = [], name = "input2")
        // add1 = tf.add(input1, input2, name = "add1")
        // add2 = tf.add(input1, input2, name = "add2")
        m_Prototext = R"(
node {
  name: "input1"
  op: "Placeholder"
  attr {
    key: "dtype"
    value {
      type: DT_FLOAT
    }
  }
  attr {
    key: "shape"
    value {
      shape {
      }
    }
  }
}
node {
  name: "input2"
  op: "Placeholder"
  attr {
    key: "dtype"
    value {
      type: DT_FLOAT
    }
  }
  attr {
    key: "shape"
    value {
      shape {
      }
    }
  }
}
node {
  name: "add1"
  op: "Add"
  input: "input1"
  input: "input2"
  attr {
    key: "T"
    value {
      type: DT_FLOAT
    }
  }
}
node {
  name: "add2"
  op: "Add"
  input: "input1"
  input: "input2"
  attr {
    key: "T"
    value {
      type: DT_FLOAT
    }
  }
}
        )";
        Setup({ { "input1", { 1 } },
                { "input2", { 1 } } },
              { "add1", "add2" });
    }
};

BOOST_FIXTURE_TEST_CASE(MultiInputsOutputs, MultiInputsOutputsFixture)
{
    RunTest<1>({ { "input1", {12.0f} }, { "input2", { 13.0f } } },
               { { "add1", { 25.0f } }, { "add2", { 25.0f } } });
}

BOOST_AUTO_TEST_SUITE_END()