// MS WARNINGS MACRO
#define _SCL_SECURE_NO_WARNINGS

// Macro: Program Settings
#define ENABLE_NETWORK_IO 1
#define ENABLE_SAVE_IMAGE 0

#include <iostream>
#include <deque>
#include <utility>
#include <boost/noncopyable.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/range/algorithm.hpp>
#include "data_type.hpp"
#include "image_comparator.hpp"
#include "ppm_reader.hpp"
#include "splitter.hpp"
#include "slide_algorithm/algorithm.hpp"
#include "slide_algorithm/algorithm_2.hpp"
#include "slide_algorithm/truncater.hpp"
#include "slide_algorithm/charles.hpp"
#include "gui.hpp"
#include "network.hpp"
#include "test_tool.hpp"

#include <image_algorithm/yrange2.hpp>
#include <image_algorithm/yrange5.hpp>
#include <image_algorithm/genetic.hpp>
#include <image_algorithm/Murakami.hpp>

class position_manager : boost::noncopyable
{
public:
    typedef question_data position_type;

    position_manager() = default;
    virtual ~position_manager() = default;

    template<class T>
    void add(T && pos, bool const auto__ = false)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        items_.push_back(std::forward<T>(pos));
        auto_.push_back(auto__);
    }

    std::pair<position_type,bool> get()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        auto res = items_.front(); auto res_b = auto_.front();
        items_.pop_front();
        auto_.pop_front();
        return std::make_pair(res, res_b);
    }

    bool empty()
    {
        return items_.empty();
    }

private:
    std::mutex mutex_;
    std::deque<position_type> items_;
    std::deque<bool>          auto_;
};

class analyzer : boost::noncopyable
{
public:
    explicit analyzer(
        boost::shared_ptr<network::client> const& network_client,
        int const problem_id, std::string const& player_id,
        bool const is_auto, bool const is_blur
#if ENABLE_SAVE_IMAGE
        ,std::string const& dir_path = "./saved_image"
#endif
        )
        : client_(network_client), problem_id_(problem_id), player_id_(player_id)
        , is_auto_(is_auto), is_blur_(is_blur)
#if ENABLE_SAVE_IMAGE
        , dir_path_(dir_path)
#endif
    {
    }
    virtual ~analyzer() = default;

    void operator() (position_manager& manager)
    {
        splitter sp;

        // 問題文の入手
        raw_data_ = get_raw_question();
        data_     = get_skelton_question(raw_data_);

        // 2次元画像に分割
        split_image_ = sp.split_image(raw_data_);

        // compare用の画像を作成
        auto const arranged_split = is_blur_ ? sp.split_image_gaussianblur(split_image_) : split_image_;
        auto const image_comp    = comparator_   .image_comp(raw_data_, arranged_split);
		auto const image_comp_dx = comparator_dx_.image_comp(raw_data_, arranged_split);

        // 原画像推測部
        yrange2 yrange2_(raw_data_, image_comp);
        Murakami murakami_(raw_data_, image_comp,true);

        // GUI Threadの起動
        gui::manager gui_thread(
            [this, &manager](std::vector<std::vector<point_type>> const& data)
            {
                // 回答としてマーク -> 回答ジョブに追加
                auto clone = data_.clone();
                clone.block = data;
                manager.add(convert_block(clone));
            });

        // YRange2 -> YRange5 Thread
        boost::thread y_thread(
            [&, this]()
            {
                // YRange2
                auto yrange2_resolve = yrange2_();
                if (!yrange2_resolve.empty())
		        {
                    // Shoot
                    if(is_auto_)
                    {
                        is_auto_ = false;

                        std::cout << "Yrange2 Auto" << std::endl;
                        auto clone = data_.clone();
                        clone.block = yrange2_resolve[0].points;
                        manager.add(convert_block(clone), true);
                    }

                    // GUI
			        for (int y2 = yrange2_resolve.size() - 1; y2 >= 0; --y2)
			        {
                        gui_thread.push_back(
                            boost::bind(gui::make_mansort_window, split_image_, yrange2_resolve.at(y2).points, "yrange2")
                            );
			        }
		        }

                // YRange5
				if (raw_data_.split_num.first > 3 || raw_data_.split_num.second > 3){
					auto yrange5_resolve = yrange5(raw_data_, image_comp)(yrange2_.sorted_matrix());
					if (!yrange5_resolve.empty())
					{
						// Shoot
						if(is_auto_)
						{
							is_auto_ = false;

                            std::cout << "Yrange5 Auto" << std::endl;
							auto clone = data_.clone();
							clone.block = yrange5_resolve[0].points;
							manager.add(convert_block(clone), true);
						}

						// GUI
						for (int y5 = yrange5_resolve.size() - 1; y5 >= 0; --y5)
						{
							gui_thread.push_back(
								boost::bind(gui::make_mansort_window, split_image_, yrange5_resolve.at(y5).points, "yrange5")
								);
						}
					}
				}
            });

        // Murakami Thread
        boost::thread m_thread(
            [&]()
            {
                // Murakami
		    	std::cout << "村上モード" << std::endl;
                auto murakami_resolve = murakami_()[0].points;
				std::vector<std::vector<point_type>> murakami_dx_resolve, murakami_w_resolve, murakami_dx_w_resolve;
				if (!murakami_resolve.empty())
				{
					if(is_auto_)
					{
						is_auto_ = false;

                        std::cout << "Murakami Auto" << std::endl;
						auto clone = data_.clone();
						clone.block = murakami_resolve;
						manager.add(convert_block(clone), true);
					}
					gui_thread.push_back(boost::bind(gui::make_mansort_window, split_image_, murakami_resolve, "Murakami"));
                }
				if (murakami_resolve.empty()){
					std::cout << "デラックス村上モード" << std::endl;
					Murakami murakami_dx(raw_data_, image_comp_dx, true);
					murakami_dx_resolve = murakami_dx()[0].points;
					if (!murakami_dx_resolve.empty())gui_thread.push_back(boost::bind(gui::make_mansort_window, split_image_, murakami_dx_resolve, "Murakami_dx"));
					if (murakami_dx_resolve.empty()){
						std::cout << "W村上モード" << std::endl;
						Murakami murakami_w(raw_data_, image_comp, false);
						murakami_w_resolve = murakami_w()[0].points;
						if (!murakami_w_resolve.empty())gui_thread.push_back(boost::bind(gui::make_mansort_window, split_image_, murakami_w_resolve, "Murakami_w"));
						if (murakami_w_resolve.empty()){
							std::cout << "デラックスW村上モード" << std::endl;
							Murakami murakami_dx_w(raw_data_, image_comp_dx, false);
							murakami_dx_w_resolve = murakami_w()[0].points;
							if (!murakami_dx_w_resolve.empty())gui_thread.push_back(boost::bind(gui::make_mansort_window, split_image_, murakami_dx_w_resolve, "Murakami_dx_w"));
						}
					}
				}
            });

        gui_thread.push_back(
            boost::bind(gui::make_mansort_window, split_image_, "Yor are the sorter!!! Sort this!")
            );

        // 各Threadの待機
        y_thread.join();
        m_thread.join();
        gui_thread.wait_all_window();
    }

