/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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 <iostream>
#include <boost/lexical_cast.hpp>

void check(const char* value)
{
     float percent = boost::lexical_cast<float>(value)*100.f ;   // express as integer percentage

     std::cout << " value "  << value
               << " percent " << percent
               << std::endl ;

     unsigned upercent = percent ;

     std::cout << " value "  << value
               << " percent " << percent
               << " upercent " << upercent
               << std::endl ;
}

int main()
{
     check("0.99");
     check("0.999");

     return 0 ;
}


/**
 * @brief tests for the bar class
 *
 * @file barraTest.cpp
 * @author Luiz Victor Linhares Rocha
 * @date 2018-09-24
 * @copyright 2018
 */

#include <iostream>
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../rede/barra.hpp"
#include "../util/complexutils.h"

namespace tt = boost::test_tools;
using neuralFlux::BarPtr;
using neuralFlux::Bar;

BOOST_AUTO_TEST_CASE(SlackTest) {
    BarPtr bar = Bar::newSlack(1, 0);
    BOOST_TEST(bar->getP() == 0, tt::tolerance(0.001));
    BOOST_TEST(bar->getQ() == 0, tt::tolerance(0.001));
    BOOST_TEST(bar->getV() == 1, tt::tolerance(0.001));
    BOOST_TEST(bar->getTeta() == 0, tt::tolerance(0.001));
    BOOST_TEST(bar->getBsh() == 0, tt::tolerance(0.001));
};

BOOST_AUTO_TEST_CASE(PQTest) {
    BarPtr bar = Bar::newPQ(1, 3);
    BOOST_TEST(bar->getP() == 1, tt::tolerance(0.001));
    BOOST_TEST(bar->getQ() == 3, tt::tolerance(0.001));
    BOOST_TEST(bar->getV() == 1, tt::tolerance(0.001));
    BOOST_TEST(bar->getBsh() == 0, tt::tolerance(0.001));
};

BOOST_AUTO_TEST_CASE(PVTest) {
    BarPtr bar = Bar::newPV(3, 1);
    BOOST_TEST(bar->getP() == 3, tt::tolerance(0.001));
    BOOST_TEST(bar->getQ() == 0, tt::tolerance(0.001));
    BOOST_TEST(bar->getV() == 1, tt::tolerance(0.001));
    BOOST_TEST(bar->getBsh() == 0, tt::tolerance(0.001));
};

