//#include <dirent.h>
//#include <iostream>
//#include <vector>
//#include <string>
//#include <utility>
//
//#include <Eigen/Core>
//#include <Eigen/LU>
//#include <Eigen/Geometry>
//
//#include <opencv2/opencv.hpp>
//const int G_land_num = 73;
//const int G_train_pic_id_num = 3300;
//const int G_nShape = 47;
//const int G_nVerts = 11510;
//const int G_nFaces = 11540;
//const int G_test_num = 77;
//const int G_iden_num = 77;
//const int G_inner_land_num = 58;
//
//
////#define normalization
//#define perspective
//
//struct Target_type {
//	Eigen::VectorXf exp;
//	Eigen::RowVector3f tslt;
//	Eigen::Matrix3f rot;
//	Eigen::MatrixX2f dis;
//
//};
//
//struct DataPoint
//{
//	cv::Mat image;
//	cv::Rect face_rect;
//	std::vector<cv::Point2d> landmarks;
//	//std::vector<cv::Point2d> init_shape;
//	Target_type shape, init_shape;
//	Eigen::VectorXf user;
//	Eigen::RowVector2f center;
//	Eigen::MatrixX2f land_2d;
//
//#ifdef perspective
//
//	float fcs;
//#endif // posit
//#ifdef normalization
//	Eigen::MatrixX3f s;
//#endif
//
//	Eigen::VectorXi land_cor;
//};
//void data_err_cnt_one(std::string name, Eigen::VectorXi &err_cnt) {
//	std::cout << "load coefficients...file:" << name << "\n";
//	FILE *fp;
//	fopen_s(&fp, name.c_str(), "rb");
//	DataPoint temp;
//	temp.user.resize(G_iden_num);
//	for (int j = 0; j < G_iden_num; j++)
//		fread(&temp.user(j), sizeof(float), 1, fp);
//
//	temp.land_2d.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.land_2d(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.land_2d(i_v, 1), sizeof(float), 1, fp);
//	}
//
//
//	fread(&temp.center(0), sizeof(float), 1, fp);
//	fread(&temp.center(1), sizeof(float), 1, fp);
//
//	temp.shape.exp.resize(G_nShape);
//	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
//		fread(&temp.shape.exp(i_shape), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.shape.rot(i, j), sizeof(float), 1, fp);
//
//	for (int i = 0; i < 3; i++) fread(&temp.shape.tslt(i), sizeof(float), 1, fp);
//
//	temp.land_cor.resize(G_land_num);
//	for (int i_v = 0; i_v < G_land_num; i_v++) fread(&temp.land_cor(i_v), sizeof(int), 1, fp);
//
//#ifdef normalization
//	temp.s.resize(2, 3);
//	for (int i = 0; i < 2; i++) for (int j = 0; j < 3; j++)
//		fread(&temp.s(i, j), sizeof(float), 1, fp);
//#endif // normalization
//
//#ifdef perspective
//	fread(&temp.fcs, sizeof(float), 1, fp);
//	std::cout << "f/tslt_z: " << temp.fcs / temp.shape.tslt(2) << " " << temp.shape.tslt(2) / temp.fcs << "\n";
//
//	//temp.fcs = temp.fcs / temp.shape.tslt(2) * 10;
//	//temp.shape.tslt(2) = 10;
//
//#endif // perspective
//
//	temp.shape.dis.resize(G_land_num, 2);
//	for (int i_v = 0; i_v < G_land_num; i_v++) {
//		fread(&temp.shape.dis(i_v, 0), sizeof(float), 1, fp);
//		fread(&temp.shape.dis(i_v, 1), sizeof(float), 1, fp);
//	}
//
//	fclose(fp);
//
//	puts("load successfully");
//
//
//	float err = sqrt(temp.shape.dis.squaredNorm() / G_land_num / 2);
//	//float err = temp.user.sum()*10;
//
//	printf("err %.5f\n", err);
//	if (err<50) err_cnt((int)(err))++;
//	else {
//		err_cnt(49)++;
//		std::cout << temp.shape.dis << "\n";
//		exit(9);
//	}
//
//}
//
////
//int cnt = 0;
//void data_err_cnt(std::string path, Eigen::VectorXi &err_cnt) {
//
//	struct dirent **namelist;
//	int n;
//	n = scandir(path.c_str(), &namelist, 0, alphasort);
//	if (n < 0)
//	{
//		std::cout << "scandir return " << n << "\n";
//		perror("Cannot open .");
//		exit(1);
//	}
//	else
//	{
//		int index = 0;
//		struct dirent *dp;
//		while (index < n)
//		{
//			dp = namelist[index];
//			std::cout << dp->d_name << ' ' << strlen(dp->d_name) << "\n";
//			if (dp->d_name[0] == '.') {
//				free(namelist[index]);
//				index++;
//				continue;
//			}
//			if (dp->d_type == DT_DIR) {
//				data_err_cnt(path + "/" + dp->d_name, err_cnt);
//			}
//			else {
//				int len = strlen(dp->d_name);
//				if (dp->d_name[len - 3] == 'p' && dp->d_name[len - 4] == 's' &&
//					dp->d_name[len - 1] == 'f' && dp->d_name[len - 2] == '_') {
//					////
//					std::string p = path + "/" + dp->d_name;
//					data_err_cnt_one(p, err_cnt);
//					std::cout << "cnt" << cnt << "\n";
//					cnt++;
//				}
//			}
//			free(namelist[index]);
//			index++;
//		}
//		free(namelist);
//	}
//
//}
//
//
//
//
//std::string fwhs_path = "./fw";
////std::string fwhs_path_p = "./data_me/fw_p1";
//std::string lfw_path = "./lfw_image";
//std::string gtav_path = "./GTAV_image";
//std::string test_path = "D:/sydney/first/data_me/test_lv";
//
//
//
//
//int main() {
//	Eigen::VectorXi err_cnt(50);
//	err_cnt.setZero();
//	data_err_cnt(fwhs_path, err_cnt);
//
//	//data_err_cnt(lfw_path,err_cnt);
//	//data_err_cnt(gtav_path,err_cnt);
//
//	std::cout << "cnt" << cnt << "\n";
//	std::cout << "err cnt" << err_cnt.transpose() << "\n";
//	return 0;
//}
////g++ -Wall -std=c++11 `pkg-config --cflags opencv` -o deal deal_falut.cpp `pkg-config --libs opencv`
//


// EBNF
// OnOff ::= 'ON' | 'OFF'
// Repeat ::= 'R'
// Idle ::= 'IDLE'
// Inactive ::= <Idle> '(' [0-9]+ ')'
// Cycle ::= <OnOff> '(' [0-9]+ ')'
// Sequence ::= <Cycle> (',' <Cycle>)*
// Repetition ::= '[' <Sequence> ']' [0-9]+
// Cycle ::= <Sequence> | <Repetition>
// Cadence ::= <Cycle> (',' <Cycle>)* (',' <Inactive>)? (',' <Repeat>)?

#include <cassert>
#include <cmath>

#include <iostream>
#include <string>

#include <boost/spirit/include/qi.hpp>

#include "snd.h"

static const int kSampleRate = 8000;
static const int kAmplitude = 1.0 * 0x7F000000;
static const int kLeftFreq = 425.0 / kSampleRate;

class Cycle {
public:
	enum cycle_type { kOn, kOff };

	Cycle(cycle_type t) : duration_(0), type_(t) {
	}

	int NumberOfSamples() const {
		return duration_ / 1000 * kSampleRate;
	}

	cycle_type type() const {
		return type_;
	}

private:
	int duration_;
	cycle_type type_;
};

class Cadence {
public:
	Cadence() : repeat_(false), inactive_(0) {
	}

	void Add(const std::string& cycle_str) {
		Cycle::cycle_type type;
		if (cycle_str == "on") {
			type = Cycle::kOn;
		} else if (cycle_str == "off") {
			type = Cycle::kOff;
		} else {
			std::cout << "wrong type: " << cycle_str << std::endl;
			return;
		}
		Cycle cycle(type);
		cycles_.push_back(cycle);
	}

	int TotalSamples() const {
		int count = 0;
		std::vector<Cycle>::const_iterator it = cycles_.begin();
		for (; it != cycles_.end(); ++it) {
			const Cycle& c = *it;
			count += c.NumberOfSamples();
		}
		return count;
	}

	std::vector<Cycle> cycles() const {
		return cycles_;
	}

private:
	std::vector<Cycle> cycles_;
	bool repeat_;
	int inactive_;
};

namespace client {

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

template <typename Iterator>
struct Parser : qi::grammar<Iterator, ascii::space_type> {
	Parser() : Parser::base_type(cadence) {
		using qi::uint_;
		on_off =  qi::lit("ON") | qi::lit("OFF");
		repeat = qi::lit("R");
		idle = qi::lit("IDLE");
		inactive = idle >> '(' >> uint_ >> ')';
		active = on_off >> '(' >> uint_ >> ')';
		sequence = active >> *(',' >> active);
		repetition = '[' >> sequence >> ']' >> uint_;
		cycle = sequence | repetition;
		cadence = cycle >> *(',' >> cycle) >> -(',' >> inactive) >> -(',' >> repeat);
	}

	qi::rule<Iterator, ascii::space_type> on_off;
	qi::rule<Iterator, ascii::space_type> repeat;
	qi::rule<Iterator, ascii::space_type> idle;
	qi::rule<Iterator, ascii::space_type> inactive;
	qi::rule<Iterator, ascii::space_type> active;
	qi::rule<Iterator, ascii::space_type> sequence;
	qi::rule<Iterator, ascii::space_type> repetition;
	qi::rule<Iterator, ascii::space_type> cycle;
	qi::rule<Iterator, ascii::space_type> cadence;
};

}

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

	std::string str;
	std::string filename;
	if (argc == 2) {
		str = "ON(10),OFF(2),IDLE(3),R";
		filename = argv[1];
	} else if (argc == 3) {
		str = argv[1];
		filename = argv[2];
	} else {
		std::cout << "Wrong number of parameters: " << argc << std::endl;
		std::cout << "Usage:" << std::endl;
		std::cout << "ring2pcmu [\"ring tone sequence\"] output.wav" << std::endl;
		return 1;
	}

	// parse cadence
	typedef client::Parser<std::string::const_iterator> Parser;
	Parser grammar;

        std::string::const_iterator itr = str.begin();
        std::string::const_iterator end = str.end();
	bool r = phrase_parse(itr, end, grammar, boost::spirit::ascii::space);

	if (r && itr == end) {
		std::cout << "parse ok" << std::endl;
	} else {
		std::string error_at(itr, end);
		std::cout << "parse failed at: " << error_at << std::endl;
		return 1;
	}

	//translate cadence in to wave
	Cadence cadence;
	std::vector<Cycle> cycles = cadence.cycles();
	int wav_buffer[cadence.TotalSamples()];
	int sample_index = 0;
	std::vector<Cycle>::const_iterator it_c = cycles.begin();
	for (; it_c != cycles.end(); ++it_c) {
		const Cycle& cycle = *it_c;
		for (int j = 0; j < cycle.NumberOfSamples(); ++j) {
			if (cycle.type() == Cycle::kOn) {
				wav_buffer[sample_index] = kAmplitude * sin(kLeftFreq * 2 * j * M_PI);
			} else {
				wav_buffer[sample_index] = 0;
			}
			std::cout << wav_buffer[sample_index] << " ";
			sample_index++;
		}
	}
	std::cout << std::endl;

	// write wave file
	SF_INFO info = snd::GetPcmuInfo(cadence.TotalSamples());
	snd::OutFile output(filename, info);
	assert(output.Good());
	int num_items = info.channels * cadence.TotalSamples();
	int written = output.Write(wav_buffer, num_items);
	if (written != num_items) {
		std::cout << "write errror. " << num_items << "!=" << written << std::endl;
		return 1;
	}

	return 0;
}


//
// Created by huangkun on 2020/11/3.
//

#include <fstream>
#include <experimental/filesystem>

#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>