    int calc_cost(answer_type const& ans) const
    {
        int cost = (timer_.elapsed().wall / 1000000000) * 100;
        cost += ans.list.size() * data_.cost_select;
        boost::for_each(
            ans.list,
            [&,this](answer_atom const& move)
            {
                cost += move.actions.size() * data_.cost_change;
            });

        return cost;
    }

    std::string submit(answer_type const& ans, bool is_auto = false) const
    {
        std::lock_guard<std::mutex> lock(submit_mutex_);
        int const cost = calc_cost(ans);

	if(is_auto)
	{
            auto submit_result = client_->submit(problem_id_, player_id_, ans);
            return submit_result.get();
	}

        char input;
        std::cout << "★Submit: Cost = " << cost << " [Y/n]★";
        std::cin.get(input);

        if(input == '\n' || input == 'y' || input == 'Y')
        {
            auto submit_result = client_->submit(problem_id_, player_id_, ans);
            return submit_result.get();
        }

        return std::string();
    }

private:
    question_raw_data get_raw_question() const
    {
#if ENABLE_NETWORK_IO
        // ネットワーク通信から
        std::string const data = client_->get_problem(problem_id_).get();
        timer_.start();
#if ENABLE_SAVE_IMAGE
        std::ofstream ofs((boost::format("%s/prob%02d.ppm") % dir_path_ % problem_id_).str(), std::ios::binary);
        ofs << data;
        ofs.close();
#endif
        return ppm_reader().from_data(data);
#else
        // ファイルから
        std::string const path("prob01.ppm");
        timer_.start();
        return ppm_reader().from_file(path);
#endif
    }

    question_data get_skelton_question(question_raw_data const& raw) const
    {
        question_data formed = {
            problem_id_,
            player_id_,
            raw.split_num,
            raw.selectable_num,
            raw.cost.first,
            raw.cost.second,
            std::vector<std::vector<point_type>>()
        };

        return formed;
    }

    question_data convert_block(question_data const& data) const
    {
        auto res = data.clone();

        for(int i = 0; i < data.size.second; ++i)
        {
            for(int j = 0; j < data.size.first; ++j)
            {
                auto const& target = data.block[i][j];
                res.block[target.y][target.x] = point_type{j, i};
		}
        }

        return res;
    }

    int const problem_id_;
    std::string const player_id_;
    bool is_auto_;
    bool const is_blur_;