BOOST_AUTO_TEST_CASE(setE) {
    BarPtr bar = Bar::newPQ(1, 1);

    bar->setE(std::complex<double>(1, 1));
    BOOST_TEST(bar->getTeta() == M_PI_4, tt::tolerance(0.001));
    BOOST_TEST(bar->getV() == sqrt(2), tt::tolerance(0.001));

    bar->setE(std::complex<double>(1, -1));
    BOOST_TEST(bar->getTeta() == -M_PI_4, tt::tolerance(0.001));
    BOOST_TEST(bar->getV() == sqrt(2), tt::tolerance(0.001));

    bar->setE(std::complex<double>(-1, 1));
    BOOST_TEST(bar->getV() == sqrt(2), tt::tolerance(0.001));
    BOOST_TEST(bar->getTeta() == 3*M_PI_4, tt::tolerance(0.001));

    bar->setE(std::complex<double>(-1, -1));
    BOOST_TEST(bar->getV() == sqrt(2), tt::tolerance(0.001));
    BOOST_TEST(bar->getTeta() == -3*M_PI_4, tt::tolerance(0.001));

    bar->setE(std::complex<double>(1./2., std::sqrt(3.)/2.));
    BOOST_TEST(bar->getV() == 1., tt::tolerance(0.001));
    BOOST_TEST(bar->getTeta() == M_PI/3., tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(setTeta) {
    BarPtr bar = Bar::newPQ(1, 1);
    std::complex<double> e;

    bar->setTeta(M_PI_2);
    e = bar->getE();
    BOOST_TEST(e.real() == 0., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == 1., tt::tolerance(0.001));

    bar->setTeta(-M_PI_2);
    e = bar->getE();
    BOOST_TEST(e.real() == 0., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == -1., tt::tolerance(0.001));

    bar->setTeta(M_PI_4);
    e = bar->getE();
    BOOST_TEST(e.real() == std::sqrt(2.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == std::sqrt(2.)/2., tt::tolerance(0.001));

    bar->setTeta(-M_PI_4);
    e = bar->getE();
    BOOST_TEST(e.real() == std::sqrt(2.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == -std::sqrt(2.)/2., tt::tolerance(0.001));

    bar->setTeta(3*M_PI_4);
    e = bar->getE();
    BOOST_TEST(e.real() == -std::sqrt(2.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() ==  std::sqrt(2.)/2., tt::tolerance(0.001));

    bar->setTeta(-3*M_PI_4);
    e = bar->getE();
    BOOST_TEST(e.real() == -std::sqrt(2.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == -std::sqrt(2.)/2., tt::tolerance(0.001));

    bar->setTeta(5*M_PI_4);
    e = bar->getE();
    BOOST_TEST(e.real() == -std::sqrt(2.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == -std::sqrt(2.)/2., tt::tolerance(0.001));

    bar->setTeta(M_PI/6.);
    e = bar->getE();
    BOOST_TEST(e.real() == std::sqrt(3.)/2., tt::tolerance(0.001));
    BOOST_TEST(e.imag() == 1/2., tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(setV) {
    BarPtr bar = Bar::newPQ(1, 1);
    std::complex<double> e;

    bar->setTeta(M_PI_4);
    bar->setV(2);
    e = bar->getE();
    BOOST_TEST(e.real() == std::sqrt(2.), tt::tolerance(0.001));
    BOOST_TEST(e.imag() == std::sqrt(2.), tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(setVZero) {
    BarPtr bar = Bar::newPQ(1, 1);
    std::complex<double> e;

    bar->setTeta(M_PI_4);
    bar->setV(0);
    e = bar->getE();

    BOOST_TEST(e.real()== 0, tt::tolerance(0.001));
    BOOST_TEST(e.imag()== 0, tt::tolerance(0.001));
    BOOST_TEST(bar->getTeta() == 0, tt::tolerance(0.001));
}


//  ================================================================
//  Created by Gregory Kramida on 10/11/18.
//  Copyright (c) 2018 Gregory Kramida
//  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.
//  ================================================================
#pragma once
//stdlib

//libraries
#include <Eigen/Eigen>
#include <unsupported/Eigen/CXX11/Tensor>
#include <boost/python.hpp>

//local
#include "../math/typedefs.hpp"

namespace bp = boost::python;
namespace eig = Eigen;

namespace nonrigid_optimization {

template<typename ElementType, typename Scalar>
Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
warp(const Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& scalar_field,
	 const Eigen::Matrix<math::Vector2<Scalar>, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& warp_field);

template<typename ElementType, typename Scalar>
eig::Tensor<ElementType, 3, Eigen::ColMajor>
warp(const eig::Tensor<ElementType, 3, Eigen::ColMajor>& scalar_field,
	 const eig::Tensor<math::Vector3<Scalar>, 3, Eigen::ColMajor>& warp_field);

template<typename ElementType, typename Scalar>
Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
warp_with_replacement(const Eigen::Matrix<ElementType, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& scalar_field,
				      const Eigen::Matrix<math::Vector2<Scalar>, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& warp_field,
				      ElementType replacement_value);

template<typename ElementType, typename Scalar>
eig::Tensor<ElementType, 3, Eigen::ColMajor>
warp_with_replacement(const eig::Tensor<ElementType, 3, Eigen::ColMajor>& scalar_field,
					  const eig::Tensor<math::Vector3<Scalar>, 3, Eigen::ColMajor>& warp_field,
					  ElementType replacement_value);

//================= legacy (Sobolev/KillingFusion only) ========================
eig::MatrixXf warp_2d_advanced(math::MatrixXv2f& warp_field,
		const eig::MatrixXf& warped_live_field, const eig::MatrixXf& canonical_field,
		bool band_union_only = false, bool known_values_only = false,
		bool substitute_original = false, float truncation_float_threshold = 1e-6);
eig::MatrixXf warp_2d_advanced_warp_unchanged(math::MatrixXv2f& warp_field,
		const eig::MatrixXf& warped_live_field, const eig::MatrixXf& canonical_field,
		bool band_union_only = false, bool known_values_only = false,
		bool substitute_original = false, float truncation_float_threshold = 1e-6);

bp::object py_warp_field_advanced(const eig::MatrixXf& warped_live_field,
		const eig::MatrixXf& canonical_field, eig::MatrixXf warp_field_u,
		eig::MatrixXf warp_field_v, bool band_union_only = false, bool known_values_only = false,
		bool substitute_original = false, float truncation_float_threshold = 1e-6);

bp::object py_warp_field_advanced_no_warp_change(const eig::MatrixXf& warped_live_field,
		const eig::MatrixXf& canonical_field, eig::MatrixXf warp_field_u,
		eig::MatrixXf warp_field_v, bool band_union_only = false, bool known_values_only = false,
		bool substitute_original = false, float truncation_float_threshold = 1e-6);

} // namespace nonrigid_optimization


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/prepend.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::prepend(hana::tuple_c<long>, hana::long_c<0>),
        hana::tuple_c<long, 0>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::prepend(hana::tuple_c<unsigned int, 1>, hana::uint_c<0>),
        hana::tuple_c<unsigned int, 0, 1>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::prepend(hana::tuple_c<long long, 1, 2>, hana::llong_c<0>),
        hana::tuple_c<long long, 0, 1, 2>
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::prepend(hana::tuple_c<unsigned long, 1, 2, 3>, hana::ulong_c<0>),
        hana::tuple_c<unsigned long, 0, 1, 2, 3>
    ));
}


//
// Created by Sveta Morkva on 10/3/20.
//

#include "ObjectDetector.h"
#include <opencv2/core.hpp>
#include <opencv2/ml.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <boost/filesystem.hpp>
#include "opencv2/viz.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/cvstd.hpp>

using namespace cv;
using namespace cv::ml;

namespace fs = boost::filesystem;

ObjectDetector::ObjectDetector(bool brisk) : mBriskDescr(brisk) {}

void ObjectDetector::train(std::vector<Mat> images, std::vector<int> labels, Ptr<ml::SVM> &model) {
    Mat samples = getDescriptors(images);
    std::string filename = "../trained_model/trained_";

//    if(fs::exists(filename)){
//        std::cout << "Found trained model: " << filename << ", loading it." << std::endl;
//        svm = SVM::load(filename);
//        return;
//    }
    model = SVM::create();
    model->setType(ml::SVM::C_SVC);
    model->setKernel(ml::SVM::LINEAR);
    model->setTermCriteria(TermCriteria(TermCriteria::MAX_ITER, 100, 1e-6));
    model->train(samples, ROW_SAMPLE, labels);
    std::cout << "Trained "<< (mBriskDescr ? "brisk" : "surf") << " SVM model." <<  std::endl;
    filename += mBriskDescr ? "svm_brisk.xml" : "svm_surf.xml";
    model->save(filename);
}

void ObjectDetector::train(std::vector<cv::Mat> images, std::vector<int> labels, cv::Ptr<cv::ml::DTrees> &model) {
    Mat samples = getDescriptors(images);
    std::string filename = "../trained_model/trained_";

//    if(fs::exists(filename)){
//        std::cout << "Found trained model: " << filename << ", loading it." << std::endl;
//        svm = SVM::load(filename);
//        return;
//    }
    model = DTrees::create();
    model->setCVFolds(1);
    model->setMaxDepth(15);
    model->train(samples, ROW_SAMPLE, labels);
    std::cout << "Trained "<< (mBriskDescr ? "brisk" : "surf") << " DTREES model." <<  std::endl;
    filename += mBriskDescr ? "dtrees_brisk.xml" : "drees_surf.xml";
    model->save(filename);
}

void ObjectDetector::predict(std::vector<Mat> images, std::vector<int> labels, cv::ml::StatModel *model) {
    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    Mat samples = getDescriptors(images);

    Mat predict_labels;
    model->predict(samples, predict_labels);
    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
    auto time = std::chrono::duration_cast<std::chrono::microseconds>(end - begin);
    printf("Time for detection and prediction: %f\n", time.count() / 1000000.0);
    int truePositivesS = 0, falsePositivesS = 0, truePositivesM = 0, falsePositivesM = 0,
            trueNegatives = 0, falseNegatives = 0, allS = 0, allM = 0;
    for (int i = 0; i < predict_labels.rows; i++) {
        if (labels[i] == 1) {
            predict_labels.at<float>(i, 0) == labels[i] ? truePositivesS++ : falsePositivesS++;
            allS++;
        } else if (labels[i] == 2) {
            predict_labels.at<float>(i, 0) == labels[i] ? truePositivesM++ : falsePositivesM++;
            allM++;
        } else {
            predict_labels.at<float>(i, 0) == labels[i] ? trueNegatives++ : falseNegatives++;
        }
    }
    printf("False positive sneaker: %d\nFalse positive money: %d\nFalse negative: %d\n", falsePositivesS, falsePositivesM,
           falseNegatives);
    double precisonS = truePositivesS * 1.0/(truePositivesS+falsePositivesS);
    double precisonM = truePositivesM * 1.0/(truePositivesM+falsePositivesM);
    double recallS = truePositivesS * 1.0/allS;
    double recallM = truePositivesM * 1.0/allM;
    double f1S = 2*precisonS*recallS/(precisonS+recallS);
    double f1M = 2*precisonM*recallM/(precisonM+recallM);
    printf("Precision sneaker: %f\nRecall sneaker: %f\nF1-score sneaker: %f\n", precisonS, recallS, f1S);
    printf("Precision money: %f\nRecall money: %f\nF1-score money: %f\n", precisonM, recallM, f1M);
}

cv::Mat ObjectDetector::getDescriptors(std::vector<Mat> images) {

    Ptr<DescriptorExtractor> extractor;
    if (mBriskDescr) {
        extractor = BRISK::create();
    } else {
        extractor = xfeatures2d::SURF::create();
    }

    Mat outSamples = Mat::zeros(images.size(), extractor->descriptorSize() * num_descr, CV_32FC1);
    for (int i = 0; i < images.size(); i++) {
        std::vector<KeyPoint> keypoints;
        cv::Mat descriptors;
        std::vector<int> clustNames;
        std::map<int, std::vector<Point2f>> clusters;
        auto img = images[i];
        extractor->detect(img, keypoints);
        extractor->compute(img, keypoints, descriptors);
        if (keypoints.size() < num_clusters) {
            continue;
        }
        cv::Mat points(keypoints.size(), descriptors.cols + 2, CV_32FC1);
        for (int z = 0; z < keypoints.size(); z++) {
            auto p = keypoints[z].pt;
            for (int j = 0; j < descriptors.cols; j++) {
                points.at<float>(z, j) = mBriskDescr ? descriptors.at<uchar>(z, j) : descriptors.at<float>(z, j);
            }
            points.at<float>(z, descriptors.cols) = p.x;
            points.at<float>(z, descriptors.cols + 1) = p.y;
        }
        kmeans(points, num_clusters, clustNames,
               TermCriteria(TermCriteria::MAX_ITER, 10, 1.0), 3,
               KMEANS_PP_CENTERS);
        Mat img_keypoints;
        for (int j = 0; j < points.rows; j++) {
            Point2f p = {points.at<float>(j, descriptors.cols),
                         points.at<float>(j, descriptors.cols + 1)};
            clusters[clustNames[j]].push_back(p);
        }

        int biggestClusterNum = 0;
        int biggestClusterName;
        for (auto cl: clusters) {
            if (biggestClusterNum < cl.second.size()*0.95) {
                biggestClusterName = cl.first;
                biggestClusterNum = cl.second.size();
            } else {
                auto r1 = boundingRect(cl.second);
                auto r2 = boundingRect(clusters[biggestClusterName]);
                Point center_of_rect1 = (r1.br() + r1.tl())*0.5;
                Point center_of_rect2 = (r2.br() + r2.tl())*0.5;
                Point center_of_img = {img.cols/2, img.rows/2};
                double dist1 = cv::norm(center_of_img-center_of_rect1);
                double dist2 = cv::norm(center_of_img-center_of_rect2);
                if (dist1 < dist2) {
                    biggestClusterName = cl.first;
                    biggestClusterNum = cl.second.size();
                }
            }

        }
        int num = 0;
        for (int z = 0; z < descriptors.rows; z++) {
            if (clustNames[z] == biggestClusterName) {
                for (int j = 0; j < descriptors.cols; j++) {
                    outSamples.at<float>(i, num*descriptors.cols+j) = points.at<float>(z, j);
                }
                if (++num == num_descr) {
                    break;
                }
            }
        }
        curImgBoundRect = boundingRect(clusters[biggestClusterName]);
//        rectangle(img, r, Scalar(255, 0, 0));
//        imshow("SURF Keypoints", img);
//        waitKey();
    }
    return outSamples;
}

void ObjectDetector::predictVideo(const std::string &filename, cv::VideoCapture video, cv::ml::StatModel *model) {
    if (!video.isOpened()) {
        std::cout << "Error opening video stream or file" << std::endl;
        return;
    }
    std::cout << "Processing video: " << filename << std::endl;
    std::vector<cv::Mat> resultVideo;
    while (true) {
        Mat frame;
        video >> frame;
        if (frame.empty()) {
            break;
        }

        cvtColor(frame, frame, COLOR_BGR2GRAY);
        auto img = getDescriptors({frame});
        float label = model->predict(img);

        cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);

        Scalar color;
        std::string overlay;
        if (label == 0) {
            color = cv::viz::Color::red();
            overlay = "None objects were found.";
        }else if (label == 1) {
            color = cv::viz::Color::green();
            overlay = "Sneaker object was found.";
        }else {
            color = cv::viz::Color::green();
            overlay = "Money object was found.";
        }
        if (label != 0) {
            rectangle(frame, curImgBoundRect, cv::viz::Color::green(), 2);
        }

        auto point = Point(frame.cols / 2, frame.rows - frame.rows /4);
        cv::putText(frame, overlay, point, FONT_HERSHEY_PLAIN, 1.0, cv::viz::Color::white(), 2.0);
        resultVideo.push_back(frame);
    }

    VideoWriter writer = cv::VideoWriter();
    auto size = cv::Size(static_cast<int>(video.get(cv::CAP_PROP_FRAME_WIDTH)),
                         static_cast<int>(video.get(cv::CAP_PROP_FRAME_HEIGHT)));

    //avi format
    if (!writer.open(filename, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 10, size)) {
        std::cout << "Error: cant open file to write" << std::endl;
        return;
    }

    for (const auto &frame: resultVideo) {
        writer.write(frame);
    }
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#include <algorithm>
#include <atomic>
#include <cmath>
#include <limits>
#include <random>
#include <vector>

#include <boost/gil/gil_all.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <core/data/image/image_util_impl.hpp>

#include <model_server/lib/image_util.hpp>
#include <toolkits/object_detection/object_detector.hpp>
#include <toolkits/object_detection/one_shot_object_detection/one_shot_object_detector.hpp>
#include <toolkits/object_detection/one_shot_object_detection/util/color_convert.hpp>
#include <toolkits/object_detection/one_shot_object_detection/util/mapping_function.hpp>
#include <toolkits/object_detection/one_shot_object_detection/util/parameter_sampler.hpp>
#include <toolkits/object_detection/one_shot_object_detection/util/superposition.hpp>

namespace turi {
namespace one_shot_object_detection {
namespace data_augmentation {

flex_dict build_annotation(ParameterSampler &parameter_sampler,
                           const std::string &label,
                           size_t object_width, size_t object_height,
                           size_t background_width, size_t background_height,
                           size_t seed, size_t row_number) {
  parameter_sampler.sample(background_width, background_height, seed, row_number);

  size_t original_top_left_x = 0;
  size_t original_top_left_y = 0;
  size_t original_top_right_x = object_width;
  size_t original_top_right_y = 0;
  size_t original_bottom_left_x = 0;
  size_t original_bottom_left_y = object_height;
  size_t original_bottom_right_x = object_width;
  size_t original_bottom_right_y = object_height;

  Eigen::Vector3f top_left_corner(3), top_right_corner(3);
  Eigen::Vector3f bottom_left_corner(3), bottom_right_corner(3);
  top_left_corner << original_top_left_x, original_top_left_y, 1;
  top_right_corner << original_top_right_x, original_top_right_y, 1;
  bottom_left_corner << original_bottom_left_x, original_bottom_left_y, 1;
  bottom_right_corner << original_bottom_right_x, original_bottom_right_y, 1;

  auto normalize = [](Eigen::Vector3f corner) {
    corner[0] /= corner[2];
    corner[1] /= corner[2];
    corner[2] = 1.0;
    return corner;
  };

  Eigen::Matrix<float, 3, 3> mat = parameter_sampler.get_transform();

  const std::vector<Eigen::Vector3f> warped_corners = {
      normalize(mat * top_left_corner), normalize(mat * top_right_corner),
      normalize(mat * bottom_left_corner),
      normalize(mat * bottom_right_corner)};
  parameter_sampler.set_warped_corners(warped_corners);

  float min_x = std::numeric_limits<float>::max();
  float max_x = std::numeric_limits<float>::min();
  float min_y = std::numeric_limits<float>::max();
  float max_y = std::numeric_limits<float>::min();
  for (const auto &corner : warped_corners) {
    min_x = std::min(min_x, corner[0]);
    max_x = std::max(max_x, corner[0]);
    min_y = std::min(min_y, corner[1]);
    max_y = std::max(max_y, corner[1]);
  }
  float center_x = (min_x + max_x) / 2;
  float center_y = (min_y + max_y) / 2;
  float bounding_box_width = max_x - min_x;
  float bounding_box_height = max_y - min_y;

  flex_dict coordinates = {std::make_pair("x", center_x),
                           std::make_pair("y", center_y),
                           std::make_pair("width", bounding_box_width),
                           std::make_pair("height", bounding_box_height)};
  flex_dict annotation = {std::make_pair("coordinates", coordinates),
                          std::make_pair("label", label)};
  return annotation;
}

static std::map<std::string, size_t> generate_column_index_map(
    const std::vector<std::string> &column_names) {
  std::map<std::string, size_t> index_map;
  for (size_t k = 0; k < column_names.size(); ++k) {
    index_map[column_names[k]] = k;
  }
  return index_map;
}

flex_image create_rgba_flex_image(const flex_image &object_input) {
  if (!(object_input.is_decoded())) {
    log_and_throw("Input object starter image is not decoded.");
  }
  flex_image rgba_flex_image =
      image_util::resize_image(object_input, object_input.m_width,
                               object_input.m_height, 4, true)
          .to<flex_image>();
  if (!(rgba_flex_image.is_decoded())) {
    log_and_throw("Resized object starter image is not decoded.");
  }
  if (rgba_flex_image.m_channels != 4) {
    log_and_throw("Object image is not resized to be 4.");
  }
  return rgba_flex_image;
}

std::pair<flex_image, flex_dict>
create_synthetic_image_from_background_and_starter(ParameterSampler &parameter_sampler,
                                                   const flex_image &starter,
                                                   const flex_image &background,
                                                   std::string &label,
                                                   size_t seed,
                                                   size_t row_number) {

  // construct annotation dictionary from parameters
  flex_dict annotation =
      build_annotation(parameter_sampler, label,
                       starter.m_width, starter.m_height,
                       background.m_width, background.m_height,
                       seed, row_number);

  if (background.get_image_data() == nullptr) {
    log_and_throw("Background image has null image data.");
  }
  if (!(starter.is_decoded())) {
    log_and_throw("Starter image is not decoded into raw format.");
  }
  if (!(background.is_decoded())) {
    log_and_throw("Background image is not decoded into raw format.");
  }

  flex_image rgba_flex_image = create_rgba_flex_image(starter);
  boost::gil::rgba8_image_t::const_view_t starter_image_view =
      interleaved_view(rgba_flex_image.m_width, rgba_flex_image.m_height,
                       reinterpret_cast<const boost::gil::rgba8_pixel_t *>(
                           rgba_flex_image.get_image_data()),
                       rgba_flex_image.m_channels *
                           rgba_flex_image.m_width  // row length in bytes
      );

  boost::gil::rgb8_image_t::const_view_t background_view = interleaved_view(
      background.m_width, background.m_height,
      reinterpret_cast<const boost::gil::rgb8_pixel_t *>(
          background.get_image_data()),
      background.m_channels * background.m_width // row length in bytes
  );
  flex_image synthetic_image = create_synthetic_image(
      starter_image_view, background_view, parameter_sampler);
  encode_image_inplace(synthetic_image);
  return std::make_pair(synthetic_image, annotation);
}

gl_sframe augment_data(const gl_sframe &data,
                       const std::string &image_column_name,
                       const std::string &target_column_name,
                       const gl_sarray &backgrounds, long long seed,
                       bool verbose) {
  size_t backgrounds_size = backgrounds.size();
  size_t total_augmented_rows = data.size() * backgrounds_size;
  table_printer table(
      {{"Images Augmented", 0}, {"Elapsed Time", 0}, {"Percent Complete", 0}});
  if (verbose) {
    logprogress_stream << "Augmenting input images using " << backgrounds_size
                       << " background images." << std::endl;
    table.print_header();
  }
  std::vector<std::string> output_column_names = {image_column_name,
                                                  "annotation"};
  std::vector<flex_type_enum> output_column_types = {flex_type_enum::IMAGE,
                                                     flex_type_enum::DICT};
  gl_sframe_writer output_writer(output_column_names, output_column_types);
  auto column_index_map = generate_column_index_map(data.column_names());
  size_t image_column_index = column_index_map[image_column_name];
  size_t target_column_index = column_index_map[target_column_name];
  if (data[image_column_name].dtype() != flex_type_enum::IMAGE) {
    log_and_throw("Image column name is not of type Image.");
  }
  if (data[target_column_name].dtype() != flex_type_enum::STRING) {
    log_and_throw("Target column name is not of type String.");
  }
  size_t nsegments = output_writer.num_segments();
  std::atomic<size_t> augmented_counter(0);
  gl_sframe decompressed_data = gl_sframe(data);
  decompressed_data[image_column_name] =
      decompressed_data[image_column_name].apply(
          [&](flexible_type starter_ft) {
            flex_image starter =
                image_util::decode_image(starter_ft.to<flex_image>());
            return starter;
          },
          flex_type_enum::IMAGE);

  /* @TODO: Split all backgrounds into as many chunks as there are cores
   * available (= nsegments), and create augmented images in parallel.
   * Replacing the `for` with a `parallel_for` fails the export_coreml unit test
   * with an EXC_BAD_ACCESS in the function call to boost::gil::resample_pixels
   */
  for (const sframe_rows::row &row : decompressed_data.range_iterator()) {
    // go through all the starter images and create augmented images for
    // all starter images and the respective chunk of background images
    const flex_image &object = row[image_column_index].get<flex_image>();
    std::string label = row[target_column_index].to<flex_string>();
    ParameterSampler parameter_sampler = ParameterSampler(
      object.m_width, object.m_height, 0, 0);

    for (size_t segment_id = 0; segment_id < nsegments; segment_id++) {
      size_t segment_start = (segment_id * backgrounds.size()) / nsegments;
      size_t segment_end = ((segment_id + 1) * backgrounds.size()) / nsegments;
      size_t row_number = segment_start;
      for (const flexible_type &background_ft :
          backgrounds.range_iterator(segment_start, segment_end)) {
        row_number++;
        flex_image flex_background =
            image_util::decode_image(background_ft.to<flex_image>());

        std::pair<flex_image, flex_dict> synthetic_row =
            create_synthetic_image_from_background_and_starter(
                parameter_sampler, object, flex_background,
                label, seed, row_number);
        flex_image synthetic_image = synthetic_row.first;
        flex_dict annotation = synthetic_row.second;
        // write the synthetically generated image and the constructed
        // annotation to output SFrame.
        output_writer.write({std::move(synthetic_image), std::move(annotation)},
                            segment_id);
        size_t augmented_rows_completed = 1 + augmented_counter.fetch_add(1);
        if (verbose) {
          std::ostringstream d;
          // For pretty printing, floor percent done
          // resolution to the nearest .25% interval.  Do this by multiplying by
          // 400, then do integer division by the total size, then float divide
          // by 4.0
          if (augmented_rows_completed % 100 == 0) {
            d << augmented_rows_completed * 400 / total_augmented_rows / 4.0
              << '%';
            table.print_progress_row(augmented_rows_completed,
                                     augmented_rows_completed, progress_time(),
                                     d.str());
          }
        }
      }
    }
  }
  if (verbose) {
    table.print_footer();
  }

  gl_sframe synthetic_sframe = output_writer.close();
  return synthetic_sframe;
}

}  // namespace data_augmentation

one_shot_object_detector::one_shot_object_detector() {
  model_.reset(new turi::object_detection::object_detector());
}

gl_sframe one_shot_object_detector::augment(
    const gl_sframe &data, const std::string &image_column_name,
    const std::string &target_column_name, const gl_sarray &backgrounds,
    std::map<std::string, flexible_type> &options) {
  // TODO: Automatically infer the image column name, or throw error if you
  // can't This should just happen on the Python side.
  gl_sframe augmented_data = data_augmentation::augment_data(
      data, image_column_name, target_column_name, backgrounds, options["seed"],
      options["verbose"]);
  // TODO: Call object_detector::train from here.
  return augmented_data;
}

}  // namespace one_shot_object_detection
}  // namespace turi


#include <QApplication>
#include <QKeyEvent>
#include <QBuffer>
#include <QFileInfo>
#include "QImageWidget.h"
#include "QKinectGrabber.h"
#include "QKinectIO.h"
#include <iostream>
#include <iterator>
#include <array>

#include "Volumetric_helper.h"
#include "Timer.h"
#include <Eigen/Dense>
#include <cuda_runtime.h>
#include <vector_types.h>
#include "cuda_kernels/cuda_kernels.h"

#include "helper_cuda.h"
#include "helper_image.h"

StopWatchInterface *timer = NULL;
StopWatchInterface *kernel_timer = NULL;

template<typename PixelType>
void run_bilateral_filter(
	QImage& outputImage,
	const QImage& inputImage,
	float gaussian_delta,
	float euclidean_delta,
	int filter_radius,
	int iterations)
{
	updateGaussian(gaussian_delta, filter_radius);

	StopWatchInterface *kernel_timer = nullptr;

	PixelType* hImage = (PixelType*)(inputImage.bits());
	unsigned int width = inputImage.width();
	unsigned int height = inputImage.height();

	size_t pitch;

	PixelType* dInputImage = nullptr;
	// copy image data to array
	checkCudaErrors(cudaMallocPitch(&dInputImage, &pitch, sizeof(PixelType) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		dInputImage,
		pitch,
		hImage,
		sizeof(PixelType) * width,
		sizeof(PixelType) * width,
		height,
		cudaMemcpyHostToDevice));

	PixelType* dOutputImage;
	checkCudaErrors(cudaMallocPitch(
		&dOutputImage,
		&pitch,
		width * sizeof(PixelType),
		height));

	sdkCreateTimer(&kernel_timer);

	if (sizeof(PixelType) == 1)
		bilateralFilterGray((uchar*)dOutputImage, (uchar*)dInputImage, width, height, pitch, euclidean_delta, filter_radius, iterations, kernel_timer);
	else
		bilateralFilterRGBA((uint*)dOutputImage, width, height, euclidean_delta, filter_radius, iterations, kernel_timer);

	sdkDeleteTimer(&kernel_timer);

	cudaMemcpy2D(
		outputImage.bits(),
		sizeof(PixelType) * width,
		dOutputImage,
		pitch,
		sizeof(PixelType) * width,
		height,
		cudaMemcpyDeviceToHost);

	checkCudaErrors(cudaFree(dInputImage));
	checkCudaErrors(cudaFree(dOutputImage));
}

template<typename PixelType>
void run_bilateral_filter_depth_buffer(
	std::vector<PixelType>& output_buffer,
	const std::vector<PixelType>& input_buffer,
	uint width,
	uint height,
	ushort max_depth,
	float gaussian_delta,
	float euclidean_delta,
	int filter_radius,
	int iterations)
{
	updateGaussian(gaussian_delta, filter_radius);

	StopWatchInterface *kernel_timer = nullptr;

	PixelType* hImage = (PixelType*)input_buffer.data();

	size_t pitch;

	PixelType* dInputImage = nullptr;
	// copy image data to array
	checkCudaErrors(cudaMallocPitch(&dInputImage, &pitch, sizeof(PixelType) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		dInputImage,
		pitch,
		hImage,
		sizeof(PixelType) * width,
		sizeof(PixelType) * width,
		height,
		cudaMemcpyHostToDevice));

	PixelType* dOutputImage;
	checkCudaErrors(cudaMallocPitch(
		&dOutputImage,
		&pitch,
		width * sizeof(PixelType),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	bilateralFilter_ushort((ushort*)dOutputImage, (ushort*)dInputImage, width, height, pitch, max_depth, euclidean_delta, filter_radius, iterations, kernel_timer);

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	output_buffer.resize(input_buffer.size());

	cudaMemcpy2D(
		output_buffer.data(),
		sizeof(PixelType) * width,
		dOutputImage,
		pitch,
		sizeof(PixelType) * width,
		height,
		cudaMemcpyDeviceToHost);

	checkCudaErrors(cudaFree(dInputImage));
	checkCudaErrors(cudaFree(dOutputImage));
}

void convertKinectFrame2QImage(const KinectFrame& frame, QImage& depthImage)
{
	QVector<QRgb> colorTable;
	for (int i = 0; i < 256; ++i)
		colorTable.push_back(qRgb(i, i, i));
	depthImage = QImage(frame.depth_width(), frame.depth_height(), QImage::Format::Format_Indexed8);
	depthImage.setColorTable(colorTable);

	// set pixels to depth image
	for (int y = 0; y < depthImage.height(); y++)
	{
		for (int x = 0; x < depthImage.width(); x++)
		{
			const unsigned short depth = frame.depth[y * frame.depth_width() + x];
			depthImage.scanLine(y)[x] = static_cast<uchar>((float)depth / (float)frame.depth_max_distance() * 255.f);;
		}
	}
}

void convertDepthBuffer2QImage(const std::vector<ushort>& depth_buffer, uint width, uint height, ushort depth_max_distance, QImage& depthImage)
{
	assert(depth_buffer.size() == width * height);

	QVector<QRgb> colorTable;
	for (int i = 0; i < 256; ++i)
		colorTable.push_back(qRgb(i, i, i));
	depthImage = QImage(width, height, QImage::Format::Format_Indexed8);
	depthImage.setColorTable(colorTable);

	// set pixels to depth image
	for (int y = 0; y < depthImage.height(); y++)
	{
		for (int x = 0; x < depthImage.width(); x++)
		{
			const unsigned short depth = depth_buffer[y * width + x];
			depthImage.scanLine(y)[x] = static_cast<uchar>((float)depth / (float)depth_max_distance * 255.f);;
		}
	}
}

int main(int argc, char **argv)
{
	int iterations = 10;
	float gaussian_delta = 4.0f;
	float euclidean_delta = 0.1f;
	int filter_radius = 5;
	std::string filename = "../../data/room.knt";

	if (argc < 6)
	{
		std::cerr << "Usage: BilateralFilter_gpu.exe ../../data/room.knt number_of_iterations gaussian_delta euclidean_delta filter_radius" << std::endl;
		std::cerr << "Usage: BilateralFilter_gpu.exe ../../data/room.knt 10 4.0 0.1 5"
		<< std::endl
		<< "The app will continue with default parameters."
		<< std::endl;
	}
	else
	{
		iterations = atoi(argv[2]);
		gaussian_delta = atof(argv[3]);
		euclidean_delta = atof(argv[4]);
		filter_radius = atoi(argv[5]);
		filename = argv[1];
	}

	QApplication app(argc, argv);
	app.setApplicationName("Bilateral Filter");

	Timer timer;

#if 0
	QImage inputImage, outputImage;
	if (!inputImage.load(filename.c_str()))
	{
		std::cout << "Error: Could not load file image: " << filename << std::endl;
		return EXIT_FAILURE;
	}

	outputImage = inputImage;

	run_bilateral_filter<unsigned char>(outputImage, inputImage, gaussian_delta, euclidean_delta, filter_radius, iterations);

	QImageWidget inputWidget;
	inputWidget.setImage(inputImage);
	inputWidget.move(0, 0);
	inputWidget.setWindowTitle("Input");
	inputWidget.show();

	QImageWidget outputWidget;
	outputWidget.setImage(outputImage);
	outputWidget.move(inputWidget.width(), 0);
	outputWidget.setWindowTitle("Output");
	outputWidget.show();
#else

	timer.start();
	KinectFrame frame;
	QKinectIO::loadFrame(QString::fromStdString(filename), frame);

	/////////////////////////////////////
	//
	// create depth image
	QVector<QRgb>		colorTable;
	QImage				depthImage;
	QImage				depthImageFiltered;

	Timer t;
	t.start();
	convertKinectFrame2QImage(frame, depthImage);
	t.print_interval("Converting original depth buffer to image : ");

	QImageWidget inputWidget;
	inputWidget.setImage(depthImage);
	inputWidget.move(0, 0);
	inputWidget.setWindowTitle("Original Depth");
	inputWidget.show();

	t.start();
	std::vector<ushort> depth_buffer_filtered;
	run_bilateral_filter_depth_buffer<ushort>(depth_buffer_filtered, frame.depth, frame.depth_width(), frame.depth_height(), frame.depth_max_distance(), gaussian_delta, euclidean_delta, filter_radius, iterations);
	t.print_interval("Running bilateral filter in GPU           : ");

	t.start();
	convertDepthBuffer2QImage(depth_buffer_filtered, frame.depth_width(), frame.depth_height(), frame.depth_max_distance(), depthImageFiltered);
	t.print_interval("Converting filtered depth buffer to image : ");

	t.start();
	frame.depth = depth_buffer_filtered;
	t.print_interval("Copying depth buffer filtered             : ");

	QString str = "_bilateral_filter_" + QString::number(iterations) + "_" + QString::number(gaussian_delta) + "_" + QString::number(euclidean_delta) + "_" + QString::number(filter_radius);

	QImageWidget outputWidget;
	outputWidget.setImage(depthImageFiltered);
	outputWidget.move(inputWidget.width(), 0);
	outputWidget.setWindowTitle(str);
	outputWidget.show();
#endif

	t.start();
	depthImageFiltered.save(QFileInfo(filename.c_str()).absolutePath() + '/' + QFileInfo(filename.c_str()).baseName() + str + ".png");
	t.print_interval("Saving image result                       : ");

	t.start();
	QString frame_filename = QFileInfo(filename.c_str()).absolutePath() + '/' + QFileInfo(filename.c_str()).baseName() + str + ".knt";
	QKinectIO::saveFrame(frame_filename, frame);
	t.print_interval("Saving frame result                       : ");

	return app.exec();
}


#include "planner/RLMPNet.h"
#include "planner/RLMPNetTree.h"
#include "system/car/ControlSpace.h"
#include "system/car/System.h"
#include <boost/format/format_fwd.hpp>
#include <cmath>
#include <cnpy/cnpy.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/control/planners/sst/SST.h>
#include <ompl/util/Console.h>
#include <torch/script.h>
#include <tuple>

namespace oc = ompl::control;
namespace ob = ompl::base;
using namespace IRLMPNet;

using StatesVec = std::vector<std::vector<double>>;

std::tuple<StatesVec, StatesVec> sampleStartGoal(unsigned int n) {
    auto car_system = std::make_shared<System::Car1OrderSystem>();
    std::vector<std::vector<double>> starts(n), goals(n);

    for (unsigned int i = 0; i < n; i++) {
        car_system->sampleValidState(starts[i]);
        car_system->sampleValidState(goals[i]);
    }

    return {starts, goals};
}

std::tuple<StatesVec, StatesVec> loadStartGoal(unsigned int n, std::string start_filename, std::string goal_filename) {
    std::ifstream start_file(start_filename);
    std::ifstream goal_file(goal_filename);
    bool file_status = start_file.good() and goal_file.good();
    start_file.close();
    goal_file.close();

    std::vector<std::vector<double>> starts, goals;
    bool regenerate_status = false;

    if (file_status) {
        auto start_data = cnpy::npy_load(start_filename);
        auto goal_data = cnpy::npy_load(goal_filename);
        auto car_system = std::make_shared<System::Car1OrderSystem>();
        if (start_data.shape[0] == n and start_data.shape[1] == 3 and goal_data.shape[0] == n and goal_data.shape[1] == 3) {
            auto pstarts = start_data.data<double>();
            auto pgoals = goal_data.data<double>();
            for (unsigned int i = 0; i < n; i++) {
                std::vector<double> start{pstarts[3 * i], pstarts[3 * i + 1], pstarts[3 * i + 2]};
                std::vector<double> goal{pgoals[3 * i], pgoals[3 * i + 1], pgoals[3 * i + 2]};
                if (car_system->isValid(start) and car_system->isValid(goal)) {
                    starts.emplace_back(start);
                    goals.emplace_back(goal);
                } else {
                    regenerate_status = true;
                    OMPL_WARN("Encounter invalid start or goal. Regenerate the dataset.");
                    break;
                }
            }
        } else {
            regenerate_status = true;
            OMPL_WARN("Dataset shape incorrect. Regenerate the dataset.");
        }
    }

    regenerate_status = !file_status or regenerate_status;

    if (regenerate_status) {
        std::tie(starts, goals) = sampleStartGoal(n);

        // flat starts and goals for save
        std::vector<double> flatten_starts, flatten_goals;
        for (auto start : starts) {
            flatten_starts.insert(flatten_starts.end(), start.begin(), start.end());
        }
        for (auto goal : goals) {
            flatten_goals.insert(flatten_goals.end(), goal.begin(), goal.end());
        }
        cnpy::npy_save(start_filename, flatten_starts.data(), {n, 3});
        cnpy::npy_save(goal_filename, flatten_goals.data(), {n, 3});
    }

    return {starts, goals};
}

enum PlannerType {
    SST = 0,
    RLMPNet,
    RLMPNetTree,
};

std::tuple<double, double> car1order_control(
    const std::vector<double> start_vec,
    const std::vector<double> goal_vec,
    const double goal_radius,
    const PlannerType planner_type,
    const double time_limit,
    const double cost_limit,
    const std::string output_filename,
    const unsigned int index) {
    double length = std::numeric_limits<double>::infinity(), time = std::numeric_limits<double>::infinity();
    auto car_system = std::make_shared<System::Car1OrderSystem>();
    auto simple_setup = std::make_shared<oc::SimpleSetup>(car_system->space_information);
    auto objective = std::make_shared<ob::PathLengthOptimizationObjective>(car_system->space_information);
    objective->setCostThreshold(ob::Cost(cost_limit));
    simple_setup->setOptimizationObjective(objective);

    ob::ScopedState<System::Car1OrderStateSpace> start(car_system->state_space);

    car_system->state_space->copyFromReals(start.get(), start_vec);

    ob::ScopedState<System::Car1OrderStateSpace> goal(car_system->state_space);
    car_system->state_space->copyFromReals(goal.get(), goal_vec);

    simple_setup->setStartAndGoalStates(start, goal, goal_radius);
    std::cout << "start: ";
    car_system->space_information->printState(start.get());
    std::cout << "goal:  ";
    car_system->space_information->printState(goal.get());

    ob::PlannerPtr planner;
    switch (planner_type) {
        case SST: {
            planner = std::make_shared<oc::SST>(car_system->space_information);
            break;
        }
        case RLMPNet: {
            auto planner_model = torch::jit::load("data/car1order/rl_result/test2/torchscript/mpc_planner.pth");
            auto transition_model = torch::jit::load("data/car1order/rl_result/test2/torchscript/transition_model.pth");
            auto encoder_model = torch::jit::load("data/car1order/rl_result/test2/torchscript/observation_encoder.pth");
            auto policy = std::make_shared<Policy>(car_system->space_information, planner_model, transition_model, encoder_model);
            auto mpnet_model = torch::jit::load("data/car1order/mpnet_result/default/torchscript/mpnet_script.pt");
            auto mpnet_sampler = std::make_shared<MPNetSampler>(car_system->state_space.get(), mpnet_model);
            planner = std::make_shared<oc::RLMPNet>(car_system->space_information, policy, mpnet_sampler);

            break;
        }
        default: {
            std::cout << "Invalid planner! " << std::endl;
            return {time, length};
        }
    }

    simple_setup->setPlanner(planner);

    simple_setup->setup();

    ob::PlannerStatus solved = simple_setup->solve(time_limit);

    if (solved == ob::PlannerStatus::EXACT_SOLUTION) {
        std::cout << "Found solution" << std::endl;
        length = simple_setup->getSolutionPath().length();
        time = simple_setup->getLastPlanComputationTime();

        auto path = simple_setup->getSolutionPath();
        auto states = path.getStates();
        auto controls = path.getControls();
        auto durations = path.getControlDurations();
        const auto n = path.getStateCount();
        const size_t state_size = 3;
        const size_t control_size = 2;
        const size_t row_size = state_size + control_size + 1;
        std::vector<double> flatten_traj(n * row_size);
        // first n-1 elements
        for (size_t i = 0; i < n - 1; i++) {
            auto pstate = states[i]->as<System::Car1OrderStateSpace::StateType>()->values;
            auto pcontrol = controls[i]->as<System::Car1OrderControlSpace::ControlType>()->values;
            for (size_t j = 0; j < state_size; j++) {
                flatten_traj[i * row_size + j] = pstate[j];
            }
            for (size_t j = 0; j < control_size; j++) {
                flatten_traj[i * row_size + state_size + j] = pcontrol[j];
            }
            flatten_traj[i * row_size + state_size + control_size] = durations[i];
        }
        // last state has no following controls
        size_t i = n - 1;
        auto pstate = states[i]->as<System::Car1OrderStateSpace::StateType>()->values;
        for (size_t j = 0; j < state_size; j++) {
            flatten_traj[i * row_size + j] = pstate[j];
        }
        for (size_t j = 0; j < control_size; j++) {
            flatten_traj[i * row_size + state_size + j] = 0;
        }
        flatten_traj[i * row_size + state_size + control_size] = 0;

        // save file
        auto mode = (index == 0) ? "w" : "a";
        std::string dataset_name = (boost::format("traj%d") % index).str();
        cnpy::npz_save(output_filename, dataset_name, flatten_traj.data(), {n, row_size}, mode);
    } else if (solved == ob::PlannerStatus::APPROXIMATE_SOLUTION) {
        std::cout << "Found approximate solution" << std::endl;
    } else {
        std::cout << "No solution found" << std::endl;
    }

    return {time, length};
}

int main(int argc, char **argv) {
    const unsigned int N = 500;
    auto [starts, goals] = loadStartGoal(N, "./data/car1order/test_traj/test_starts.npy", "./data/car1order/test_traj/test_goals.npy");
    auto output_filename = "data/car1order/test_traj/test_traj.npz";

    for (unsigned int i = 0; i < N; i++) {
        auto [time, length] = car1order_control(starts[i], goals[i], 0.5, SST, 60.0, 500.0, output_filename, i);
        if (!std::isinf(time)) {
            std::cout << "Find traj " << i << std::endl;
        }
    }
}


#include <calibn/caliboptim.hpp>
#include <calibn/helpers.hpp>
#include <iomanip>
#include <boost/thread.hpp>

void WorkerThread(CalibOptim*, const Indices&);

size_t CalibOptim::Optimise(const cv::TermCriteria& criterion, size_t threads)
{
	m_termCriteria = criterion;
	m_threads = threads;
	m_state.errors = std::vector<double>(m_termCriteria.maxCount + 1);

	m_state.x = MakeIntialGuess(); // get the initial estimate
	m_state.y = Evaluate(m_state.x); // and use it to evaluate the objective function

	assert(m_state.x.rows > 0 && m_state.x.cols == 1 && m_state.y.rows > 0 && m_state.y.cols == 1);

	m_state.errors[0] = rmse(m_state.y);

	// data dimensions
	m_state.d = m_state.x.rows; // number of parameters
	m_state.n = m_state.y.rows; // number of constraints

	// mask to accelerate function evaluation
	m_state.evalMask = std::vector<Mask>(m_state.d);

	// convergence control
	m_state.lambda = 0; // doesn't matter here, the value will be estimated in the first iteration
	m_state.converged = false;
	m_state.i = 0;
	m_state.evals = 0;

	// initialise thread indices
	m_state.threadIdx = std::vector<Indices>(m_threads);
	for (size_t idx = 0; idx < m_state.d; idx++)
	{
		m_state.threadIdx[idx % m_threads].push_back(idx);
	}

	// start iterative optimisation
	using namespace std;

	cout << "Starting optimising " << m_state.d << " parameter(s) with " << m_state.n << " condition(s):" << endl;
	cout <<	left << setw(16) << "epsilon =" << "[" << right << setw(16) << setprecision(5) << scientific << m_termCriteria.epsilon << "]" << endl;
	cout << left << setw(16) << "#iterations =" << "[" << right << setw(16) << m_termCriteria.maxCount << "]" << endl;
	cout << left << setw(16) << "#threads =" << "[" << right << setw(16) << m_threads << "]" << endl;
	cout << left << setw(16) << "initial err =" << "["  << right << setw(16) << setprecision(5) << fixed << m_state.errors[0] << "]" << endl;
	cout << left;
	cout << setfill('-') << setw(80 - 1) << "" <<endl;
	cout << setfill(' ') << setw(8) << "# Itr" << setw(8) << "Evals" << setw(16) << "Error" << setw(16) << "Improvement (%)" << setw(16) << "Step Size (%)" << setw(16 - 1) << "Lambda" << endl;
	cout << setfill('-') << setw(80 - 1) << "" << endl;
	clock_t t0 = clock();
	m_state.tSolve	= m_state.tDiff = 0;
	while (Loop());
	m_state.tSum = clock() - t0;
	cout << setfill('-') << setw(80 - 1) << "" << endl;
	cout << "Optimisation finished, " << fixed << setprecision(2) << (1000.0f * m_state.tSum) / CLOCKS_PER_SEC << " milliseccond(s) elapsed." << endl;

	// finished
	Finalise(m_state);

	return m_state.i;
}

bool CalibOptim::Loop()
{
	if (m_state.converged)
	{
		return false;
	}

	try
	{
		// approximate Jocobian matrix by numerical differentiation
		m_state.J = cv::Mat::zeros((int)m_state.n, (int)m_state.d, CV_64F);

		clock_t t0 = clock();
		boost::thread* threads = new boost::thread[m_threads];
		/** launch differentiation thrads ******/ for (size_t j = 0; j < m_threads; j++) threads[j] = boost::thread(WorkerThread, this, m_state.threadIdx[j]);
		/** .. and wait for their completions **/ for (size_t j = 0; j < m_threads; j++) threads[j].join();
		delete[] threads;
		m_state.tDiff += (clock() - t0);

		cv::Mat D = m_state.J.t() * m_state.y; // error gradiant
		cv::Mat H = m_state.J.t() * m_state.J; // Hessian matrix

		if (m_state.lambda == 0) // set initial value of lambda
		{
			m_state.lambda = 1e-3 * cv::mean(H.diag())[0]; //lambda = 1e-3 * mean(diag(N));
		}

        bool better = false;
        cv::Mat x2, y2, x_delta;

        while (!better)
        {
		    // compute delta by solving the augmented normal equations
		    t0 = clock();
		    cv::Mat A = H + m_state.lambda * cv::Mat::diag(H.diag()); // or A = N + lambda*eye(d);
		    //Mat A = H + m_state.lambda * Mat::eye(H.rows, H.cols, H.type()); // or A = N + lambda*eye(d);
		    x_delta	=  A.inv() * -D; // x_delta =  A \ -D;
		    m_state.tSolve += (clock() - t0);

		    // estimate standard error
		    cv::Mat C = (0.5 * H).inv(); // C = (H/2)^-1
		    cv::sqrt(C.diag(), m_state.sigma); // sigma(i) = sqrt(C(i,i))

		    // test new parameters
		    x2 = m_state.x + x_delta;
		    y2 = Evaluate(x2); // evaluate the improvement
		    m_state.de	= rmse(y2) - m_state.errors[m_state.i];
		    m_state.H	= H;
		    better	    = m_state.de < 0;

		    t0 = clock();
		    m_state.dt = m_state.t - t0;
		    m_state.t = t0;

		    if (!better) // a worse result?
		    {
			    m_state.lambda *= eta;	// try a new lambda,
			    //return true; //  reject the update
		    }
		}

		m_state.lambda /= eta;
		m_state.x = x2; // accept the update
		m_state.y = y2;

		m_state.errors[++m_state.i] = m_state.errors[m_state.i-1] + m_state.de;

		// check convengence conditions
		m_state.derrRatio = std::abs(m_state.de / m_state.errors[m_state.i]);
		m_state.stepRatio = cv::norm(x_delta) / cv::norm(m_state.x);

		m_state.converged = false;
		m_state.converged |= (m_state.i >= m_termCriteria.maxCount); // # iterations check
		m_state.converged |= (m_state.i > 1) && (m_state.derrRatio < m_termCriteria.epsilon); // error differential check
		m_state.converged |= (m_state.i > 1) && (m_state.stepRatio < m_termCriteria.epsilon); // step ratio check

        using namespace std;

		cout << setfill(' ') << setw(8) << m_state.i;
		cout << setfill(' ') << setw(8) << m_state.evals;
		cout << setfill(' ') << setw(16) << setprecision(5) << scientific << m_state.errors[m_state.i];
		cout << setfill(' ') << setw(16) << setprecision(5) << scientific << m_state.derrRatio;
		cout << setfill(' ') << setw(16) << setprecision(5) << scientific << m_state.stepRatio;
		cout << setfill(' ') << setw(15) << setprecision(5) << scientific << m_state.lambda << endl;

		return !m_state.converged;

	}
	catch (std::exception& ex)
	{
		E_ERROR << "error iterating";
        E_ERROR << ex.what();
		m_state.i = (size_t) -1;

		return false;
	}

	return true;
}

cv::Mat CalibOptim::Differentiate(const cv::Mat& x, const cv::Mat& y, size_t j, const std::vector<bool>& mask) const
{
	clock_t t0 = clock();

	cv::Mat x2 = x.clone(); x2.at<double>((int)j,0) += dx;
	cv::Mat y2 = Evaluate(x2, m_state.evalMask[j]);
	cv::Mat dy = y2 - y;

	return dy;
}

void WorkerThread(CalibOptim* optim, const Indices& threadIdx)
{
	for (Indices::const_iterator idx = threadIdx.begin(); idx != threadIdx.end(); idx++)
	{
		// dereferencing
		size_t d = *idx;
		Mask& mask = optim->m_state.evalMask[d];

		// should we build the evaluation mask for
		//  skipping unrelated parameter-condition pairs?
		bool buildMask = mask.empty();

		if (buildMask) // initialise the mask
		{
			mask = Mask(optim->m_state.n, true);
		}

		// differentiation
		cv::Mat dy = optim->Differentiate(optim->m_state.x, optim->m_state.y, d, mask);
		//cout << threadIdx[0] << " DIFF " << d << endl;

		if (buildMask)
		{
			for (size_t k = 0; k < mask.size(); k++)
			{
				mask[k] = (dy.at<double>((int)k,0) != 0);
			}
		}
		else
		{
			for (size_t k = 0; k < mask.size(); k++)
			{
				if (!mask[k]) dy.at<double>((int)k,0) = 0;
			}
		}

		divide(dy, optim->dx, optim->m_state.J.col(d)); //J = dy / dx;
		optim->m_state.evals++; // TODO: protect this with a mutex lock
	}
}


#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
#include <boost/thread/mutex.hpp>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/GetPlanningScene.h>

using namespace std;

struct BoundingBox{
    double x_min;
    double x_max;
    double y_min;
    double y_max;
    double z_min;
    double z_max;
};

shape_msgs::SolidPrimitive shape_;
geometry_msgs::Pose pose_;
boost::mutex object_mutex;
BoundingBox findBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& pc);
const float bad_point = std::numeric_limits<float>::quiet_NaN();
ros::Publisher pc_publisher;
ros::Publisher bb_publisher;
ros::Publisher planning_scene_publisher;
struct BoundingBox myBoundingBox;
struct BoundingBox oldBoundinxBox;
int bbox_seq_number = 0;
double object_removal_padding_cm; // ROS PARAM
ros::ServiceClient client_get_scene_;

void resetOctomap();
void printBoundingBox(BoundingBox in_bb);
void publishBoundingBoxAsPointCloud(BoundingBox in_bb);
void addPaddingToBoundingBox(BoundingBox &in_bb);
bool areSameBoundingBoxes(BoundingBox bb1, BoundingBox bb2);

void object_callback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& pc){

    struct BoundingBox temp_BB = findBoundingBox(pc);

    object_mutex.lock();
    myBoundingBox = temp_BB;
    object_mutex.unlock();
}

void scene_callback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& scene_cloud){

    pcl::PointCloud<pcl::PointXYZRGB> pc = *scene_cloud;
    //cout << "Frame_id = " << pc.header.frame_id << endl;

    object_mutex.lock();
    struct BoundingBox BB = myBoundingBox;
    object_mutex.unlock();

    bool areTheSame = areSameBoundingBoxes(BB, oldBoundinxBox);
    oldBoundinxBox = BB;

    if(!areTheSame){
        resetOctomap();
    }

    addPaddingToBoundingBox(BB);

   // publishBoundingBoxAsPointCloud(BB);
   // printBoundingBox(BB);

    for(int i = 0; i < scene_cloud->points.size(); i++){

        double x = pc.points.at(i).x;
        double y = pc.points.at(i).y;
        double z = pc.points.at(i).z;

        if(x >= BB.x_min && x <= BB.x_max && y >= BB.y_min && y <= BB.y_max && z >= BB.z_min && z <= BB.z_max ){
            pc.points.at(i).x =  bad_point;
            pc.points.at(i).y =  bad_point;
            pc.points.at(i).z =  bad_point;
        }
    }

    pc_publisher.publish(pc);

}

// Find a bounding box around the object to grasp (pc)
// Outputs the shape and the pose of the bounding box
// Inspired from https://github.com/unboundedrobotics/ubr1_preview/blob/master/ubr1_grasping/src/shape_extraction.cpp
BoundingBox findBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& pc){

    struct BoundingBox BB;

    ros::NodeHandle nh;
    ros::Rate r(3);
    double x_min =  9999.0;
    double x_max = -9999.0;
    double y_min =  9999.0;
    double y_max = -9999.0;
    double z_min =  9999.0;
    double z_max = -9999.0;

    for(int i = 0; i < pc->size(); i++){
        double pc_x = pc->at(i).x;
        double pc_y = pc->at(i).y;
        double pc_z = pc->at(i).z;

        if(pc_x < x_min) x_min = pc_x;
        if(pc_y < y_min) y_min = pc_y;
        if(pc_z < z_min) z_min = pc_z;

        if(pc_x > x_max) x_max = pc_x;
        if(pc_y > y_max) y_max = pc_y;
        if(pc_z > z_max) z_max = pc_z;
    }

    BB.x_min = x_min;
    BB.x_max = x_max;
    BB.y_min = y_min;
    BB.y_max = y_max;
    BB.z_min = z_min;
    BB.z_max = z_max;

    return BB;

}

void printBoundingBox(BoundingBox in_bb){

    cout << "X : [" << in_bb.x_min << " -> " << in_bb.x_max << "]" << endl;
    cout << "Y : [" << in_bb.y_min << " -> " << in_bb.y_max << "]" << endl;
    cout << "Z : [" << in_bb.z_min << " -> " << in_bb.z_max << "]" << endl;

}

void publishBoundingBoxAsPointCloud(BoundingBox in_bb){
    pcl::PointCloud<pcl::PointXYZRGB> pc;
    pc.header.frame_id = "/camera_rgb_optical_frame";
    pc.header.seq = bbox_seq_number;
    //pc.header.stamp = ros::Time::now().toSec();
    bbox_seq_number++;

//    int NumberOfPointsPerAxis = 10;
//    double x_increment = (in_bb.x_max - in_bb.x_min) / NumberOfPointsPerAxis;
//    double y_increment = (in_bb.y_max - in_bb.y_min) / NumberOfPointsPerAxis;
//    double z_increment = (in_bb.z_max - in_bb.z_min) / NumberOfPointsPerAxis;

//    for(double x = in_bb.x_min; x < in_bb.x_max; x+=x_increment){
//        for(double y = in_bb.y_min; y < in_bb.y_max; y+=y_increment){
//            for(double z = in_bb.z_min; z < in_bb.z_max; z+=z_increment){
//                pcl::PointXYZ pt(x,y,z);
//                pc.push_back(pt);
//            }
//        }
//    }

    pcl::PointXYZRGB pt;
    pt.x = in_bb.x_min; pt.y = in_bb.y_min; pt.z = in_bb.z_min; pc.push_back(pt);
    pt.x = in_bb.x_min; pt.y = in_bb.y_min; pt.z = in_bb.z_max; pc.push_back(pt);
    pt.x = in_bb.x_min; pt.y = in_bb.y_max; pt.z = in_bb.z_min; pc.push_back(pt);
    pt.x = in_bb.x_min; pt.y = in_bb.y_max; pt.z = in_bb.z_max; pc.push_back(pt);

    pt.x = in_bb.x_max; pt.y = in_bb.y_min; pt.z = in_bb.z_min; pc.push_back(pt);
    pt.x = in_bb.x_max; pt.y = in_bb.y_min; pt.z = in_bb.z_max; pc.push_back(pt);
    pt.x = in_bb.x_max; pt.y = in_bb.y_max; pt.z = in_bb.z_min; pc.push_back(pt);
    pt.x = in_bb.x_max; pt.y = in_bb.y_max; pt.z = in_bb.z_max; pc.push_back(pt);

    cout << "bbox published as pointcloud of size " << pc.size() << endl;
    bb_publisher.publish(pc);

}

void addPaddingToBoundingBox(BoundingBox &in_bb){

    double padding_m = object_removal_padding_cm / 100;

    in_bb.x_min -= padding_m;
    in_bb.x_max += padding_m;
    in_bb.y_min -= padding_m;
    in_bb.y_max += padding_m;
    in_bb.z_min -= padding_m;
    in_bb.z_max += padding_m;
}

/* Publishes an empty scene to reset/refresh it */
void resetOctomap(){
    cout << "Clearing Octomap !" << endl;
    moveit_msgs::PlanningScene planning_scene;

    moveit_msgs::GetPlanningScene scene_srv;
    scene_srv.request.components.components =   scene_srv.request.components.ALLOWED_COLLISION_MATRIX +
                                                scene_srv.request.components.LINK_PADDING_AND_SCALING +
                                                scene_srv.request.components.OBJECT_COLORS +
                                                scene_srv.request.components.OCTOMAP +
                                                scene_srv.request.components.ROBOT_STATE +
                                                scene_srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS +
                                                scene_srv.request.components.SCENE_SETTINGS +
                                                scene_srv.request.components.TRANSFORMS +
                                                scene_srv.request.components.WORLD_OBJECT_GEOMETRY +
                                                scene_srv.request.components.WORLD_OBJECT_NAMES;

    if(client_get_scene_.call(scene_srv)){
         moveit_msgs::PlanningScene planning_scene = scene_srv.response.scene;
         planning_scene.world.octomap.octomap.data.clear();
         ros::WallDuration sleep_time(0.5);
         //planning_scene.is_diff = true;
         planning_scene_publisher.publish(planning_scene);
         sleep_time.sleep();
    }
}

bool areSameBoundingBoxes(BoundingBox bb1, BoundingBox bb2){

    if(bb1.x_min == bb2.x_min &&
            bb1.y_min == bb2.y_min &&
            bb1.z_min == bb2.z_min &&
            bb1.x_max == bb2.x_max &&
            bb1.y_max == bb2.y_max &&
            bb1.z_max == bb2.z_max
            )
    {
        return true;
    }

    else {
        return false;
    }

}

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

    ros::init(argc,argv,"point_cloud_object_remover");

    ros::NodeHandle nh;

    oldBoundinxBox.x_min = 0;
    oldBoundinxBox.x_max = 0;
    oldBoundinxBox.y_min = 0;
    oldBoundinxBox.y_max = 0;
    oldBoundinxBox.z_min = 0;
    oldBoundinxBox.z_max = 0;

    ros::Subscriber subA = nh.subscribe<pcl::PointCloud<pcl::PointXYZRGB> > ("/camera/depth_registered/points", 1, scene_callback);
    ros::Subscriber subB = nh.subscribe<pcl::PointCloud<pcl::PointXYZRGB> > ("/grasp_object", 1, object_callback);

    pc_publisher = nh.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("/point_cloud_minus_object", 1);
    bb_publisher = nh.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("/object_to_grasp_bounding_box_DEBUG", 1);
    planning_scene_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
    client_get_scene_ = nh.serviceClient<moveit_msgs::GetPlanningScene>("/get_planning_scene");

    nh.param<double>("object_removal_padding_cm", object_removal_padding_cm, 1.0);

//    myBoundingBox.x_min = 0;
//    myBoundingBox.x_max = 2;
//    myBoundingBox.y_min = 0;
//    myBoundingBox.y_max = 2;
//    myBoundingBox.z_min = 0;
//    myBoundingBox.z_max = 2;

    ros::Rate r(5);
    while(ros::ok()){
        ros::spinOnce();
        r.sleep();
    }

  //  ros::spin();

    return 0;
}


#include <boost/graph/distributed/fruchterman_reingold.hpp>