#include <opengv2/frame/Bodyframe.hpp>
#include <opengv2/system/SystemBase.hpp>
#include <opengv2/viewer/PointLandmarkViewer.hpp>

using namespace opengv2;
using namespace std;

int main(int argc, char **argv) {
    if (argc != 7) {
        cerr << endl
             << "Usage: ./visulization_eventCalib settingFilePath GTFilePath EventTrajectoryFile startTime endTime fps"
             << endl;
        return 1;
    }

    //Check settings file
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "Failed to open settings file at: " << string(argv[1]) << endl;
        exit(-1);
    }

    // map part
    MapBase::Ptr map = make_shared<MapBase>();
    // viewer part
    PointLandmarkViewer::Ptr viewer = make_shared<PointLandmarkViewer>(string(argv[1]), map, nullptr);
    // Extrinsic parameters, to be calibrated
    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> Qsb;
    std::vector<Eigen::Vector3d> tsb;
    Qsb.emplace_back(1, 0, 0, 0);
    tsb.emplace_back(0, 0, 0);

    SystemBase SLAM(nullptr, map, viewer, nullptr, Qsb, tsb, nullptr, nullptr, string(argv[1]));

    // add landmarks
    bool isAsymmetric = true;
    fsSettings["Is_Pattern_Asymmetric"] >> isAsymmetric;
    int rows = fsSettings["BoardSize_Rows"];
    int cols = fsSettings["BoardSize_Cols"];
    double squareSize = fsSettings["Square_Size"];
    if (isAsymmetric) {
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++) {
                auto lm = std::make_shared<LandmarkBase>
                        (j + i * cols, Eigen::Vector3d((2 * j + i % 2) * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    } else {
        for (int i = 0; i < rows; ++i)
            for (int j = 0; j < cols; ++j) {
                auto lm = std::make_shared<LandmarkBase>
                        (j + i * cols, Eigen::Vector3d(j * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    }

    // load trajectory
    std::map<double, Eigen::Matrix<double, 7, 1>> GT_Tws, event_Tws;
    opengv2::SystemBase::loadTUM(string(argv[2]), GT_Tws);
    opengv2::SystemBase::loadTUM(string(argv[3]), event_Tws);

    // load image
    string path = string(argv[3]);
    auto index = path.find_last_of('/');
    string folder = path.substr(0, index);
    std::map<double, cv::Mat> eventImages, undistortedImages;
    for (const auto &entry : experimental::filesystem::directory_iterator(folder + "/image/")) {
        auto beginIdx = entry.path().string().find_last_of('/');
        auto endIdx = entry.path().string().find_last_of('.');
        string rawName = entry.path().string().substr(beginIdx + 1, endIdx - beginIdx - 1);
        double timestamp = std::stod(rawName);
        cv::Mat image = imread(entry.path().string(), cv::IMREAD_COLOR);
        eventImages.emplace(timestamp, image);
    }
    for (const auto &entry : experimental::filesystem::directory_iterator(folder + "/undistortedImage/")) {
        auto beginIdx = entry.path().string().find_last_of('/');
        auto endIdx = entry.path().string().find_last_of('_');
        if (entry.path().string()[endIdx + 1] == 'd')
            continue;
        string rawName = entry.path().string().substr(beginIdx + 1, endIdx - beginIdx - 1);
        double timestamp = std::stod(rawName);
        cv::Mat image = imread(entry.path().string(), cv::IMREAD_COLOR);
        undistortedImages.emplace(timestamp, image);
    }

    SLAM.loadGT(0, GT_Tws);
    cv::namedWindow("Current Frame");
    bool start = true;
    double fps = std::stod(argv[6]);
    std::pair duration(std::stod(argv[4]), std::stod(argv[5]));
    for (auto itr = eventImages.lower_bound(duration.first); itr != eventImages.lower_bound(duration.second); itr++) {
        auto eventItr = event_Tws.lower_bound(itr->first);
        if (eventItr == event_Tws.end()) {
            eventItr--;
        } else if (eventItr != event_Tws.begin()) {
            auto n2 = eventItr--;
            eventItr = std::abs(itr->first - eventItr->first) > std::abs(itr->first - n2->first) ? n2 : eventItr;
        }
        if (!SLAM.map->hasKeyframe(eventItr->first)) {
            Eigen::Quaterniond unitQws(eventItr->second.data());
            Eigen::Vector3d tws(eventItr->second.data() + 4);

            vector<FrameBase::Ptr> frames(1);
            auto bf = std::make_shared<Bodyframe>(frames, eventItr->first, tws, unitQws);
            SLAM.map->addFrame(bf);
        }

        auto undisItr = undistortedImages.lower_bound(itr->first);
        if (undisItr == undistortedImages.end()) {
            undisItr--;
        } else if (undisItr != undistortedImages.begin()) {
            auto n2 = undisItr--;
            undisItr = std::abs(itr->first - undisItr->first) > std::abs(itr->first - n2->first) ? n2 : undisItr;
        }
        cv::Mat undis;
        cv::resize(undisItr->second, undis, cv::Size(itr->second.size[1], itr->second.size[0]));

        cv::Mat dst;
        cv::Mat matArray[] = {undis, itr->second};
        cv::hconcat(matArray, 2, dst);
        cv::imshow("Current Frame", dst);
        cv::waitKey(1000 / fps);
        if (start) {
            cv::waitKey();
            start = false;
        }
    }

    std::cout << "press any key to exit..." << std::endl;
    std::cin.ignore();
    SLAM.shutdown();
    return 0;
}


//
// Created by briand on 2/20/19.
//

#include "IteratedLocalSearch.h"

#undef BOOST_LOG

#include <boost/log/trivial.hpp>
#include <Engines/GlobalHeuristics/Utilities/MetropolisHastings.h>

namespace SmolDock::Heuristics {

    IteratedLocalSearch::IteratedLocalSearch(Score::ScoringFunction* scorFunc_, Optimizer::Optimizer* optimizer_,
                                             unsigned int seed_, IteratedLocalSearch::Parameters params_) :
            scorFunc(scorFunc_), optimizer(optimizer_), rndGenerator(seed_), params(params_) {

    }

    bool IteratedLocalSearch::search() {

        arma::mat currentState = scorFunc->getStartingConditions();
        arma::mat statingState = currentState;
        arma::mat bestState = currentState;

        double score_ = 0.0; //scorFunc->Evaluate(currentState);
        double oldScore = std::numeric_limits<double>::max();
        double bestScore = std::numeric_limits<double>::max();

        double temperature_constant = 10.0;
        double temperature = temperature_constant;
        double best_temperature = 10.0;

        unsigned int restart_count = 0;
        unsigned int metropolis_count = 0;

        unsigned int allowedMetropolisRestartCount = 3;

        while (score_ == 0 || score_ > 10.0) {
            restart_count++;
            std::uniform_real_distribution<double> randomRestartTranslationDistribution(-this->params.proteinMaxRadius,
                                                                                this->params.proteinMaxRadius);
            std::uniform_real_distribution<double> randomRestartQuatDistribution(-3, +3);
            std::uniform_real_distribution<double> randomRestartRotDistribution(-180.0 * (3.14 / 180),
                                                                                +180.0 * (3.14 / 180));

            for (unsigned int i = 0; i < 3; i++) {
                double perturbation = randomRestartTranslationDistribution(this->rndGenerator);
                currentState[i] = statingState[i] + perturbation;
            }

            for (unsigned int i = 3; i < 8; i++) {
                double perturbation = randomRestartQuatDistribution(this->rndGenerator);
                currentState[i] = statingState[i] + perturbation;
            }

            for (unsigned int i = 8; i < currentState.n_rows; i++) {
                double perturbation = randomRestartRotDistribution(this->rndGenerator);
                currentState[i] = statingState[i] + perturbation;
            }

            score_ = scorFunc->Evaluate(currentState);
        }

        optimizer->optimize(currentState);
        score_ = optimizer->getScore();
        currentState = optimizer->getRawResultMatrix();

        BOOST_ASSERT(score_ < oldScore);

        while (true) {

            if (score_ < bestScore) {
                bestScore = score_;
                bestState = currentState;
                best_temperature = temperature;
            }

            metropolis_count++;
            oldScore = score_;
            temperature = temperature_constant * std::pow(0.9, 1 + metropolis_count);
            BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: Temperature    = " << temperature;
            BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: RestartAllowed = " << allowedMetropolisRestartCount;
            BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: BestScore      = " << bestScore;
            BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: CurrentScore   = " << score_;

            std::uniform_real_distribution<double> perturbationTranslationDistribution(-4.0, +4.0);
            std::uniform_real_distribution<double> perturbationQuatDistribution(-2.0, +2.0);
            std::uniform_real_distribution<double> perturbationRotationDistribution(-20.0 * (3.14 / 180),
                                                                                +20.0 * (3.14 / 180));
            for (unsigned int i = 0; i < 3; i++) {
                double perturbation = perturbationTranslationDistribution(this->rndGenerator);
                currentState[i] += perturbation;
            }

            for (unsigned int i = 3; i < 8; i++) {
                double perturbation = perturbationQuatDistribution(this->rndGenerator);
                currentState[i] += perturbation;
            }

            for (unsigned int i = 8; i < currentState.n_rows; i++) {
                double perturbation = perturbationRotationDistribution(this->rndGenerator);
                currentState[i] += perturbation;
            }

            optimizer->optimize(currentState);
            score_ = optimizer->getScore();
            currentState = optimizer->getRawResultMatrix();

            bool metropolisAccept = MetropolisAccept(oldScore, score_, temperature, this->rndGenerator);

            if ((score_ == 0 && oldScore == 0) || !metropolisAccept) {
                if (bestScore == 0 || allowedMetropolisRestartCount <= 0)
                    break;
                currentState = bestState;
                allowedMetropolisRestartCount--;
                metropolis_count = 0;
                temperature = best_temperature;
                continue; // restart from good result
            }

        }

        this->result = bestState;

        BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: Best score = " << bestScore;
        BOOST_LOG_TRIVIAL(debug) << "IteratedLocalSearch: " << restart_count << " restarts";
        BOOST_LOG_TRIVIAL(debug) << "======================= END ILS ================================";

        return true;
    }

    arma::mat IteratedLocalSearch::getResultMatrix() {
        return this->result;
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 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
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SCALAR_HMSB_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SCALAR_HMSB_HPP_INCLUDED

#include <boost/simd/reduction/functions/hmsb.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/dispatch/attributes.hpp>
#include <cstddef>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( hmsb_, tag::cpu_
                                    , (A0)
                                    , (scalar_< fundamental_<A0> >)
                                    )
  {
    typedef std::size_t result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return b_and(a0, boost::simd::Signmask<A0>()) != 0;
    }
  };

} } }

#endif


#pragma once
#include <limits>
#include <memory>
#include <vector>
#include <Eigen/Dense>
#include "Ray.hpp"
#include "object/Object.hpp"
#include "accelerate/Accelerate.hpp"
#include "accelerate/BVH.hpp"

class Scene
{
public:
    Scene() : objects(), accelerate(nullptr){};

    Eigen::Vector3f cast_ray(const Ray &ray) const
    {
        auto intersect = intersect_ray(ray);
        if (!intersect.happend)
            return {0.0f, 0.0f, 0.0f};

        return intersect.color;
    };

    void add(std::unique_ptr<Object> obj) {
        if (accelerate)
        {
            objects = accelerate->dispose();
            accelerate = nullptr;
        }

        objects.emplace_back(std::move(obj));
    };

    void build_bvh() { accelerate = BVH::build(std::move(objects)); };

private:
    std::vector<std::unique_ptr<Object>> objects;

    std::unique_ptr<Accelerate> accelerate;

    Intersect intersect_ray(const Ray &ray) const
    {
        if (accelerate)
            return accelerate->intersect_ray(ray);

        Intersect intersect{};
        for (const auto &obj : objects)
            if (auto inter = obj->intersect_ray(ray); inter.happend && inter.t < intersect.t)
                intersect = inter;

        return intersect;
    };
};


/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <x/vision/tracker.h>
#include <x/vision/timing.h>
#include <x/vision/utils.h>

#include <math.h>
#include <iostream>
#include <boost/bind.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace x;

Tracker::Tracker()
{}

Tracker::Tracker(const Camera& cam,
                 const unsigned int fast_detection_delta,
                 const bool non_max_supp,
                 const unsigned int block_half_length,
                 unsigned int margin,
                 unsigned int n_feat_min,
                 int outlier_method,
                 double outlier_param1,
                 double outlier_param2)
: camera_(cam)
, fast_detection_delta_(fast_detection_delta)
, non_max_supp_(non_max_supp)
, block_half_length_(block_half_length)
, margin_(margin)
, n_feat_min_(n_feat_min)
, outlier_method_(outlier_method)
, outlier_param1_(outlier_param1)
, outlier_param2_(outlier_param2)
{}

void Tracker::setParams(const Camera& cam,
                        const unsigned int fast_detection_delta,
                        const bool non_max_supp,
                        const unsigned int block_half_length,
                        unsigned int margin,
                        unsigned int n_feat_min,
                        int outlier_method,
                        double outlier_param1,
                        double outlier_param2)
{
  camera_ = cam;
  fast_detection_delta_ = fast_detection_delta;
  non_max_supp_ = non_max_supp;
  block_half_length_ = block_half_length;
  margin_ = margin;
  n_feat_min_ = n_feat_min;
  outlier_method_ = outlier_method;
  outlier_param1_ = outlier_param1;
  outlier_param2_ = outlier_param2;
}

MatchList Tracker::getMatches() const
{
  return matches_;
}

int Tracker::getImgId() const
{
  return img_id_;
}

void Tracker::track(TiledImage& current_img,
                    double timestamp,
                    unsigned int frame_number)
{
  // Increment the current image number
  img_id_++;

  #ifdef TIMING
  // Set up and start internal timing
  clock_t clock1, clock2, clock3, clock4, clock5, clock6, clock7;
  clock1 = clock();
  #endif

  // Build image pyramid
  getImagePyramid(current_img, img_pyramid_);

  #ifdef TIMING
  clock2 = clock();
  #endif

  //============================================================================
  // Feature detection and tracking
  //============================================================================

  FeatureList current_features;
  MatchList matches;

  // If first image
  if(img_id_ == 1)
  {
    // Detect features in first image
    FeatureList old_features;
    featureDetection(current_img, current_features, timestamp, frame_number, old_features);

    // Undistortion of current features
    camera_.undistortFeatures(current_features);

    #ifdef TIMING
    clock3 = clock();

    // Deal with unused timers
    clock4 = clock3;
    clock5 = clock4;
    clock6 = clock5;
    #endif
  }
  else // not first image
  {
    #ifdef TIMING
    // End detection timer even if it did not occur
    clock3 = clock();
    #endif

    // Track features
    featureTracking(previous_img_, current_img,
                    previous_features_, current_features,
                    timestamp, frame_number);

    #ifdef TIMING
    clock4 = clock();
    #endif

    removeOverflowFeatures(previous_img_, current_img, previous_features_, current_features);

    // Refresh features if needed
    if(current_features.size() < n_feat_min_)
    {
      #ifdef DEBUG
      std::cout << "Number of tracked features reduced to "
                << current_features.size()
                << std::endl;
      std::cout << "Triggering re-detection"
                << std::endl;
      #endif

      // Detect and track new features outside of the neighborhood of
      // previously-tracked features
      FeatureList previous_features_new, current_features_new;
      featureDetection(previous_img_,
                       previous_features_new,
                       previous_img_.getTimestamp(),
                       previous_img_.getFrameNumber(),
                       previous_features_);
      featureTracking(previous_img_,
                      current_img,
                      previous_features_new,
                      current_features_new,
                      timestamp,
                      frame_number);

      // Concatenate previously-tracked and newly-tracked features
      previous_features_.insert(previous_features_.end(),
                                          previous_features_new.begin(),
                                          previous_features_new.end());

      current_features.insert(current_features.end(),
                              current_features_new.begin(),
                              current_features_new.end());
    }

    #ifdef TIMING
    clock5 = clock();
    #endif

    // Undistortion of current features
    camera_.undistortFeatures(current_features);

    // Undistort features in the previous image
    // TODO(jeff) only do it for new features newly detected
    camera_.undistortFeatures(previous_features_);

    //==========================================================================
    // Outlier removal
    //==========================================================================

    unsigned int n_matches = previous_features_.size();
    if (n_matches)
    {
      //TODO(jeff) Store coordinates as cv::Point2f in Feature class to avoid
      // theses conversions
      // Convert features to OpenCV keypoints
      std::vector<cv::Point2f> pts1, pts2;
      pts1.resize(n_matches);
      pts2.resize(n_matches);
      for(unsigned int i = 0; i < pts1.size(); i++)
      {
        pts1[i].x = previous_features_[i].getX();
        pts1[i].y = previous_features_[i].getY();
        pts2[i].x = current_features[i].getX();
        pts2[i].y = current_features[i].getY();
      }

      std::vector<uchar> mask = detectOutliers(pts1, pts2, outlier_method_, outlier_param1_, outlier_param2_);
      /*cv::findHomography(pts1,
                           current_features,
                           outlier_method_,
                           outlier_param1_,
                           mask,
                           2000,
                           0.995);*/

      FeatureList current_features_refined, previous_features_refined;
      for(size_t i = 0; i < mask.size(); ++i)
      {
        if(mask[i] != 0)
        {
          previous_features_refined.push_back(previous_features_[i]);
          current_features_refined.push_back(current_features[i]);
        }
      }

      std::swap(previous_features_refined, previous_features_);
      std::swap(current_features_refined, current_features);
    }
    #ifdef TIMING
    clock6 = clock();
    #endif

    //==========================================================================
    // Match construction
    //==========================================================================

    // Reset match list
    n_matches = previous_features_.size();
    matches.resize(n_matches);

    // Store the message array into a match list
    for(unsigned int i = 0; i < n_matches; ++i)
    {
      Match current_match;
      current_match.previous = previous_features_[i];
      current_match.current  = current_features[i];

      // Add match to list
      matches[i] = current_match;
    }
  }

  // Store results so it can queried outside the class
  matches_ = matches;

  // Store info to match at next image
  previous_features_           = current_features;
  previous_timestamp_          = timestamp;
  previous_img_                = current_img;

  // Exit the function if this is the first image
  if(img_id_ < 2)
    return;

  #ifdef TIMING
  clock7 = clock();
  #endif

  // Print TIMING info
#ifdef TIMING
  std::cout << "Tracking Info ====================================" << std::endl;
  std::cout << "Number of matches:                   " << matches.size() << std::endl;
  std::cout << "==================================================" << std::endl;
  std::cout <<
      "Times ====================================" << std::endl <<
      "Build image pyramid:               " << (double) (clock2  - clock1)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "Feature detection:                 " << (double) (clock3  - clock2)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "Feature tracking:                  " << (double) (clock4  - clock3)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "Feature re-detection and tracking: " << (double) (clock5  - clock4)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "Outlier check:                     " << (double) (clock6  - clock5)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "Feature conversion:                " << (double) (clock7  - clock6)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "------------------------------------------" << std::endl <<
      "TOTAL:                             " << (double) (clock7 - clock1)/CLOCKS_PER_SEC*1000 << " ms" << std::endl <<
      "==========================================" << std::endl;
#endif

  // Plot matches in the image
  plotMatches(matches_, current_img);
}

/** \brief True if matches are chronological order
 *  @todo Make Match struct a class with this function in it.
 */
bool Tracker::checkMatches()
{
  if (!matches_.empty())
  {
    Match match(matches_[0]);
    if (match.previous.getTimestamp() < match.current.getTimestamp())
    {
      return true;
    }
  }
  return false;
}

void Tracker::plotMatches(MatchList matches, TiledImage& img)
{
  // Convert grayscale image to color image
#if CV_MAJOR_VERSION == 4
    cv::cvtColor(img, img, cv::COLOR_GRAY2BGR);
#else
    cv::cvtColor(img, img, CV_GRAY2BGR);
#endif

  // Plot tiles
  img.plotTiles();

  // Plot matches
  const cv::Scalar green(100,255,100);
  const unsigned int n = matches.size();
  for(unsigned int ii = 0; ii < n; ii++)
    img.plotFeature(matches[ii].current, green);

  std::string str = std::string("Matches: ") + std::to_string(n);

  cv::putText(img, str, cv::Point((int) 10, (int) img.rows-10), cv::FONT_HERSHEY_PLAIN,
                1.0, green, 1.5, 8, false);
}

void Tracker::featureDetection(const TiledImage& img,
                               FeatureList& new_feature_list,
                               double timestamp,
                               unsigned int frame_number,
                               FeatureList& old_feature_list)
{
  EASY_BLOCK("XVIO feature detection", profiler::colors::Green);
  // Pyramidal OpenCV FAST
  ImagePyramid pyr;
  getImagePyramid(img, pyr);

  // Extract new features
  FeatureList feature_list;
  getFASTFeaturesPyramid(pyr, timestamp, frame_number, new_feature_list, old_feature_list);
}

void Tracker::getImagePyramid (const TiledImage& img,
                               ImagePyramid& pyramid)
{
  pyramid.clear();                                  // clear the vector of images
  pyramid.push_back(img);                           // the first pyramid level is the highest resolution image
  for(unsigned int i = 1; i < pyramid_depth_; i++)  // for all pyramid depths
  {
    TiledImage downsampledImage;                       // create a down-sampled image
    downsampledImage.setTileSize(img.getTileHeight(), img.getTileWidth());
    cv::pyrDown(pyramid[i-1],downsampledImage);     // reduce the image size by a factor of 2
    pyramid.push_back(downsampledImage);            // add the down-sampled image to the pyramid
  }
}

void Tracker::getFASTFeaturesPyramid(ImagePyramid& pyramid,
                                     double timestamp,
                                     unsigned int frame_number,
                                     FeatureList& features,
                                     FeatureList& old_features)
{
  // Loop through the image pyramid levels and get the fast features
  for(unsigned int l = 0; l < pyramid.size(); l++)
    getFASTFeaturesImage(pyramid[l], timestamp, frame_number, l, features, old_features);
}

void Tracker::getFASTFeaturesImage(TiledImage& img,
                                   double timestamp,
                                   unsigned int frame_number,
                                   unsigned int pyramid_level,
                                   FeatureList& features,
                                   FeatureList& old_features)
{
  Keypoints keypoints; // vector of cv::Keypoint
  // scaling for points dependent on pyramid level to get their position
  // (x, y) = (x*2^pyramid_level,y*2^pyramid_level)
  double scale_factor = pow(2,(double) pyramid_level);

  // Get FAST features
#define FEATURES_USE_FAST
#ifdef FEATURES_USE_FAST
  cv::FAST(img, keypoints, fast_detection_delta_, non_max_supp_);
#endif
#ifdef FEATURES_USE_HARRIS
  //TODO(frockenb): Finish implementation and clean up!
  //This implementation is not finisched and should be pulled threw the whole feature chain.
  std::vector<cv::Point2d> points;
  cv::Mat mask = cv::Mat::ones(img.rows, img.cols, CV_8UC1);
  cv::goodFeaturesToTrack(img, points, 100,
                          0.2,
                          margin_, mask,
                          3,
                          true,
                          0.04);
  for (const auto & point:points) {
    cv::KeyPoint kp(point, 1);
    keypoints.push_back(kp);
  }
#endif

  // Create a blocked mask which is 1 in the neighborhood of old features, and 0
  // elsewhere.
  cv::Mat blocked_img = cv::Mat::zeros(img.rows, img.cols, CV_8U);
  computeNeighborhoodMask(old_features, img, blocked_img);

  // For all keypoints, extract the relevant information and put it into the
  // vector of Feature structs
  FeatureList candidate_features;
  for(unsigned int i = 0; i < keypoints.size(); i++)
  {
    Feature feature(timestamp,
                    frame_number,
                    ((double) keypoints[i].pt.x) * scale_factor,
                    ((double) keypoints[i].pt.y) * scale_factor,
                    pyramid_level,
                    keypoints[i].response);

    // Check whether features fall in the bounds of allowed features; if yes,
    // add it to candidate features and store its designated bucket information
    if(isFeatureInsideBorder(feature,
                             img,
                             margin_,
                             feature.getPyramidLevel()))
    {
      candidate_features.push_back(feature);
    }
  }

  // Sort the candidate features by FAST score (highest first)
  std::sort(candidate_features.begin(), candidate_features.end(),
            boost::bind(&Tracker::compareFASTFeaturesScore, this, _1, _2));

  // Append candidate features which are not neighbors of tracked features.
  // Note: old features are not supposed to be in here, see. updateTrailLists()
  appendNonNeighborFeatures(img, features, candidate_features, blocked_img);
}

void Tracker::computeNeighborhoodMask(const FeatureList& features,
                                      const cv::Mat& img,
                                      cv::Mat& mask)
{
  // Loop through candidate features.
  // Create submask for each feature
  for(int i = 0; i < (int) features.size(); i++)
  {
    // Rounded feature coordinates
    double x = std::round(features[i].getXDist());
    double y = std::round(features[i].getYDist());

    // Submask top-left corner and width/height determination.
    int x_tl, y_tl, w, h;
    // Width
    if(x - block_half_length_ < 0)
    {
      x_tl = 0;
      w    = block_half_length_ + 1 + x;
    }
    else if(x + block_half_length_ > img.cols - 1)
    {
      x_tl = x - block_half_length_;
      w    = block_half_length_ + img.cols - x;
    }
    else
    {
      x_tl = x - block_half_length_;
      w    = 2*block_half_length_ + 1;
    }
    // Height
    if(y - block_half_length_ < 0)
    {
      y_tl = 0;
      h    = block_half_length_ + 1 + y;
    }
    else if(y + block_half_length_ > img.rows - 1)
    {
      y_tl = y - block_half_length_;
      h    = block_half_length_ + img.rows - y;
    }
    else
    {
      y_tl = y - block_half_length_;
      h    = 2*block_half_length_ + 1;
    }

    // Submask application
    cv::Mat blocked_box = mask(cv::Rect(x_tl, y_tl, w, h)); // box to block off in the image
    blocked_box.setTo(cv::Scalar(1)); // block out the surrounding area by setting the mask to 1
  }
}

bool Tracker::isFeatureInsideBorder(Feature& feature,
                                    const TiledImage& img,
                                    unsigned int margin,
                                    unsigned int pyramid_level) const
{
  // Return true if the feature falls within the border at the specified pyramid level
  int xMin = (int) margin;
  int xMax = (int) floor(img.cols/pow(2,(double) pyramid_level) - margin) - 1;
  int yMin = (int) margin;
  int yMax = (int) floor(img.rows/pow(2,(double) pyramid_level) - margin) - 1;
  PixelCoor feature_coord = {0, 0};
  getScaledPixelCoordinate(feature, feature_coord);
  return ( (feature_coord.x >= xMin) && (feature_coord.x <= xMax) &&
           (feature_coord.y >= yMin) && (feature_coord.y <= yMax));
}

void Tracker::getScaledPixelCoordinate(const Feature& feature,
                                       PixelCoor& scaled_coord) const
{
  // Transform the feature into scaled coordinates
  scaled_coord.x = (int) std::round(feature.getXDist() / pow(2, (double) feature.getPyramidLevel()));
  scaled_coord.y = (int) std::round(feature.getYDist() / pow(2, (double) feature.getPyramidLevel()));
}

bool Tracker::compareFASTFeaturesScore(const Feature& feature1,
                                       const Feature& feature2) const
{
  return feature1.getFastScore() > feature2.getFastScore();
}

void Tracker::appendNonNeighborFeatures(TiledImage& img,
                                        FeatureList& features,
                                        FeatureList& candidate_features,
                                        cv::Mat& mask)
{
  // Loop through candidate features.
  // If the patch is not blocked and there is room in the bucket, add it to features
  for(unsigned int i = 0; i < candidate_features.size(); i++)
  {
    double x = std::round(candidate_features[i].getXDist()); // approximate pixel coords (integer)
    double y = std::round(candidate_features[i].getYDist()); // approximate pixel coords (integer)

    // if the point is not blocked out
    if(mask.at<unsigned char>(y, x) == 0)
    {
      // if there is room in the bucket
      Feature& candidate_feature = candidate_features[i];
      img.setTileForFeature(candidate_feature);

	  // add it to the features and update the bucket count
      features.push_back(candidate_features[i]);

      //block out the surrounding area by setting the mask to 1
      cv::Mat blocked_box = mask(cv::Rect(x - block_half_length_, y - block_half_length_,
                                            2*block_half_length_ + 1, 2*block_half_length_ + 1));
      blocked_box.setTo(cv::Scalar(1));
    }
  }
}

void Tracker::removeOverflowFeatures(TiledImage& img1,
                                      TiledImage& img2,
                                      FeatureList& features1,
                                      FeatureList& features2) const
{
  img1.resetFeatureCounts();

  // Loop through current features to update their tile location
  for(int i = features2.size(); i >= 1; i--)
  {
    img1.setTileForFeature(features1[i-1]);
    img2.setTileForFeature(features2[i-1]);

    img1.incrementFeatureCountAtTile(features1[i-1].getTileRow(), features1[i-1].getTileCol());
    img2.incrementFeatureCountAtTile(features2[i-1].getTileRow(), features2[i-1].getTileCol());
  }
  // Loop through current features to check if there are more than the max
  // allowed per tile and delete all features from this tile if so.
  // They are sorted by score so start at the bottom.
  for(int i = features2.size()-1; i >= 1; i--)
  {
    const unsigned int feat2_row = features2[i-1].getTileRow();
    const unsigned int feat2_col = features2[i-1].getTileCol();
    const unsigned int count = img2.getFeatureCountAtTile(feat2_row, feat2_col);
    if(count > img1.getMaxFeatPerTile())
    {
      //if the bucket is overflowing, remove the feature.
      features1.erase(features1.begin() + i-1);
      features2.erase(features2.begin() + i-1);
    }
  }
}

//this function automatically gets rid of points for which tracking fails
void Tracker::featureTracking(const cv::Mat& img1,
                              const cv::Mat& img2,
                              FeatureList& features1,
                              FeatureList& features2,
                              const double timestamp2,
                              const unsigned int frame_number2) const
{
  EASY_BLOCK("XVIO feature tracking", profiler::colors::Green);
  // Convert features to OpenCV keypoints
  //TODO(jeff) Store coordinates as cv::Point2f in Feature class to avoid
  // theses conversions
  std::vector<cv::Point2f> pts1;
  pts1.resize(features1.size());
  for(unsigned int i = 0; i < pts1.size(); i++)
  {
    pts1[i].x = features1[i].getXDist();
    pts1[i].y = features1[i].getYDist();
  }

  std::vector<uchar> status;
  std::vector<float> err;
  cv::Size win_size = cv::Size(31,31);
  cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01);

  // Prevents calcOpticalFlowPyrLK crash
  std::vector<cv::Point2f> pts2;
  if(pts1.size() > 0)
  {
    cv::calcOpticalFlowPyrLK(img1, img2,
                             pts1, pts2,
                             status, err, win_size, 2, term_crit,
                             cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 0.0); //0.001
  }

  // Output features which KLT tracked and stayed inside the frame
  int index_correction = 0;
  for(unsigned int i = 0; i < status.size(); i++)
  {
    const cv::Point2f pt = pts2.at(i);
    if(status.at(i) != 0
       && pt.x >= -0.5 && pt.y >= -0.5
       && pt.x <= img2.cols-0.5 && pt.y <= img2.rows-0.5)
    {
      Feature new_feature(timestamp2,
                          frame_number2,
                          pt.x,
                          pt.y,
                          features1[i].getPyramidLevel(),
                          features1[i].getFastScore());

      features2.push_back(new_feature);
    }
    else
    {
      features1.erase(features1.begin() + (i - index_correction));
      index_correction++;
    }
  }

  // Sanity check
  assert(features1.size() == features2.size());
}