    question_raw_data raw_data_;
    question_data         data_;
    split_image_type  split_image_;
	split_image_type  split_image_gaussianblur;

    mutable boost::shared_ptr<network::client> client_;
    image_comparator    comparator_;
	image_comparator_dx comparator_dx_;

    // 送信用mutex
    mutable boost::timer::cpu_timer timer_;
    mutable std::mutex submit_mutex_;

#if ENABLE_SAVE_IMAGE
    mutable std::string dir_path_;
#endif
};

void submit_func(question_data question, analyzer const& analyze, bool is_auto)
{
    algorithm algo;
    truncater talgo;

    algorithm_2 algo2;
	charles algo3;

    boost::optional<answer_type> algo_answer;
    boost::optional<answer_type> talgo_answer;
    boost::optional<answer_type> answer;

    auto algo_thread = boost::thread(
        [&algo, &algo_answer, &question]() {
            algo.reset(question);
            algo_answer = algo.get();
        }
    );

    auto talgo_thread = boost::thread(
        [&talgo, &talgo_answer, &question]() {
            talgo.reset(question);
            talgo_answer = talgo.get();
        }
    );

    algo_thread.join();
    talgo_thread.join();

    int algo_score = algo_answer->get_score(question.cost_select, question.cost_change);
    int talgo_score = talgo_answer->get_score(question.cost_select, question.cost_change);
    if (algo_score < talgo_score) {
        answer = algo_answer;
    } else {
        answer = talgo_answer;
    }

    if(answer) // 解が見つかった
    {
#if ENABLE_NETWORK_IO
        // TODO: 前より良くなったら提出など(普通にいらないかも．提出前に目grepしてるわけだし)
		std::cout << "Submit! Wait 5 sec" << std::endl;

        std::string result;

        result = analyze.submit(answer.get(), is_auto);
        std::cout << "Submit Result: " << result << std::endl;

        if(result.find("ACCEPTED") == std::string::npos)
        {
            std::cout << "Submit Result is not \"ACCEPTED\"" << std::endl;
            return;
        }

	// FIXME: 汚いしセグフォする//セグフォはしなくなったらしい
	int wrong_number = std::stoi(result.substr(result.find(" ")));
	if (wrong_number == 0)
	{
		algo2.reset(question);
		auto const answer = algo2.get();
		if (algo2.overlimitcheck()){
			if (answer)
			{
				result = analyze.submit(answer.get());
				std::cout << "Submit Result: " << result << std::endl;
				std::cout << "勝った！" << std::endl;
			}
		}
		else{
			std::cout << "algo2は答えを返しませんでした" << std::endl;
		}

		algo3.reset(question);
		auto const answer2 = algo3.get();
		if (algo3.overlimitcheck()){
			if (answer2)
			{
				result = analyze.submit(answer2.get());
				std::cout << "Submit Result 2 : " << result << std::endl;
				std::cout << "さらに勝った！" << std::endl;
			}
		}
		else{
			std::cout << "algo3は答えを返しませんでした" << std::endl;
		}
	}
#else
        test_tool::emulator emu(question);
        auto result = emu.start(answer.get());
        std::cout << "Wrong: " << result.wrong << std::endl;
        std::cout << "Cost : " << result.cost  << std::endl;
        std::cout << "---" << std::endl;
#endif
    }
}

int main(int const argc, char const* argv[])
{
    std::string server_addr;
	int         problemid = 0;
    auto const  token = "1";//"3935105806";
    bool        is_auto;
    bool        is_blur;
    std::string url_format;
#if ENABLE_SAVE_IMAGE
    std::string save_dir;
#endif

    try
    {
        namespace po = boost::program_options;
        po::options_description opt("option");
        opt.add_options()
            ("help,h"    , "produce help message")
            ("auto,a"    , "auto submit flag")
            ("blur,b"    , "gaussian blur to image")
#if ENABLE_SAVE_IMAGE
            ("save_dir"  , po::value<std::string>(&save_dir)   , "(require)set image dir to save")
#endif
#if ENABLE_NETWORK_IO
            ("prob_format,f", po::value<std::string>(&url_format)->default_value("/problem/prob%02d.ppm"), "set problem format(ex. /problem/prob%02d.ppm)")
            ("server,s"  , po::value<std::string>(&server_addr), "(require)set server ip address")
            ("problem,p" , po::value<int>(&problemid)          , "(require)set problem_id")
#endif
            ;

        po::variables_map argmap;
        po::store(po::parse_command_line(argc, argv, opt), argmap);
        po::notify(argmap);

#if ENABLE_NETWORK_IO
        if(argmap.count("help") || !argmap.count("server") || !argmap.count("problem"))
#else
        if(argmap.count("help"))
#endif
        {
            std::cout << opt << std::endl;
            return 0;
        }

        is_auto = argmap.count("auto");
        is_blur = argmap.count("blur");
    }
    catch(boost::program_options::error_with_option_name const& e)
    {
        std::cout << e.what() << std::endl;
        std::exit(-1);
    }

    auto             network_client = boost::make_shared<network::client>(server_addr, url_format/*, submit_path*/);

#if ENABLE_SAVE_IMAGE
    if(save_dir.empty()) save_dir = "./saved_image";
    analyzer         analyze(network_client, problemid, token, is_auto, is_blur, save_dir);
#else
    analyzer         analyze(network_client, problemid, token, is_auto, is_blur);
#endif
    position_manager manager;

    boost::thread thread(boost::bind(&analyzer::operator(), &analyze, std::ref(manager)));
    boost::thread_group submit_threads;

    while(thread.joinable())
    {
        if(!manager.empty())
        {
            auto question = manager.get();

            // 手順探索部
            submit_threads.create_thread(
                boost::bind(submit_func, question.first, boost::ref(analyze), question.second)
                );
        }
    }

    thread.join();
    submit_threads.join_all();

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/factorial.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/timing.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <cmath>

using nt2::tag::factorial_;

#define RS(T,V1,V2) (T, (V1) ,(V2))

#undef RS


#include "fixtures.h"
#include "geometric/coordinate.hpp"
#include "id/line.hpp"
#include "id/stop.hpp"
#include "service/master.hpp"

#include "test/toolkit.hpp"

#include <vector>

using namespace nepomuk;

// make sure we get a new main function here
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(annotate_stops)
{
    service::Master data_service(TRANSIT_THREE_LINES_EXAMPLE_FIXTURE);

    auto const &geometry_annotation = data_service.geometry_annotation();

    // construct once
    BOOST_CHECK(&geometry_annotation == &data_service.geometry_annotation());

    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{0}), make_coordinate(0.005, 0.0051));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{1}), make_coordinate(0.005, 0.0049));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{2}), make_coordinate(0.0152, 0.005));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{3}), make_coordinate(0.0253, 0.005));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{4}), make_coordinate(0.0354, 0.005));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{5}), make_coordinate(0.0455, 0.005));
    BOOST_CHECK_EQUAL(geometry_annotation.get(StopID{6}), make_coordinate(0.0253, -0.104));
}

BOOST_AUTO_TEST_CASE(annotate_lines)
{
    service::Master data_service(TRANSIT_THREE_LINES_EXAMPLE_FIXTURE);

    auto const &geometry_annotation = data_service.geometry_annotation();

    std::vector<geometric::WGS84Coordinate> expected_coordinates;
    expected_coordinates.push_back(make_coordinate(0.005, 0.005));
    expected_coordinates.push_back(make_coordinate(0.0051, 0.005));
    expected_coordinates.push_back(make_coordinate(0.0052, 0.005));
    expected_coordinates.push_back(make_coordinate(0.00525, 0.005));
    expected_coordinates.push_back(make_coordinate(0.0053, 0.005));
    expected_coordinates.push_back(make_coordinate(0.00535, 0.005));
    expected_coordinates.push_back(make_coordinate(0.0054, 0.005));
    expected_coordinates.push_back(make_coordinate(0.00545, 0.005));
    expected_coordinates.push_back(make_coordinate(0.0055, 0.005));

    auto const range = geometry_annotation.get(LineID{0}, StopID{0}, StopID{5});
    BOOST_CHECK_EQUAL_COLLECTIONS(
        range.begin(), range.end(), expected_coordinates.begin(), expected_coordinates.end());
}


//
// Created by nvidia on 6/22/18.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <algorithm> // for copy
#include <iterator> // for ostream_iterator
#include <boost/graph/graph_utility.hpp>
#include "../lib/SpVec.hpp"
#include "../lib/superpixel_seg.hpp"
#include "../lib/distance.hpp"
#include "../lib/FloodFillSsdSuperpixelAlgorithm.hpp"
#include "../lib/RTreesClassifier.hpp"
#include "../lib/util.hpp"

using namespace cv;
using namespace std;
using namespace cv::ximgproc;
using namespace cove;