/*
 * This file is subject to the terms and conditions defined in
 * file 'LICENSE', which is part of this source code package.
 *
 * Partialy financiated as part of the protocol between UTAustin I Portugal - UTA-P.
 * [2017] - [2018] University of Minho, Filipe Costa Oliveira
 * All Rights Reserved.
 */

#ifndef SRC_INTENSITY_COLUMNS_H__
#define SRC_INTENSITY_COLUMNS_H__

/* BEGIN BOOST */
#include <boost/iostreams/device/mapped_file.hpp> // for mmap
#include <boost/iostreams/stream.hpp>             // for stream
#include <boost/filesystem.hpp>                      // for path, operator==, oper...
#include <boost/filesystem/path.hpp>                      // for path, operator==, oper...
/* END BOOST */

#include <string>                        // for allocator, char_traits, to_s...
#include <vector>                        // for vector, vector<>::iterator
#include <algorithm>                     // for max_element
#include <cassert>                       // for assert
#include <cmath>                         // for round
#include <cstdio>                        // for perror, NULL, SEEK_END
#include <iomanip>                       // for operator<<, setw, setfill
#include <iostream>                      // for operator<<, basic_ostream
#include <iterator>                      // for distance
#include <limits>                        // for numeric_limits
#include <fstream>                    // for ofstream
#include <utility>                     // for pair
#include <math.h>       /* nearbyint */