int main(int argc, char** argv) {
    bool fastFinish = false;
#ifdef DEBUG
    namedWindow("params", 1);
#endif
    ALGORITHM_PARAM(blurKernel, 3, 10)
    ALGORITHM_PARAM(iterations, 10, 20)
    ALGORITHM_PARAM(blurDev, 0, 100)
    ALGORITHM_PARAM(regionSize, 7, 50)
    ALGORITHM_PARAM(ratio, 70, 200)
    ALGORITHM_PARAM(cannyLow, 70, 150)
    ALGORITHM_PARAM(ssdEpsilonSlider, 10, 250)

    int frameno = 0;
    Mat frame;
    Mat label;

    RTreesClassifier classifier;
//    Ptr<ml::RTrees> model;
//    model = ml::StatModel::load<ml::RTrees>("RTREES");
    if( classifier.loadModelFromPath("RTREES") ) {
        cout << "The classifier " << "RTREES" << " is loaded.\n";
    } else {
        cout << "Could not read the classifier " << "RTREES" << endl;
        return 1;
    }

    FloodFillSsdSuperpixelAlgorithm algorithm(80, 60, regionSize, static_cast<float>(ssdEpsilonSlider) / 10, blurKernel, blurDev, iterations,
                                              static_cast<float>(ratio) / 1000, cannyLow, cannyLow*2.5);

    vector<double> ius;
    for (int idx = static_cast<int>(argc*0.8); idx < argc; idx++) {
        clock_t start=CLOCK();

#ifdef DEBUG
        if (blurKernel % 2 == 0) {
            blurKernel++;
        }
#endif

        string path(argv[idx]);
        string filename = util::getFileName(path, true);
        string labelPath = "img/labels/" + filename;
        frame = imread(path);
        label = imread(labelPath);
        SHOW_ALWAYS("frame", frame)
        SHOW_ALWAYS("label", label)
        if (frame.empty() || label.empty()) {
            cout << "frame empty" << endl;
            break;
        }

        Mat floorMask;
        Mat resizedLabel;
        Mat sizedFrame;

        Mat coloredSizedFrame;
        auto super = algorithm.getSuperPixels(frame, floorMask, sizedFrame);
        cvtColor(sizedFrame, coloredSizedFrame, COLOR_BGR2Lab);
        super->iterate(iterations);
        super->enforceLabelConnectivity(15);
        Mat spxLabels;
        // floor inference
        super->getLabels(spxLabels);
        cout << "t" << endl;
        classifier.getFloorMask(spxLabels, coloredSizedFrame, super->getNumberOfSuperpixels(), floorMask);
        cout << "t" << endl;

//
//
//        Mat pxSamples = Mat::zeros(0, 10, CV_32F);
//        for (int i = 0; i < super->getNumberOfSuperpixels(); i++) {
//            Mat mask;
//            inRange(spxLabels, Scalar(i), Scalar(i), mask);
//            pxSamples.push_back(RTreesClassifier::getData(mask, coloredSizedFrame));
//            // inference
//        }
//        Mat labels;
//        model->predict(pxSamples, labels);
//        cout << labels.size() << endl;
//        cout << labels.depth() << endl;
//        // compute floorMask
//        Mat temp;
//        floorMask = Mat::zeros(sizedFrame.size(), CV_8U);
//        for (int i = 0; i < super->getNumberOfSuperpixels(); i++) {
//            if (labels.at<float>(0, i) > 2.5) {
//                continue;
//            }
//            inRange(spxLabels, Scalar(i), Scalar(i), temp);
//            bitwise_or(floorMask, temp, floorMask);
//        }

        dilate(floorMask, floorMask, Mat(), Point(-1, -1), 2);
        erode(floorMask, floorMask, Mat(), Point(-1, -1), 2);
        resize(label, resizedLabel, floorMask.size(), 0, 0, INTER_NEAREST);
        SHOW_ALWAYS("resize", resizedLabel)
        SHOW_ALWAYS("calced floor", floorMask)
        util::overlayMask(sizedFrame, floorMask, Scalar(0, 0, 255), sizedFrame);
        SHOW_ALWAYS("display", sizedFrame);

        // get labeled floor mask
        Mat tmp;
        inRange(resizedLabel, Scalar(0,0,255), Scalar(0,0,255), tmp);
        inRange(resizedLabel, Scalar(0,255,0), Scalar(0,255,0), resizedLabel);
        bitwise_or(tmp, resizedLabel, resizedLabel);

        // iu calculation
        bitwise_and(resizedLabel, floorMask, tmp);
        SHOW_ALWAYS("TRUE POS", tmp);
        int truePositive = countNonZero(tmp);
        bitwise_xor(resizedLabel, floorMask, tmp);
        SHOW_ALWAYS("FALSE *", tmp);
        int falsePosandNeg = countNonZero(tmp);
        double iu = truePositive / static_cast<double>(falsePosandNeg + truePositive);
        ius.push_back(iu);
        cout << "IU: " << iu << endl;

//        true positive / (true positive + false positive + false negative)

        // mask the floor and etc

//        Mat zeros(floorMask.size(), CV_8UC3, Scalar(0));
//        Mat red(floorMask.size(), CV_8UC3, Scalar(0, 0, 255));
//        red.copyTo(zeros, floorMask);
//        addWeighted(resized, 0.7, zeros, 0.3, 0.0, resized);

//        SHOW_ALWAYS("output", resized);
        // =========== time fps and exit ===========
        double dur = CLOCK()-start;
        printf("avg time per frame %f ms. fps %f. frameno = %d\n",avgdur(dur),avgfps(),frameno++ );
        cout << argv[idx] << endl;
        if (fastFinish) {
            continue;
        }
        int key = waitKey(1000);
        if (key == 27) {
            break;
        } else if (key == 99) {
            fastFinish = true;
        }

    }
    double sum = std::accumulate(ius.begin(), ius.end(), 0.0);
    double mean = sum / ius.size();
    std::vector<double> diff(ius.size());
    std::transform(ius.begin(), ius.end(), diff.begin(), [mean](double x) { return x - mean; });
    double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
    double stdev = std::sqrt(sq_sum / ius.size());

    cout << "MEAN: " << mean << endl;
    cout << "STDV: " << stdev << endl;

    return 0;
}


#include <iostream>
#include <string>

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <tms_msg_ss/SkeletonArray.h>
#include <tms_msg_ss/CameraPosture.h>
#include <tms_msg_ss/SkeletonStreamWrapper.h>

<<<<<<< HEAD
static const float marker_color[][3] = {  // BGR
    {1.0f, 0, 0},
    {0, 1.0f, 0},
    {0, 0, 1.0f},
    {1.0f, 1.0f, 0},
    {1.0f, 0, 1.0f},
    {0, 1.0f, 1.0f}};

class SkeletonViewer
{
public:
  SkeletonViewer();
  ~SkeletonViewer();
  void callback_skeleton(const tms_msg_ss::SkeletonArray::ConstPtr &msg);
  void run();

private:
  ros::NodeHandle _nh;
  ros::Publisher *_pmarker_array_pub;
  bool gotCameraPosture;
  Eigen::Vector3f translation;
  Eigen::Quaternionf rotation;

  inline void toWorldPoint(Eigen::Vector3f &vec);
};

SkeletonViewer::SkeletonViewer() : gotCameraPosture(false)
=======
static const float marker_color[][3] = { // BGR
  {1.0f, 0, 0},
  {0, 1.0f, 0},
  {0, 0, 1.0f},
  {1.0f, 1.0f, 0},
  {1.0f, 0, 1.0f},
  {0, 1.0f, 1.0f}
};

class SkeletonViewer
{
  public:
    SkeletonViewer();
    ~SkeletonViewer();
    void callback_skeleton(const tms_msg_ss::SkeletonArray::ConstPtr& msg);
    void run();
  private:
    ros::NodeHandle _nh;
    ros::Publisher *_pmarker_array_pub;
    bool gotCameraPosture;
    Eigen::Vector3f translation;
    Eigen::Quaternionf rotation;

    inline void toWorldPoint(Eigen::Vector3f &vec);
};

SkeletonViewer::SkeletonViewer() :
  gotCameraPosture(false)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
}

SkeletonViewer::~SkeletonViewer()
{
}

inline void SkeletonViewer::toWorldPoint(Eigen::Vector3f &vec)
{
  vec = this->rotation.matrix() * vec + this->translation;
  return;
}

<<<<<<< HEAD
template < class T >
std::string to_str(const T &t)
=======
template <class T>
std::string to_str(const T& t)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  std::stringstream ss;
  ss << t;
  return ss.str();
}

<<<<<<< HEAD
void SkeletonViewer::callback_skeleton(const tms_msg_ss::SkeletonArray::ConstPtr &msg)
=======
void SkeletonViewer::callback_skeleton(const tms_msg_ss::SkeletonArray::ConstPtr& msg)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  visualization_msgs::MarkerArray marker_array;
  for (int j = 0; j < msg->data.size(); j++)
  {
    tms_msg_ss::Skeleton skeleton = msg->data[j];
    ROS_INFO("Received skeleton %d", skeleton.user_id);

    uint32_t shape = visualization_msgs::Marker::SPHERE;
    for (int i = 0; i < 25; i++)
    {
      visualization_msgs::Marker marker;
      marker.header.frame_id = "/world_link";
      marker.header.stamp = ros::Time::now();
      std::string name("skeleton");
<<<<<<< HEAD
      marker.ns = name.append(to_str< int >(j + 1));
=======
      marker.ns = name.append(to_str<int>(j+1));
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      marker.id = i;
      marker.type = shape;
      marker.action = visualization_msgs::Marker::ADD;
      Eigen::Vector3f pos(skeleton.position[i].x, skeleton.position[i].y, skeleton.position[i].z);
      if (gotCameraPosture)
      {
        this->toWorldPoint(pos);
      }
      marker.pose.position.x = pos.x();
      marker.pose.position.y = pos.y();
      marker.pose.position.z = pos.z();
      marker.pose.orientation.x = skeleton.orientation[i].x;
      marker.pose.orientation.y = skeleton.orientation[i].y;
      marker.pose.orientation.z = skeleton.orientation[i].z;
      marker.pose.orientation.w = skeleton.orientation[i].w;
      marker.scale.x = 0.05;
      marker.scale.y = 0.05;
      marker.scale.z = 0.05;
<<<<<<< HEAD
      marker.color.r = marker_color[j / 6][2];
      marker.color.g = marker_color[j / 6][1];
      marker.color.b = marker_color[j / 6][0];
      marker.color.a = 1.0f * (float)skeleton.confidence[i] / 2.0f;
=======
      marker.color.r = marker_color[j/6][2];
      marker.color.g = marker_color[j/6][1];
      marker.color.b = marker_color[j/6][0];
      marker.color.a = 1.0f*(float)skeleton.confidence[i]/2.0f;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      marker.lifetime = ros::Duration();
      marker_array.markers.push_back(marker);
    }
  }
  _pmarker_array_pub->publish(marker_array);

  return;
}