#include <opencv2/opencv.hpp>           //
#include <opencv2/core/hal/interface.h>  // for CV_32FC1, CV_8UC1, CV_32F
#include <opencv2/core/types_c.h>        // for CvScalar, cvScalar, CvPoint
#include <opencv2/imgproc/imgproc_c.h>   // for CV_AA
#include <opencv2/imgproc/types_c.h>     // for ::CV_TM_CCOEFF_NORMED
#include <opencv2/core.hpp>              // for minMaxLoc, Exception, Hershe...
#include <opencv2/core/base.hpp>         // for Code::StsNoConv, NormTypes::...
#include <opencv2/core/cvstd.inl.hpp>    // for operator<<, String::String
#include <opencv2/highgui.hpp>           // for imshow, namedWindow, waitKey
#include <opencv2/imgcodecs.hpp>         // for imwrite
#include <opencv2/imgproc.hpp>           // for putText, resize, Interpolati...
#include <opencv2/core/mat.hpp>        // for Mat
#include <opencv2/core/mat.inl.hpp>    // for Mat::~Mat
#include <opencv2/core/types.hpp>      // for Point, Point3d, Point2f, Rect
#include <opencv2/video/tracking.hpp>  // for ::MOTION_EUCLIDEAN

#include "wavimg_prm.hpp"              // for WAVIMG_prm
#include "base_crystal.hpp"
#include "base_image.hpp"
#include "application_log.hpp"
#include "opencv2/core.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"

Q_DECLARE_METATYPE(cv::Point2i)

class IntensityColumns : public QObject {
  Q_OBJECT
private:

    // // // // //
    // simulation parameters
    // // // // //

    BaseCrystal* sim_crystal_properties = nullptr;
    bool _flag_sim_crystal_properties = false;

    BaseImage* exp_image_properties = nullptr;
    bool _flag_exp_image_properties = false;
    cv::Mat exp_image_dist_transform;

    BaseImage* sim_image_properties = nullptr;
    bool _flag_sim_image_properties = false;
    cv::Mat sim_image_dist_transform;

    double  stddev_threshold_factor = 1.0f;
    bool _flag_stddev_threshold_factor = true;

    int threshold_value = 0;
    bool _flag_threshold_value = false;

    WAVIMG_prm* wavimg_parameters = nullptr;

    /* Loggers */
    ApplicationLog::ApplicationLog* logger = nullptr;
    bool _flag_logger = false;

    float intensity_columns_keypoint_diameter = 16.0f;
    cv::Mat imgResult;

    cv::Ptr<Feature2D> f2d;

    cv::Mat sim_image_descriptor;
    std::vector<cv::KeyPoint> sim_image_keypoints;
    bool _flag_sim_image_keypoints = false;