void SkeletonViewer::run()
{
  ros::Subscriber sub_skeleton =
<<<<<<< HEAD
      this->_nh.subscribe("integrated_skeleton_stream", 1, &SkeletonViewer::callback_skeleton, this);
  ros::Publisher marker_pub = _nh.advertise< visualization_msgs::MarkerArray >("skeleton_visualization", 1);
=======
    this->_nh.subscribe("integrated_skeleton_stream", 1, &SkeletonViewer::callback_skeleton, this);
  ros::Publisher marker_pub = _nh.advertise<visualization_msgs::MarkerArray>(
      "skeleton_visualization", 1);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  _pmarker_array_pub = &marker_pub;
  ros::spin();
  return;
}

<<<<<<< HEAD
int main(int argc, char **argv)
=======
int main (int argc, char **argv)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  ros::init(argc, argv, "skeleton_viewer");

  SkeletonViewer viewer;

  viewer.run();

  return 0;
}


//----------------------------------------------------------------------------
// Copyright (C) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the Server Side Public License, version 1,
// as published by the author.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// Server Side Public License for more details.
//
// You should have received a copy of the Server Side Public License
// along with this program. If not, see
// <https://github.com/NilFoundation/plugin/blob/master/LICENSE_1_0.txt>.
//----------------------------------------------------------------------------

#define BOOST_TEST_MODULE por_test

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/filecoin/storage/proofs/core/por.hpp>
#include <nil/filecoin/storage/proofs/core/drgraph.hpp>

BOOST_AUTO_TEST_SUITE(por_test_suite)

template<typename MerkleTreeType>
void test_merklepor() {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    const std::size_t leaves = 16;
    const auto pub_params = PublicParams {
        leaves,
        private : false,
    };

    std::vector<std::uint8_t> data = (0..leaves).flat_map(| _ | fr_into_bytes(&Fr::random(rng))).collect();
    const auto porep_id = [3; 32];
    const auto graph = BucketGraph<typename MerkleTreeType::hash_type> (leaves, BASE_DEGREE, 0, porep_id);
    const auto tree = create_base_merkle_tree::<Tree>(None, graph.size(), data.as_slice());

    const auto pub_inputs = PublicInputs {
        challenge : 3,
        commitment : Some(tree.root()),
    };

    const auto leaf =
        typename MerkleTreeType::hash_type::digest_type::try_from_bytes(data_at_node(data.as_slice(), pub_inputs.challenge), )
            ;

    const auto priv_inputs = PrivateInputs(leaf, &tree);

    const auto proof = PoR::<Tree>::prove(&pub_params, &pub_inputs, &priv_inputs).expect("proving failed");

    const auto is_valid = PoR::<Tree>::verify(&pub_params, &pub_inputs, &proof).expect("verification failed");

    BOOST_ASSERT(is_valid);
}

type TestTree<H, U> = MerkleTreeWrapper<H, DiskStore <H::digest_type>, U, 0, 0> ;

BOOST_AUTO_TEST_CASE(merklepor_pedersen_binary) {
    test_merklepor<TestTree<PedersenHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_poseidon_binary) {
    test_merklepor<TestTree<PoseidonHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_sha256_binary) {
    test_merklepor<TestTree<Sha256Hasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_blake2s_binary) {
    test_merklepor<TestTree<Blake2sHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_pedersen_quad) {
    test_merklepor<TestTree<PedersenHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_poseidon_quad) {
    test_merklepor<TestTree<PoseidonHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_sha256_quad) {
    test_merklepor<TestTree<Sha256Hasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_blake2s_quad) {
    test_merklepor<TestTree<Blake2sHasher, 4>>();
}

// Takes a valid proof and breaks it.
DataProof<Proof> make_bogus_proof<Proof : BasicMerkleProof>(rng
                                              : XorShiftRng, proof
                                              : DataProof<Proof>) {
    const auto bogus_leaf = Proof::Hasher::digest_type::random(rng);
    proof.proof.break_me(bogus_leaf);
    return proof;
}

void test_merklepor_validates<Tree : MerkleTreeTrait>() {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    const std::size_t leaves = 64;
    const auto pub_params = PublicParams {
        leaves,
        private : false,
    };

    std::vector<std::uint8_t> data = (0..leaves).flat_map(| _ | fr_into_bytes(&Fr::random(rng))).collect();

    const auto porep_id = [99; 32];

    const auto graph = BucketGraph<typename MerkleTreeType::hash_type> (leaves, BASE_DEGREE, 0, porep_id);
    const auto tree = create_base_merkle_tree::<Tree>(None, graph.size(), data.as_slice());

    const auto pub_inputs = PublicInputs {
        challenge : 3,
        commitment : Some(tree.root()),
    };

    const auto leaf =
        typename MerkleTreeType::hash_type::digest_type::try_from_bytes(data_at_node(data.as_slice(), pub_inputs.challenge), )
            ;

    const auto priv_inputs = PrivateInputs::<Tree> (leaf, &tree);

    const auto good_proof = PoR::<Tree>::prove(&pub_params, &pub_inputs, &priv_inputs).expect("proving failed");

    const auto verified = PoR::<Tree>::verify(&pub_params, &pub_inputs, &good_proof).expect("verification failed");
    BOOST_ASSERT(verified);

    const auto bad_proof = make_bogus_proof::<MerkleTreeType::Proof>(rng, good_proof);

    const auto verified = PoR::<Tree>::verify(&pub_params, &pub_inputs, &bad_proof).expect("verification failed");

    // A bad proof should not be verified!
    BOOST_ASSERT(!verified);
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_sha256_binary) {
    test_merklepor_validates<TestTree<Sha256Hasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_blake2s_binary) {
    test_merklepor_validates<TestTree<Blake2sHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_pedersen_binary) {
    test_merklepor_validates<TestTree<PedersenHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_poseidon_binary) {
    test_merklepor_validates<TestTree<PoseidonHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_sha256_quad) {
    test_merklepor_validates<TestTree<Sha256Hasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_blake2s_quad) {
    test_merklepor_validates<TestTree<Blake2sHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_pedersen_quad) {
    test_merklepor_validates<TestTree<PedersenHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_poseidon_quad) {
    test_merklepor_validates<TestTree<PoseidonHasher, 4>>();
}

template<typename MerkleTreeType>
void test_merklepor_validates_challenge_identity() {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    const std::size_t leaves = 64;

    const auto pub_params = PublicParams {
        leaves,
        private : false,
    };

    std::vector<std::uint8_t> data = (0..leaves).flat_map(| _ | fr_into_bytes(&Fr::random(rng))).collect();

    const auto porep_id = [32; 32];
    const auto graph = BucketGraph<typename MerkleTreeType::hash_type> (leaves, BASE_DEGREE, 0, porep_id);
    const auto tree = create_base_merkle_tree::<Tree>(None, graph.size(), data.as_slice());

    const auto pub_inputs = PublicInputs {
        challenge : 3,
        commitment : Some(tree.root()),
    };

    const auto leaf =
        typename MerkleTreeType::hash_type::digest_type::try_from_bytes(data_at_node(data.as_slice(), pub_inputs.challenge), )
            ;

    const auto priv_inputs = PrivateInputs::<Tree> (leaf, &tree);

    const auto proof = PoR::<Tree>::prove(&pub_params, &pub_inputs, &priv_inputs).expect("proving failed");

    const auto different_pub_inputs = PublicInputs {
        challenge : 999,
        commitment : Some(tree.root()),
    };

    const auto verified = PoR::<Tree>::verify(&pub_params, &different_pub_inputs, &proof).expect("verification failed");

    // A proof created with a the wrong challenge not be verified!
    BOOST_ASSERT(!verified);
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_sha256_binary) {
    test_merklepor_validates_challenge_identity<TestTree<Sha256Hasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_blake2s_binary) {
    test_merklepor_validates_challenge_identity<TestTree<Blake2sHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_pedersen_binary) {
    test_merklepor_validates_challenge_identity<TestTree<PedersenHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_poseidon_binary) {
    test_merklepor_validates_challenge_identity<TestTree<PoseidonHasher, 2>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_sha256_quad) {
    test_merklepor_validates_challenge_identity<TestTree<Sha256Hasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_blake2s_quad) {
    test_merklepor_validates_challenge_identity<TestTree<Blake2sHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_pedersen_quad) {
    test_merklepor_validates_challenge_identity<TestTree<PedersenHasher, 4>>();
}

BOOST_AUTO_TEST_CASE(merklepor_actually_validates_challenge_identity_poseidon_quad) {
    test_merklepor_validates_challenge_identity<TestTree<PoseidonHasher, 4>>();
}

BOOST_AUTO_TEST_SUITE_END()