    std::vector<cv::Point2i> sim_image_intensity_columns_center;
    bool _flag_sim_image_intensity_columns_center = false;

    cv::Mat exp_image_descriptor;
    std::vector<cv::KeyPoint> exp_image_keypoints;
    bool _flag_exp_image_keypoints = false;

    std::vector< std::vector<cv::Point> > sim_image_intensity_columns;
    bool _flag_sim_image_intensity_columns = false;

    std::vector<cv::Point2i> sim_image_intensity_columns_projective_2D_coordinate;
    bool _flag_sim_image_intensity_columns_projective_2D_coordinate = false;

    std::vector<int> sim_image_intensity_columns_mean_statistical;
    std::vector<int> sim_image_intensity_columns_trans_mean_statistical;
    std::vector<int> sim_image_intensity_columns_roi_mean_statistical;

    bool _flag_sim_image_intensity_columns_mean_statistical = false;

    std::vector<int> exp_image_intensity_columns_mean_statistical;
    bool _flag_exp_image_intensity_columns_mean_statistical = false;

    std::vector<int> sim_image_intensity_columns_stddev_statistical;
        std::vector<int> sim_image_intensity_columns_trans_stddev_statistical;
    std::vector<int> sim_image_intensity_columns_roi_stddev_statistical;

    bool _flag_sim_image_intensity_columns_stddev_statistical = false;

    std::vector<int> exp_image_intensity_columns_stddev_statistical;
    bool _flag_exp_image_intensity_columns_stddev_statistical = false;

    std::vector<int> sim_image_intensity_columns_threshold_value;
    bool _flag_sim_image_intensity_columns_threshold_value = false;

    std::vector<double> sim_image_intensity_columns_trans_integrate_intensity;
    std::vector<double> sim_image_intensity_columns_integrate_intensity;
    std::vector<double> sim_image_intensity_columns_roi_integrate_intensity;

    bool _flag_sim_image_intensity_columns_integrate_intensity = false;

    std::vector<double> exp_image_intensity_columns_integrate_intensity;
    bool _flag_exp_image_intensity_columns_integrate_intensity = false;

    std::vector<bool> sim_image_intensity_columns_marked_delete;
    bool _flag_sim_image_intensity_columns_marked_delete = false;

    std::vector< cv::Mat1b > sim_image_intensity_columns_masks;
    std::vector< cv::Mat1b > sim_image_intensity_columns_roi_masks;

    std::vector< std::vector<cv::Point> > exp_image_intensity_columns;
    cv::Point2i exp_image_delta_factor_constant;
    bool _flag_exp_image_delta_factor_constant = false;

    bool auto_calculate_threshold_value();

public:

    IntensityColumns();
    bool read_simulated_image_from_dat_file();
    bool segmentate_sim_image();
    bool segmentate_exp_image();
    bool feature_match();
    bool map_sim_intensity_cols_to_exp_image();

    /** getters **/

    // flag getters

    bool get_flag_logger(){ return _flag_logger; }
    bool get_flag_sim_crystal_properties(){ return _flag_sim_crystal_properties; }
    bool get_flag_exp_image_properties(){ return _flag_exp_image_properties; }
    bool get_flag_sim_image_properties(){ return _flag_sim_image_properties; }
    std::vector<cv::KeyPoint> get_sim_image_keypoints(){ return sim_image_keypoints; }
    std::vector<cv::Point2i> get_sim_image_intensity_columns_center(){ return sim_image_intensity_columns_center; }
    std::vector<cv::KeyPoint> get_exp_image_keypoints(){ return exp_image_keypoints; }
    std::vector<cv::Point2i> get_sim_image_intensity_columns_projective_2D_coordinate(){ return sim_image_intensity_columns_projective_2D_coordinate; }
    std::vector<bool> get_sim_image_intensity_columns_marked_delete(){ return sim_image_intensity_columns_marked_delete; }

    std::vector<int> get_sim_image_intensity_columns_mean_statistical(){ return sim_image_intensity_columns_mean_statistical; }
    std::vector<int> get_exp_image_intensity_columns_mean_statistical(){ return exp_image_intensity_columns_mean_statistical; }
    std::vector<int> get_sim_image_intensity_columns_stddev_statistical(){ return sim_image_intensity_columns_stddev_statistical; }
    std::vector<int> get_exp_image_intensity_columns_stddev_statistical(){ return exp_image_intensity_columns_stddev_statistical; }
    std::vector<int> get_sim_image_intensity_columns_threshold_value(){ return sim_image_intensity_columns_threshold_value; }
    std::vector<double> get_sim_image_intensity_columns_integrate_intensity(){ return sim_image_intensity_columns_integrate_intensity; }
    std::vector<double> get_exp_image_intensity_columns_integrate_intensity(){ return exp_image_intensity_columns_integrate_intensity; }

    bool get_flag_sim_image_intensity_columns_center(){ return _flag_sim_image_intensity_columns_center; }
    bool get_flag_sim_image_intensity_columns_projective_2D_coordinate(){ return _flag_sim_image_intensity_columns_projective_2D_coordinate; }
    bool get_flag_sim_image_intensity_columns_marked_delete(){ return _flag_sim_image_intensity_columns_marked_delete; }
    bool get_flag_sim_image_intensity_columns_mean_statistical(){ return _flag_sim_image_intensity_columns_mean_statistical; }
    bool get_flag_exp_image_intensity_columns_mean_statistical(){ return _flag_exp_image_intensity_columns_mean_statistical; }
    bool get_flag_sim_image_intensity_columns_stddev_statistical(){ return _flag_sim_image_intensity_columns_stddev_statistical; }
    bool get_flag_exp_image_intensity_columns_stddev_statistical(){ return _flag_exp_image_intensity_columns_stddev_statistical; }
    bool get_flag_sim_image_intensity_columns_threshold_value(){ return _flag_sim_image_intensity_columns_threshold_value; }
    bool get_flag_sim_image_intensity_columns_integrate_intensity(){ return _flag_sim_image_intensity_columns_integrate_intensity; }
    bool get_flag_exp_image_intensity_columns_integrate_intensity(){ return _flag_exp_image_intensity_columns_integrate_intensity; }

    std::string get_export_integrated_intensities_filename_hint(){ return "integrated_intensities.csv"; }
    bool export_sim_image_intensity_columns_integrated_intensities_to_csv( std::string sim_grid_file_name_image , bool onlymapped = true );

    /* Loggers */
    ApplicationLog::ApplicationLog* get_logger(){ return logger; }

    bool produce_png_from_dat_file();

    bool set_sim_crystal_properties ( BaseCrystal* crystal_prop );
    bool set_exp_image_properties ( BaseImage* exp_image_properties );
    bool set_sim_image_properties ( BaseImage* sim_image_properties , bool auto_segmentate = false );
    bool set_wavimg_var( WAVIMG_prm *wavimg_var );
    bool set_stddev_threshold_factor( double factor );

    void print_var_state();

    /* Loggers */
    bool set_application_logger( ApplicationLog::ApplicationLog* logger );
    virtual std::ostream& output(std::ostream& stream) const;

    /* extra */
    cv::Point op_Point2i_padding (cv::Point point, const cv::Point padd );
    cv::KeyPoint op_KeyPoint_padding ( cv::KeyPoint point, const cv::Point2f padd );
    std::vector<cv::Point> op_contour_padding ( std::vector<cv::Point> vec, const cv::Point padd );

    public slots:
    void apply_exp_image_delta_factor();

    signals:
    void sim_image_intensity_columns_changed();
    void sim_image_intensity_keypoints_changed();
    void exp_image_intensity_columns_changed();
    void exp_image_intensity_keypoints_changed();
    void exp_image_delta_factor_constant_changed();

};

#endif


#include "ntl/scheduler.hpp"
#include "ntl/link.hpp"

#include <boost/preprocessor/iteration/local.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>

typedef ntl::gateway_registers<int> gateway_registers;

struct scheduler_cmd {
    int port;
    uint32_t quantum;
};

template <unsigned log_size>
class sched_wrapper
{
public:
    typedef ntl::scheduler<log_size> scheduler_t;
    typedef typename scheduler_t::index_t index_t;

    static void decode_gateway_address(int address, int& flow_id, int& cmd,
		    bool read)
    {
        flow_id = address >> 2;
        cmd = address & 1;
	read = address & 2;
    }

    void step(gateway_registers& g)
    {
#pragma HLS pipeline II=3
        /* Inline the gateway here */
#pragma HLS inline region
        gateway.gateway(g, [&](int addr, int& data) -> bool {
#pragma HLS inline
            int flow_id, cmd;
	    bool read;
            decode_gateway_address(addr, flow_id, cmd, read);
            return sched.rpc(cmd, &data, flow_id, read);
        });

        if (sched.update())
            return;

        index_t req;
        if (!tx_requests.empty()) {
            req = tx_requests.read();
            sched.schedule(req);
        }

        if (scheduler_decision.full())
            return;

        index_t selected_stream;
        uint32_t quota;
        if (!sched.next_flow(&selected_stream, &quota))
            return;

        scheduler_decision.write(scheduler_cmd{selected_stream, quota});
    }
    ntl::stream<index_t> tx_requests;
    ntl::stream<scheduler_cmd> scheduler_decision;
private:
    scheduler_t sched;
    ntl::gateway_impl<int> gateway;
};

#define NUM_SCHEDULERS 8
void sched_top(BOOST_PP_ENUM_PARAMS(NUM_SCHEDULERS, gateway_registers& g),
        BOOST_PP_ENUM_PARAMS(NUM_SCHEDULERS, ntl::stream<int>& tx_requests),
        BOOST_PP_ENUM_PARAMS(NUM_SCHEDULERS, ntl::stream<scheduler_cmd>& scheduler_decision))
{
#pragma HLS dataflow
#pragma HLS interface ap_ctrl_none port=return

#define BOOST_PP_LOCAL_MACRO(n) \
    GATEWAY_OFFSET(g ## n, 0x100, 0x118, 0xfc) \
    static sched_wrapper<BOOST_PP_INC(n)> sched ## n; \
    sched ## n.step(g ## n); \
    ntl::link(tx_requests ## n, sched ## n.tx_requests); \
    ntl::link(sched ## n.scheduler_decision, scheduler_decision ## n);
#define BOOST_PP_LOCAL_LIMITS (0, NUM_SCHEDULERS - 1)
%:include BOOST_PP_LOCAL_ITERATE()
}

int main()
{
    return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  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_SIMD_FUNCTION_IS_EQZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_EQZ_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/logical_not.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(is_eqz_
                          , (typename A0,typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>,X>
                          )

  {
    BOOST_FORCEINLINE bs::as_logical_t<A0> operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return is_equal(a0, Zero<A0>());
    }
  };

   BOOST_DISPATCH_OVERLOAD(is_eqz_
                          , (typename A0,typename X)
                          , bd::cpu_
                          , bs::pack_<bs::logical_<A0>,X>
                          )

  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return logical_not(a0);
    }
  };

} } }

#endif


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "include/AdjacentMatrix.hh"
#include "include/HyperVertex.hh"
#include "include/HyperEdge.hh"

#include <boost/foreach.hpp>

AdjacentMatrix::AdjacentMatrix() : AdjacentMatrix(0, 0) {

}

AdjacentMatrix::AdjacentMatrix(const unsigned int& m, const unsigned int& n) : _m( m ), _n( n ) {
	_adjacentMatrixBool.resize(boost::extents[m][n]);
}

void
AdjacentMatrix::resize(const unsigned int& m, const unsigned int& n) {
	_m = m;
	_n = n ;
	_adjacentMatrixBool.resize(boost::extents[m][n]);
}

void
AdjacentMatrix::addHyperVertex(const boost::shared_ptr<HyperVertex>& hyperVertex) {

	const unsigned int id( hyperVertex->getIdentifier() );
	LibType::ListHyperEdge heList( hyperVertex->getHyperEdgeList() );

	for(unsigned int u=0; u < heList.size(); u++)_adjacentMatrixBool[id][heList[u]->getIdentifier()] = true;

}

void
AdjacentMatrix::addHyperEdge(const boost::shared_ptr<HyperEdge>& hyperEdge) {

	const unsigned int id( hyperEdge->getIdentifier() );
	LibType::ListHyperVertex hvList( hyperEdge->getHyperVertexList() );

	for(unsigned int u=0; u < hvList.size(); u++)_adjacentMatrixBool[hvList[u]->getIdentifier()][id] = true;

}

unsigned int AdjacentMatrix::getVertexDegree(const boost::shared_ptr<HyperVertex>& hyperVertex) const {

	unsigned int sum( 0 );

#pragma omp for schedule( dynamic )
	for(unsigned int i=0; i < _n; i++) {
		sum += _adjacentMatrixBool[hyperVertex->getIdentifier()][i];
	};

	return sum;
}

unsigned int
AdjacentMatrix::getEdgeSize(const boost::shared_ptr<HyperEdge>& hyperEdge) const {

	unsigned int sum( 0 );

#pragma omp for schedule( dynamic )
	for(unsigned int i=0; i < _m; i++) {
		sum += _adjacentMatrixBool[i][hyperEdge->getIdentifier()];
	};

	return sum;
}

boost::tuple<unsigned int, unsigned int>&
AdjacentMatrix::getMatrixDimension() {
	boost::tuple<unsigned int, unsigned int>
	*p = new boost::tuple<unsigned int, unsigned int>(_n, _m);
	return *p;
}

bool
AdjacentMatrix::isVertexInEdge(const int& vertexId, const int& edgeId) const {
	return _adjacentMatrixBool[vertexId][edgeId];
}

bool
AdjacentMatrix::isEdgeInVertex(const int& edgeId, const int& vertexId) const {
	return _adjacentMatrixBool[edgeId][vertexId];
}

bool
AdjacentMatrix::isEdgeInVertex(const boost::shared_ptr<HyperEdge>& hEdge, const boost::shared_ptr<HyperVertex>& hVertex) const {
	return isEdgeInVertex(hEdge->getIdentifier(), hVertex->getIdentifier());
}

bool
AdjacentMatrix::isVertexInEdge(const boost::shared_ptr<HyperVertex>& hVertex, const boost::shared_ptr<HyperEdge>& hEdge) const {
	return isVertexInEdge(hVertex->getIdentifier(), hEdge->getIdentifier());
}

unsigned int
AdjacentMatrix::getCoRank() const {

	auto edgeSize = [&](const int& edgeId) {
		unsigned int sum( 0 );
		for(unsigned int i=0; i < _m; i++) {
			sum += _adjacentMatrixBool[i][edgeId];
		}
		return sum;
	};

	unsigned int corank( -1 );
	for(unsigned int k=0; k < _n; k++) {
		corank = (edgeSize(k) < corank ? edgeSize(k) : corank);
	}

	return corank;
}

unsigned int
AdjacentMatrix::getRank() const {

	auto edgeSize = [&](const int& edgeId) {
		unsigned int sum( 0 );
		for(unsigned int i=0; i < _m; i++) {
			sum += _adjacentMatrixBool[i][edgeId];
		}
		return sum;
	};

	unsigned int rank(0);
	for(unsigned int k=0; k < _n; k++) {
		rank = (edgeSize(k) > rank ? edgeSize(k) : rank);
	}

	return rank;
}

LibType::AdjacentMatrixContainerBool&
AdjacentMatrix::getBoolAdjacentMatrix() {
	return _adjacentMatrixBool;
}


#include "Poly1d.h"

#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <sstream>

#include <Eigen/Dense>

#include <isce3/core/Poly1d.h>
#include <isce3/except/Error.h>

// alias
namespace py = pybind11;
using isce3::core::Poly1d;
using isce3::except::InvalidArgument;
using isce3::except::LengthError;

void addbinding(pybind11::class_<isce3::core::Poly1d>& pyPoly1d)
{
    pyPoly1d
            // constructors
            .def(py::init([](const std::vector<double>& coeffs,
                                  double mean = 0.0, double std = 1.0) {
                if (!(std > 0.0))
                    throw InvalidArgument(
                            ISCE_SRCINFO(), "STD must be a positive value!");
                auto pf_obj = Poly1d(coeffs.size() - 1, mean, std);
                pf_obj.coeffs = coeffs;
                return pf_obj;
            }),
                    py::arg("coeffs"), py::arg("mean") = 0.0,
                    py::arg("std") = 1.0)

            // dunder methods
            .def("__repr__",
                    [](const Poly1d& self) {
                        std::stringstream os;
                        os << "Poly1d(order= " << self.order
                           << ", mean=" << self.mean << ", std=" << self.norm
                           << ")";
                        return os.str();
                    })
            .def("__str__",
                    [](const Poly1d& self) {
                        Eigen::Map<const Eigen::ArrayXd> coeff(
                                self.coeffs.data(), self.coeffs.size());
                        std::stringstream os;
                        os << "Coeffs in ascending order -> " << coeff;
                        return os.str();
                    })
            .def("__call__", [](const Poly1d& self) { return self.coeffs; })
            .def("__getitem__",
                    [](const Poly1d& self, int pos) {
                        return self.coeffs.at(pos);
                    })
            .def("__getitem__",
                    [](const Poly1d& self, py::slice slice) {
                        size_t start, stop, step, slicelength;
                        if (!slice.compute(self.order + 1, &start, &stop, &step,
                                    &slicelength))
                            throw py::error_already_set();
                        py::list result;
                        for (size_t i = 0; i < slicelength; ++i) {
                            result.append(self.coeffs[start]);
                            start += step;
                        }
                        return result;
                    })
            .def("__len__",
                    [](const Poly1d& self) { return self.coeffs.size(); })

            // methods
            .def("eval", &Poly1d::eval, py::arg("x"))
            .def("eval",
                    [](const Poly1d& self, const std::vector<double>& x) {
                        if (x.size() < 1)
                            throw LengthError(ISCE_SRCINFO(),
                                    "The size of the vector must be non zero!");
                        std::vector<double> y;
                        y.reserve(x.size());
                        for (const auto& element : x)
                            y.push_back(self.eval(element));
                        return y;
                    },
		 py::arg("x"))
            .def("derivative",
                    [](const Poly1d& self) { return self.derivative(); })

            // properties
            .def_property_readonly("order",
                    [](const Poly1d& self) { return self.coeffs.size() - 1; })
            .def_property_readonly(
                    "mean", [](const Poly1d& self) { return self.mean; })
            .def_property_readonly(
                    "std", [](const Poly1d& self) { return self.norm; })
            .def_property_readonly(
                    "coeffs", [](const Poly1d& self) { return self.coeffs; })
            .doc() = R"(
Data structure for representing 1D polynomials in ascending order with centralization and scaling)";
}


#include <ros/ros.h>

#include <string>
#include <fstream>
#include <memory>

// ros includes
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <std_srvs/Trigger.h>
#include <sensor_msgs/Image.h>

// ros actionlib includes
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

// industrial_extrinsic_cal includes
#include <industrial_extrinsic_cal/basic_types.h>
#include <industrial_extrinsic_cal/camera_observer.hpp>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <industrial_extrinsic_cal/observation_scene.h>
#include <industrial_extrinsic_cal/observation_data_point.h>
#include <industrial_extrinsic_cal/ceres_blocks.h>
#include <industrial_extrinsic_cal/ros_camera_observer.h>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>
#include <industrial_extrinsic_cal/ceres_costs_utils.h>
#include <industrial_extrinsic_cal/circle_cost_utils.hpp>
#include <industrial_extrinsic_cal/calibration_job_definition.h>
#include <industrial_extrinsic_cal/calibrationAction.h>
#include <industrial_extrinsic_cal/calibrate.h>
#include <industrial_extrinsic_cal/covariance.h>
#include <industrial_extrinsic_cal/ros_target_display.hpp>
#include <industrial_extrinsic_cal/camera_yaml_parser.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>
#include <industrial_extrinsic_cal/ros_transform_interface.h>

//defined services
#include <industrial_extrinsic_cal/cal_srv_solve.h>
#include <industrial_extrinsic_cal/FileOp.h>

// boost includes
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <boost/shared_ptr.hpp>

#include <yaml-cpp/yaml.h>

using boost::shared_ptr;
using boost::filesystem::path;
using boost::dynamic_pointer_cast;

using ceres::Solver;
using ceres::CostFunction;

using industrial_extrinsic_cal::Camera;
using industrial_extrinsic_cal::Target;
using industrial_extrinsic_cal::CeresBlocks;
using industrial_extrinsic_cal::Roi;
using industrial_extrinsic_cal::CameraObservations;
using industrial_extrinsic_cal::Point3d;
using industrial_extrinsic_cal::Pose6d;
using industrial_extrinsic_cal::P_BLOCK;

class stereoCalServiceNode
{
public:
  stereoCalServiceNode(const  ros::NodeHandle& nh)
    :nh_(nh), P_(NULL), problem_initialized_(false), total_observations_(0), scene_(0)
  {
    std::string nn = ros::this_node::getName();
    ros::NodeHandle priv_nh("~");
    std::string target_mount_frame;
    std::string camera_mount_frame;

    // load cameras and targets
    priv_nh.getParam("yaml_file_path", yaml_file_path_);
    std::string camera_file, target_file;
    priv_nh.getParam("camera_file", camera_file);
    priv_nh.getParam("target_file", target_file);
    priv_nh.getParam("save_data", save_data_);
    priv_nh.getParam("data_directory", data_directory_);
    camera_file_ = yaml_file_path_ + camera_file ;
    target_file_ = yaml_file_path_ + target_file ;

    // provide feedback for the launch file developer
    ROS_INFO("yaml_file_path: %s", yaml_file_path_.c_str());
    ROS_INFO("camera_file: %s",  camera_file_.c_str());
    ROS_INFO("target_file: %s",  target_file_.c_str());

    if(!load_camera()){
      ROS_ERROR("can't load the camera from %s", (yaml_file_path_+camera_file_).c_str());
      exit(1);
    }
    if(!load_target()){
      ROS_ERROR("can't load the target from %s", (yaml_file_path_+target_file_).c_str());
      exit(1);
    }

    // intiialize ceres blocks
    init_blocks();

    // advertise services
    start_server_       = nh_.advertiseService( "StereoCalSrvStart", &stereoCalServiceNode::startCallBack, this);
    observation_server_ = nh_.advertiseService( "StereoCalSrvObs", &stereoCalServiceNode::observationCallBack, this);
    run_server_         = nh_.advertiseService( "StereoCalSrvRun", &stereoCalServiceNode::runCallBack, this);
    save_server_        = nh_.advertiseService( "StereoCalSrvSave", &stereoCalServiceNode::saveCallBack, this);
    load_server_        = nh_.advertiseService( "StereoCalSrvImport", &stereoCalServiceNode::loadCallBack, this);
    covariance_server_  = nh_.advertiseService( "ICalSrvCov",        &stereoCalServiceNode::covCallBack, this);

  };// end of constructor

  void init_blocks()
  {
    // adds the intial block for each camera and target
    for (int i = 0; i < (int)all_cameras_.size(); i++)
      {
	if (all_cameras_[i]->is_moving_)
	  {
	    int scene_ = 0;
	    ceres_blocks_.addMovingCamera(all_cameras_[i], scene_);
	  }
	else
	  {
	    ceres_blocks_.addStaticCamera(all_cameras_[i]);
	  }
      }

    for (int i = 0; i < (int)all_targets_.size(); i++)
      {
	if (all_targets_[i]->pub_rviz_vis_){ // use rviz visualization marker to display the target, currently must be modified circle grid
	  displayRvizTarget(all_targets_[i]);
	}
	if (all_targets_[i]->is_moving_)
	  {
	    int scene_ = 0;
	    //	    ROS_ERROR("adding moving target: %s to scene 0", all_targets_[i]->target_name_.c_str());
	    ceres_blocks_.addMovingTarget(all_targets_[i], scene_);
	  }
	else
	  {
	    //	    ROS_ERROR("adding static target");
	    ceres_blocks_.addStaticTarget(all_targets_[i]);
	  }
      }  // end for every target found

    // if loaded camera is the right side of a stereo pair, it contains a pointer to the left one.
    for (int i = 0; i < (int)all_cameras_.size(); i++)
      {
	if(all_cameras_[i]->is_right_stereo_camera_)
	  {
	    all_cameras_[i]->left_stereo_camera_ = ceres_blocks_.getCameraByName(all_cameras_[i]->left_stereo_camera_name_);
	  }
      }

    // set reference frame for all transform interfaces
    ceres_blocks_.setReferenceFrame(all_cameras_[0]->transform_interface_->getTransformFrame());
  }; // end init_blocks()

  // read the camera yaml file
  bool load_camera()
  {
    bool rtn = true;
    if (!parseCameras(camera_file_, all_cameras_))
      {
	ROS_ERROR("failed to parse cameras from %s", camera_file_.c_str());
	rtn = false;
      }
    for(int i=0;i<(int)all_cameras_.size();i++){
      all_cameras_[i]->transform_interface_->setDataDirectory(all_cameras_[i]->camera_observer_->image_directory_);
    }
    return rtn;
  };// end of load_camera()

  // read the target yaml file
  bool load_target()
  {
    bool rtn = true;

    if (!parseTargets(target_file_, all_targets_))
      {
	ROS_ERROR("failed to parse targets from %s", target_file_.c_str());
	rtn = false;
      }
    for(int i=0;i<(int)all_targets_.size();i++){
      all_targets_[i]->transform_interface_->setDataDirectory(all_cameras_[0]->camera_observer_->image_directory_);
    }

    return rtn;
  }; // end load_target()

  // callback functions
  bool startCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    if(problem_initialized_)	delete(P_);
    P_ = new ceres::Problem;
    problem_initialized_ = true;
    total_observations_  = 0;
    scene_=0;
    ceres_blocks_.clearCamerasTargets();
    init_blocks();
    res.message = std::string("Stereo calibration service started");
    res.success = true;
    return(true);
  }

  // called to collect observations for the current pose of the scene
  bool observationCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];

    if(problem_initialized_ != true ){
      std_srvs::TriggerRequest  sreq;
      std_srvs::TriggerResponse sres;
      ROS_INFO("Problem not yet initialized, calling start service");
      startCallBack(sreq, sres);
    }

    // WARNING
    // assume only one target and two cameras 1st camera is the left camera and 2nd right
    // get the intitial conditions for this scene
    all_targets_[0]->pullTransform();

    // add the target to each camera's observer
    Roi roi;
    roi.x_min = 0;
    roi.y_min = 0;

    roi.x_max = all_cameras_[0]->camera_parameters_.width;
    roi.y_max = all_cameras_[0]->camera_parameters_.height;
    all_cameras_[0]->camera_observer_->clearTargets();
    all_cameras_[0]->camera_observer_->clearObservations();
    all_cameras_[0]->camera_observer_->addTarget(all_targets_[0], roi, industrial_extrinsic_cal::cost_functions::WristStereoCal);

    roi.x_max = all_cameras_[1]->camera_parameters_.width;
    roi.y_max = all_cameras_[1]->camera_parameters_.height;
    all_cameras_[1]->camera_observer_->clearTargets();
    all_cameras_[1]->camera_observer_->clearObservations();
    all_cameras_[1]->camera_observer_->addTarget(all_targets_[0], roi, industrial_extrinsic_cal::cost_functions::WristStereoCal);

    // trigger both cameras as close to one another as possible
    all_cameras_[0]->camera_observer_->triggerCamera();
    all_cameras_[1]->camera_observer_->triggerCamera();

    // wait for both observers to get their images
    while (!all_cameras_[0]->camera_observer_->observationsDone());
    while (!all_cameras_[1]->camera_observer_->observationsDone());

    // get the observations from left and right cameras
    CameraObservations left_camera_observations;
    CameraObservations right_camera_observations;
    all_cameras_[0]->camera_observer_->getObservations(left_camera_observations);
    all_cameras_[1]->camera_observer_->getObservations(right_camera_observations);

    // only add if there are enough observations in both
    int num_observations = (int)left_camera_observations.size() + (int)right_camera_observations.size();

    if(left_camera_observations.size() == all_targets_[0]->num_points_ && right_camera_observations.size() == all_targets_[0]->num_points_){
      ROS_INFO("Found %d left camera observations and %d right camera observations", (int)left_camera_observations.size(), (int) right_camera_observations.size());

      // add the moving target to the blocks
      shared_ptr<Target> target = left_camera_observations[0].target;
      ceres_blocks_.addMovingTarget(all_targets_[0], scene_);
      P_BLOCK left_intrinsics  = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[0]->camera_name_);
      P_BLOCK right_intrinsics = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[1]->camera_name_);
      P_BLOCK right_extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(all_cameras_[1]->camera_name_);
      P_BLOCK target_pb        = ceres_blocks_.getMovingTargetPoseParameterBlock(target->target_name_,scene_);
      double lfx = left_intrinsics[0];
      double lfy = left_intrinsics[1];
      double lcx = left_intrinsics[2];
      double lcy = left_intrinsics[3];
      double rfx = right_intrinsics[0];
      double rfy = right_intrinsics[1];
      double rcx = right_intrinsics[2];
      double rcy = right_intrinsics[3];

      // add observations from left and right camera to problem
      for (int k = 0; k < all_targets_[0]->num_points_; k++)
	{
	  total_observations_ += 2;
	  double left_image_x = left_camera_observations[k].image_loc_x;
	  double left_image_y = left_camera_observations[k].image_loc_y;
	  Point3d left_point  = target->pts_[left_camera_observations[k].point_id];
	  double right_image_x = right_camera_observations[k].image_loc_x;
	  double right_image_y = right_camera_observations[k].image_loc_y;
	  Point3d right_point  = target->pts_[right_camera_observations[k].point_id];
	  CostFunction* cost_function = industrial_extrinsic_cal::WristStereoCal::Create(left_image_x,  left_image_y,  left_point,
											 right_image_x, right_image_y, right_point,
											 lfx, lfy, lcx, lcy,
											 rfx, rfy, rcx, rcy);
	  P_->AddResidualBlock(cost_function, NULL, target_pb, right_extrinsics);
	}  // for each point on the target
      if (save_data_){
	  char pose_scene_chars[8];
	  char image_scene_chars[7];
	  sprintf(pose_scene_chars,"_%03d.yaml",scene_);
	  sprintf(image_scene_chars,"_%03d.jpg",scene_);
	  std::string image1_file = all_cameras_[0]->camera_name_ + std::string(image_scene_chars);
	  std::string image2_file = all_cameras_[1]->camera_name_ + std::string(image_scene_chars);
	  std::string extrinsics_scene_d_yaml = std::string("_extrinsics") + std::string(pose_scene_chars);
	  std::string target_extrinsics = all_targets_[0]->target_name_ + extrinsics_scene_d_yaml;
	  if(!all_cameras_[0]->camera_observer_->save_current_image(scene_,image1_file)){
	    ROS_ERROR("couldn't save image named %s",image1_file.c_str());
	  }
	  else{
	    ROS_INFO("saved image as %s",image1_file.c_str());
	  }
	  if(!all_cameras_[1]->camera_observer_->save_current_image(scene_,image2_file)){
	    ROS_ERROR("couldn't save image named %s",image2_file.c_str());
	  }
	  else{
	    ROS_INFO("saved image as %s",image2_file.c_str());
	  }
	  all_targets_[0]->transform_interface_->saveCurrentPose(scene_, target_extrinsics);
	  if(scene_ == 0){
	    std::string camera_extrinsics = all_cameras_[1]->camera_name_ + extrinsics_scene_d_yaml;
	    all_cameras_[1]->pullTransform();
	    all_cameras_[1]->transform_interface_->saveCurrentPose(scene_,camera_extrinsics);
	  }

      }// end save_data_

      ROS_INFO("now have %d observations after scene %d",total_observations_, scene_);
      scene_++;

      sprintf(msg, "stereo_cal_srv now has %d observations after scene %d",total_observations_, scene_);
      res.message = std::string(msg);
      res.success = true;
    }// end if both cameras were abale to observe the target
    else{
      ROS_INFO("Found %d left camera observations and %d right camera observations", (int)left_camera_observations.size(), (int) right_camera_observations.size());
      sprintf(msg, "stereo_cal_srv couldn't find target");
      res.message = std::string(msg);
      res.success = false;
    }
    return(true);
  }; // end observation service};

  bool runCallBack( industrial_extrinsic_cal::cal_srv_solveRequest &req, industrial_extrinsic_cal::cal_srv_solveResponse &res)
  {
    char msg[100];

    // check for obvious errors
    if(problem_initialized_==false){
      ROS_ERROR("must call start service");
      sprintf(msg, "must call start service to initialized ical service");
      res.message = std::string(msg);
      res.success = false;
      return(true);
    }
    if(total_observations_ == 0){
      ROS_ERROR("must call observations service at least once");
      sprintf(msg, "must call observations service at least once");
      res.message = std::string(msg);
      res.success = false;
      return(true);
    }

    // Set intitial conditions of camera poses
    for(int i=0; i<all_cameras_.size(); i++){
      all_cameras_[i]->pullTransform();
      Pose6d CP = all_cameras_[i]->transform_interface_->pullTransform();
      char msg[100];
      sprintf(msg,"Camera %s initial pose",all_cameras_[i]->camera_name_.c_str());
      CP.show(msg);
    }
    /* show initial conditions  TODO remove this debugging code before PR to upstream master
    for(int i=0;i<scene_;i++){
      P_BLOCK te = ceres_blocks_.getMovingTargetPoseParameterBlock(all_targets_[0]->target_name_,i);
      Pose6d TP(te[3],te[4],te[5],te[0],te[1],te[2]);
      char msg[100];
      sprintf(msg,"Target Pose in Scene %d",i);
      TP.show(msg);
    }
    */

    Solver::Options options;
    Solver::Summary summary;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 1000;
    ceres::Solve(options, P_, &summary);
    if (summary.termination_type != ceres::NO_CONVERGENCE)
      {
	double initial_cost = sqrt(2*summary.initial_cost / total_observations_);
	double final_cost = sqrt(2*summary.final_cost / total_observations_);

	ROS_INFO("Problem solved, initial cost = %lf, final cost = %lf", initial_cost, final_cost);
	res.final_cost_per_observation = final_cost;
	if (final_cost <= req.allowable_cost_per_observation)
	  {
	    ROS_INFO("Stereo calibration was successful");
	  }
	else
	  {
	    ROS_ERROR("allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    sprintf(msg, "allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    Pose6d P(all_cameras_[1]->camera_parameters_.pb_extrinsics[3],
		     all_cameras_[1]->camera_parameters_.pb_extrinsics[4],
		     all_cameras_[1]->camera_parameters_.pb_extrinsics[5],
		     all_cameras_[1]->camera_parameters_.pb_extrinsics[0],
		     all_cameras_[1]->camera_parameters_.pb_extrinsics[1],
		     all_cameras_[1]->camera_parameters_.pb_extrinsics[2]);
	    P.show("final right camera_extrinsics");

	    /* show poses of target after optimization TODO remove this debugging code before PR to upstream master
	       for(int i=0;i<scene_;i++){
	       P_BLOCK te = ceres_blocks_.getMovingTargetPoseParameterBlock(all_targets_[0]->target_name_,i);
	       Pose6d TP(te[3],te[4],te[5],te[0],te[1],te[2]);
	       char msg[100];
	       sprintf(msg,"Target Pose in Scene %d",i);
	       TP.show(msg);
	       }
	    */

	    res.message = std::string(msg);
	    res.success = false;
	    return(true);
	  }
	sprintf(msg, "final cost %f", final_cost);
	res.message = std::string(msg);
	res.success = true;
	return(true);
      }
    ROS_ERROR("Stereo Cal NO CONVERGENCE");
    sprintf(msg, "Stereo Cal NO CONVERGENCE");
    res.message = std::string(msg);
    res.success = false;
    return(true);
  }; // end runCallBack()

  bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    for(int i=0; i<all_cameras_.size(); i++){
      all_cameras_[i]->pushTransform();
    }
    for(int i=0; i<all_targets_.size(); i++){
      all_targets_[i]->pushTransform();
    }

    res.message = "Stereo Cal: pushed transforms to cameras and targets";
    res.success = true;
    return(true);

  }// end saveCallback()

  /**
   * \brief Load and set the calibration data from a previous job.
   */
  bool loadCallBack(industrial_extrinsic_cal::FileOp::Request &req, industrial_extrinsic_cal::FileOp::Response &resp)
  {
    if(problem_initialized_ != true ){ // scene_id=0, problem re-initialized
      ROS_INFO("calling start");
      std_srvs::TriggerRequest  sreq;
      std_srvs::TriggerResponse sres;
      startCallBack(sreq,sres);
    }

      // data includes
      //   initial pose between right camera and left camera
      // for each scene
      //   a. image from left camera
      //   b. image from right camera
      //   c. pose from left camera mount to target mount

    // read initial stereo pose
    std::string null_string("");
    all_cameras_[1]->transform_interface_->loadPose(0,null_string);
    Pose6d initial_stereo_cal = all_cameras_[1]->transform_interface_->getCurrentPose();

    bool data_read_ok=true;
    while(data_read_ok){ //read all the scene data from image_directory
      data_read_ok &= all_cameras_[0]->camera_observer_->load_current_image(scene_, null_string);
      data_read_ok &= all_cameras_[1]->camera_observer_->load_current_image(scene_, null_string);
      data_read_ok &= all_targets_[0]->transform_interface_->loadPose(scene_, null_string);
      scene_++;
      if(data_read_ok)
	{
	  // add the target to each camera's observer
	  Roi roi;
	  roi.x_min = 0;
	  roi.y_min = 0;

	  roi.x_max = all_cameras_[0]->camera_parameters_.width;
	  roi.y_max = all_cameras_[0]->camera_parameters_.height;
	  all_cameras_[0]->camera_observer_->clearTargets();
	  all_cameras_[0]->camera_observer_->clearObservations();
	  all_cameras_[0]->camera_observer_->addTarget(all_targets_[0], roi, industrial_extrinsic_cal::cost_functions::WristStereoCal);

	  roi.x_max = all_cameras_[1]->camera_parameters_.width;
	  roi.y_max = all_cameras_[1]->camera_parameters_.height;
	  all_cameras_[1]->camera_observer_->clearTargets();
	  all_cameras_[1]->camera_observer_->clearObservations();
	  all_cameras_[1]->camera_observer_->addTarget(all_targets_[0], roi, industrial_extrinsic_cal::cost_functions::WristStereoCal);

	  // wait for both observers to get their images
	  while (!all_cameras_[0]->camera_observer_->observationsDone());
	  while (!all_cameras_[1]->camera_observer_->observationsDone());

	  // get the observations from left and right cameras
	  CameraObservations left_camera_observations;
	  CameraObservations right_camera_observations;
	  all_cameras_[0]->camera_observer_->getObservations(left_camera_observations);
	  all_cameras_[1]->camera_observer_->getObservations(right_camera_observations);

	  // only add if there are enough observations in both
	  int num_observations = (int)left_camera_observations.size() + (int)right_camera_observations.size();

	  if(left_camera_observations.size() == all_targets_[0]->num_points_ && right_camera_observations.size() == all_targets_[0]->num_points_){
	    ROS_INFO("Found %d left camera observations and %d right camera observations", (int)left_camera_observations.size(), (int) right_camera_observations.size());

	    // add the moving target to the blocks
	    shared_ptr<Target> target = left_camera_observations[0].target;
	    ceres_blocks_.addMovingTarget(all_targets_[0], scene_);
	    P_BLOCK left_intrinsics  = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[0]->camera_name_);
	    P_BLOCK right_intrinsics = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(all_cameras_[1]->camera_name_);
	    P_BLOCK right_extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(all_cameras_[1]->camera_name_);
	    P_BLOCK target_pb        = ceres_blocks_.getMovingTargetPoseParameterBlock(target->target_name_,scene_);
	    double lfx = left_intrinsics[0];
	    double lfy = left_intrinsics[1];
	    double lcx = left_intrinsics[2];
	    double lcy = left_intrinsics[3];
	    double rfx = right_intrinsics[0];
	    double rfy = right_intrinsics[1];
	    double rcx = right_intrinsics[2];
	    double rcy = right_intrinsics[3];

	    // add observations from left and right camera to problem
	    for (int k = 0; k < all_targets_[0]->num_points_; k++)
	      {
		total_observations_ += 2;
		double left_image_x = left_camera_observations[k].image_loc_x;
		double left_image_y = left_camera_observations[k].image_loc_y;
		Point3d left_point  = target->pts_[left_camera_observations[k].point_id];
		double right_image_x = right_camera_observations[k].image_loc_x;
		double right_image_y = right_camera_observations[k].image_loc_y;
		Point3d right_point  = target->pts_[right_camera_observations[k].point_id];
		CostFunction* cost_function = industrial_extrinsic_cal::WristStereoCal::Create(left_image_x,  left_image_y,  left_point,
											       right_image_x, right_image_y, right_point,
											       lfx, lfy, lcx, lcy,
											       rfx, rfy, rcx, rcy);
		P_->AddResidualBlock(cost_function, NULL, target_pb, right_extrinsics);
	      }  // for each point on the target
	  }// end if enough observations, must have found the target in both images
	}// end if data_read_ok
    }
  }

  bool covCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    std::string covariance_file_name("/home/clewis/junk.txt");
    if(!computeCovariance(covariance_file_name)){
      ROS_ERROR("could not compute covariance");
      res.success = false;
      res.message = "failure";
    }
    else{
      res.message = "good job";
      res.success = true;
    }
    return(true);
  };

  bool computeCovariance(std::string& covariance_file_name)
  {
    FILE* fp;
    if ((fp = fopen(covariance_file_name.c_str(), "w")) != NULL)
    {
      ceres::Covariance::Options covariance_options;
      covariance_options.algorithm_type = ceres::DENSE_SVD;
      ceres::Covariance covariance(covariance_options);

      P_BLOCK extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(all_cameras_[1]->camera_name_);

      std::vector<std::pair<const double*, const double*> > covariance_pairs;
      covariance_pairs.push_back(std::make_pair(extrinsics, extrinsics));

      if(covariance.Compute(covariance_pairs, P_))
      {
        fprintf(fp, "stereo covariance block:\n");
        double cov[6*6];
        if(covariance.GetCovarianceBlock(extrinsics, extrinsics, cov)){
          fprintf(fp, "cov is 6x6\n");
          for(int i=0;i<6;i++){
            double sigma_i = sqrt(fabs(cov[i*6+i]));
            for(int j=0;j<6;j++){
              double sigma_j = sqrt(fabs(cov[j*6+j]));
              double value;
              if(i==j){
                value = sigma_i;
              }
              else{
                if(sigma_i==0) sigma_i = 1;
                if(sigma_j==0) sigma_j = 1;
                value = cov[i * 6 + j]/(sigma_i*sigma_j);
              }
              fprintf(fp, "%16.5f ", value);
            }  // end of j loop
            fprintf(fp, "\n");
          }  // end of i loop
        }// end if success getting covariance
        fclose(fp);
        return(true);
      }// end if covariances could be computed
      else{
        ROS_ERROR("could not compute covariance");
      }
    }// end if file opens
    ROS_ERROR("could not open covariance file %s", covariance_file_name.c_str());
    return (false);
  };  // end computeCovariance()

private:
  ros::NodeHandle nh_;
  std::vector<shared_ptr<Camera> > all_cameras_;
  std::vector<shared_ptr<Target> > all_targets_;
  std::string yaml_file_path_;
  std::string camera_file_;
  std::string target_file_;
  CeresBlocks ceres_blocks_;                 /*!< This structure maintains the parameter sets for ceres */
  ros::ServiceServer start_server_;
  ros::ServiceServer observation_server_;
  ros::ServiceServer run_server_;
  ros::ServiceServer save_server_;
  ros::ServiceServer load_server_;
  ros::ServiceServer covariance_server_;
  ceres::Problem *P_;
  bool problem_initialized_;

  bool save_data_;
  std::string data_directory_;
  std::string save_subdir_;

  int total_observations_;
  int scene_;
  industrial_extrinsic_cal::ROSListenerTransInterface *target_to_left_camera_TI_;
};// end of class stereoCalServiceNode

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stereo_cal_service");
  ros::NodeHandle node_handle;
  stereoCalServiceNode SCSN(node_handle);
  ros::spin();
  ros::waitForShutdown();
  return 0;
}


#ifndef REALTIME_UTILS_HPP
#define REALTIME_UTILS_HPP

/*
 * Utilities for realtime control
 *
 * Author: Andrew Barry, <abarry@csail.mit.edu> 2015
 *
 */

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <sstream>
#include <boost/algorithm/string/replace.hpp> // for substring replacement
#include <boost/format.hpp>
#include <boost/filesystem.hpp>

#include <bot_core/rotations.h>
#include <bot_frames/bot_frames.h>

#include <GL/gl.h>
#include <bot_lcmgl_client/lcmgl.h>

#include "../../LCM/mav_pose_t.h"

#include <Eigen/Core>

#include "gtest/gtest.h"

// for some reason gtest segfaults on the odroids
// when you call EXPECT_EQ
#define EXPECT_EQ_ARM(val1, val2) EXPECT_TRUE(val1 == val2) << "\tval1: " << val1 << std::endl << "\tval2: " << val2 << std::endl;

#define EXPECT_APPROX_MAT(expected_val, got_val, tolerance) EXPECT_TRUE( expected_val.isApprox(got_val, tolerance) ) << std::endl << "Expected:" << std::endl << expected_val << std::endl << "Got:" << std::endl << got_val << std::endl;

/**
 * Converts mav_pose_t message into a 12-state vector in the State Estimator frame.
 * @param msg message to convert
 * @param Mz (optional) yaw rotation
 */
Eigen::VectorXd PoseMsgToStateEstimatorVector(const mav_pose_t *msg, const Eigen::Matrix3d Mz = Eigen::Matrix3d::Identity());

/**
 * Converts mav_pose_t message into the Drake global frame.
 *
 * Supports an optional rotation matrix to rotate about yaw, allowing the user to use a local frame that sets
 * yaw to a specified value.
 */
Eigen::VectorXd StateEstimatorToDrakeVector(const mav_pose_t *msg, const Eigen::Matrix3d Mz = Eigen::Matrix3d::Identity());

/**
 * Computes an angle that removes wrapping and minimizes the jump between the last angle
 * and the current angle, assuming that the difference between them is small.
 *
 * @param angle_rad_in angle input (in radians)
 * @param last_angle_rad last angle (in radians) to attempt to find something close to
 *
 * @retval angle without wrapping
 */
double AngleUnwrap(double angle_rad_in, double last_angle_rad);

Eigen::Matrix3d rpy2rotmat(Eigen::Vector3d rpy);
Eigen::Vector3d angularvel2rpydot(Eigen::Vector3d rpy, Eigen::Vector3d omega);
Eigen::Matrix3d angularvel2rpydotMatrix(Eigen::Vector3d rpy);
Eigen::Matrix3d quat2rotmat(Eigen::Vector4d q);
Eigen::Matrix3d rotz(double rotation_around_z);
Eigen::Vector3d rotmat2rpy(Eigen::Matrix3d rot_mat);

double deg2rad(double input_in_deg);

int64_t GetTimestampNow();

bool NonBlockingLcm(lcm_t *lcm);

void DrawOriginLcmGl(lcm_t *lcm);

std::string ReplaceUserVarInPath(std::string path);

double ConvertTimestampToSeconds(int64_t timestamp);

std::tuple<std::string, std::string> GetVideoDirectory(int64_t timestamp, std::string log_directory);

std::string ExecuteProcessGetString(std::string cmd);

std::string GetRealtimeDir();

#endif