#include <iostream>
#include <core/circuit.hpp>
#include <core/functions/add_gates.hpp>
#include <boost/lexical_cast.hpp>
using namespace revkit;
void example (qint a, qint b){
    qint c;
    zero_to_garbage d;
    cnot(a,b);
    NOT(a);
    toffoli(a,b,b);
    a_swap_b(a,b,1);
//    a_eq_a_plus_b(a,b,1);
//    a_eq_a_minus_b(a,b,1);
//    a_eq_a_plus_b_times_c(a,b,c,1);
    assign_value_of_b_to_a(a,b,1);
}
// test2 = main_module
int main (int argc, char** argv){
    cnot(a,b);
    cnot(b,c);
    toffoli(a,b,c);
    example(a, b);
}


//
// Created by Aman LaChapelle on 12/22/17.
//
// BatchingRPCServer
// Copyright (c) 2017 Aman LaChapelle
// Full license at BatchingRPCServer/LICENSE.txt
//

/*
    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 <dlib/data_io.h>
#include <sstream>

#include "DlibServable.hpp"
#include "Servable.hpp"

#include "TBServer.hpp"

#include "gtest/gtest.h"

#include "BatchingRPC.grpc.pb.h"
#include "BatchingRPC.pb.h"

#include <future>

namespace Serving {
namespace {

using namespace dlib;

using net_type = loss_multiclass_log<fc<
    10,
    relu<fc<
        84,
        relu<fc<
            120,
            max_pool<2, 2, 2, 2,
                     relu<con<16, 5, 5, 1, 1,
                              max_pool<2, 2, 2, 2,
                                       relu<con<6, 5, 5, 1, 1,
                                                input<matrix<
                                                    unsigned char>>>>>>>>>>>>>>;

Serving::TensorMessage ToMessage(std::vector<matrix<unsigned char>> &&arr) {

  Serving::TensorMessage message;
  std::ostringstream buffer_stream(std::ios::binary);
  serialize(arr, buffer_stream);
  message.set_serialized_buffer(buffer_stream.str());
  message.set_n(arr.size());

  return message;
}

void TrainNetwork(const std::string &dirname) {
  std::vector<matrix<unsigned char>> training_images;
  std::vector<unsigned long> training_labels;
  std::vector<matrix<unsigned char>> testing_images;
  std::vector<unsigned long> testing_labels;
  load_mnist_dataset(dirname, training_images, training_labels, testing_images,
                     testing_labels);

  std::ifstream f(
      "../Servable/DlibServable/test/assets/mnist_network.dat");
  if (f.is_open()) {
    f.close();
    return;
  }

  net_type net;
  dnn_trainer<net_type> trainer(net);
  trainer.set_learning_rate(0.01);
  trainer.set_min_learning_rate(0.0001);
  trainer.set_mini_batch_size(128);
  trainer.be_verbose();
  trainer.set_synchronization_file("mnist_sync", std::chrono::seconds(20));
  trainer.train(training_images, training_labels);
  net.clean();
  serialize("../Servable/DlibServable/test/assets/mnist_network.dat")
      << net;
}

void ThreadProcess(std::unique_ptr<BatchingServer::Stub> &stub,
                   TensorMessage msg, TensorMessage &reply) {
  grpc::Status status;
  ConnectionReply rep;

  {
    grpc::ClientContext context;
    stub->Connect(&context, ConnectionRequest(), &rep);
  }

  msg.set_client_id(rep.client_id());

  {
    grpc::ClientContext context;
    stub->Process(&context, msg, &reply);
  }
}

class TestIntegrationDlib : public ::testing::Test {
protected:
  void SetUp() override {
    TrainNetwork("../Servable/DlibServable/test/assets");
    deserialize(
        "../Servable/DlibServable/test/assets/mnist_network.dat") >>
        raw_args.net;
    file_args.filename =
        "../Servable/DlibServable/test/assets/mnist_network.dat";

    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long> training_labels;
    load_mnist_dataset("../Servable/DlibServable/test/assets",
                       training_images, training_labels, input_, output_);

    Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long> *servable =
            new Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>(1);
    servable->Bind(raw_args);
    srv = new TBServer(servable);

    srv->StartInsecure("localhost:50051");

    msg = ToMessage({input_[0]});
  }

  Serving::TBServer *srv;
  Serving::DlibRawBindArgs<net_type> raw_args;
  Serving::DlibFileBindArgs file_args;

  std::vector<matrix<unsigned char>> input_;
  std::vector<unsigned long> output_;
  Serving::TensorMessage msg;
};

TEST_F(TestIntegrationDlib, Connect) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  grpc::ClientContext context;

  ConnectionReply rep;

  grpc::Status status = stub->Connect(&context, ConnectionRequest(), &rep);

  EXPECT_TRUE(status.ok());
  EXPECT_FALSE(rep.client_id().empty());
}

TEST_F(TestIntegrationDlib, SetBatchSize) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  grpc::ClientContext context;

  AdminRequest req;
  req.set_new_batch_size(5);
  AdminReply rep;

  grpc::Status status = stub->SetBatchSize(&context, req, &rep);

  EXPECT_TRUE(status.ok());
}

TEST_F(TestIntegrationDlib, Process) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  ConnectionReply rep;
  grpc::Status status;

  {
    grpc::ClientContext context;
    status = stub->Connect(&context, ConnectionRequest(), &rep);
    EXPECT_TRUE(status.ok());
    EXPECT_FALSE(rep.client_id().empty());
  }

  msg.set_client_id(rep.client_id());

  TensorMessage tensor_reply;

  {
    grpc::ClientContext context;
    status = stub->Process(&context, msg, &tensor_reply);
    EXPECT_TRUE(status.ok());

    std::istringstream output_buffer(tensor_reply.serialized_buffer(),
                                     std::ios::binary);

    std::vector<unsigned long> results;
    deserialize(results, output_buffer);
    // this particular image is a seven, since the net is trained we might as well
    // run a prediction
    EXPECT_EQ(results[0], 7);
  }
}

TEST_F(TestIntegrationDlib, ThreadedProcessSingle) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  TensorMessage tensor_reply;

  std::thread processing_thread(ThreadProcess, std::ref(stub), msg,
                                std::ref(tensor_reply));

  processing_thread.join();

  std::istringstream output_buffer(tensor_reply.serialized_buffer(),
                                   std::ios::binary);
  std::vector<unsigned long> results;
  deserialize(results, output_buffer);
  // this particular image is a seven, since the net is trained we might as well
  // run a prediction
  EXPECT_EQ(results[0], 7);
}

TEST_F(TestIntegrationDlib, ThreadedProcessMultiple_SingleBatch) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  int batch_size = 50;

  {
    grpc::ClientContext context;
    AdminRequest req;
    req.set_new_batch_size(batch_size);
    AdminReply rep;

    grpc::Status status = stub->SetBatchSize(&context, req, &rep);
    EXPECT_TRUE(status.ok());
  }

  std::vector<std::thread> request_threads;
  std::vector<TensorMessage> results(batch_size);

  for (int i = 0; i < batch_size; i++) {
    request_threads.emplace_back(
        std::thread(ThreadProcess, std::ref(stub), msg, std::ref(results[i])));
  }

  for (int i = 0; i < batch_size; i++) {
    request_threads[i].join();
    TensorMessage &tensor_reply = results[i];

    std::istringstream output_buffer(tensor_reply.serialized_buffer(),
                                     std::ios::binary);
    std::vector<unsigned long> results;
    deserialize(results, output_buffer);
    // this particular image is a seven, since the net is trained we might as well
    // run a prediction
    EXPECT_EQ(results[0], 7);
    tensor_reply.clear_buffer();
  }
}

TEST_F(TestIntegrationDlib, ThreadedProcessMultiple_MultiBatch) {
  std::shared_ptr<grpc::Channel> channel = grpc::CreateChannel(
      "localhost:50051", grpc::InsecureChannelCredentials());
  std::unique_ptr<BatchingServer::Stub> stub = BatchingServer::NewStub(channel);

  int batch_size = 50;

  {
    grpc::ClientContext context;
    AdminRequest req;
    req.set_new_batch_size(10);
    AdminReply rep;

    grpc::Status status = stub->SetBatchSize(&context, req, &rep);
    EXPECT_TRUE(status.ok());
  }

  std::vector<std::thread> request_threads;
  std::vector<TensorMessage> results(batch_size);

  for (int i = 0; i < batch_size; i++) {
    request_threads.emplace_back(
        std::thread(ThreadProcess, std::ref(stub), msg, std::ref(results[i])));
  }

  for (int i = 0; i < batch_size; i++) {
    request_threads[i].join();
    TensorMessage &tensor_reply = results[i];

    std::istringstream output_buffer(tensor_reply.serialized_buffer(),
                                     std::ios::binary);
    std::vector<unsigned long> results;
    deserialize(results, output_buffer);
    // this particular image is a seven, since the net is trained we might as well
    // run a prediction
    EXPECT_EQ(results[0], 7);
    tensor_reply.clear_buffer();
  }
}
}
} // namespace Serving::


// Copyright 2015-2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/function.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

template<class T, T N> using integral = std::integral_constant<T, N>;

int main()
{
    using boost::mp11::mp_plus;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<>, integral<int, 0>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<char, 1>>, integral<int, 1>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<short, 2>>, integral<int, 2>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<int, 3>>, integral<int, 3>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<unsigned, 4>>, integral<unsigned, 4>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<char, 1>, integral<char, 2>>, integral<int, 3>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<short, 1>, integral<short, 2>, integral<short, 3>>, integral<int, 6>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_plus<integral<int, 1>, integral<int, 2>, integral<int, 3>, integral<long, 4>>, integral<long, 10>>));

    return boost::report_errors();
}


//  Copyright (c) 2019 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// compile only test to test constexpr casting

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/native.hpp>
#include <boost/safe_numerics/exception_policies.hpp>

template <class T>
using safe_t = boost::safe_numerics::safe<
    T,
    boost::safe_numerics::native,
    boost::safe_numerics::trap_exception
>;

constexpr const char * test_casting_results[] = {
//      0       0       0       0
//      012345670123456701234567012345670
//      012345678901234567890123456789012
/* 0*/ ".....xxx.xxx.xxx.xxx.xxx.xxx.xxx.",
/* 1*/ ".........xxx.xxx...x.xxx.xxx.xxx.",
/* 2*/ ".............xxx...x...x.xxx.xxx.",
/* 3*/ "...................x...x...x.xxx.",
/* 4*/ "..xx.xxx.xxx.xxx..xx.xxx.xxx.xxx.",
/* 5*/ "..xx..xx.xxx.xxx......xx.xxx.xxx.",
/* 6*/ "..xx..xx..xx.xxx..........xx.xxx.",
/* 7*/ "..xx..xx..xx..xx..............xx.",
};

#include <boost/safe_numerics/safe_integer_literal.hpp>
using namespace boost::safe_numerics;

#include <boost/mp11/algorithm.hpp>

using namespace boost::mp11;

template<class T>
struct p {
    constexpr static bool value = '.' == test_casting_results[mp_first<T>::value][mp_second<T>::value];
};

template<class T2, class T1>
constexpr bool test_cast_constexpr(const T1 & v1){
    // if we don't expect the operation to pass, we can't
    // check the constexpr version of the calculation so
    // just return success.
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wunused-value"
    static_cast<safe_t<T2>>(v1);
    static_cast<T2>(v1);
    #pragma GCC diagnostic pop
    return true;
}

#include "test_values.hpp"

template<typename L2>
struct test {
    static_assert(mp_is_list<L2>(), "must be a list of two indices");
    const static std::size_t i = mp_first<L2>();
    const static std::size_t j = mp_second<L2>();
    using T = mp_at_c<test_types, i>; // first element is a type
    using T1 = typename mp_at_c<test_values, j>::value_type;
    const static T1 v = mp_at_c<test_values, j>::value;
    const static bool value =
        test_cast_constexpr<T>(make_safe_literal(v, native, trap_exception));
};

int main(){
    using namespace boost::safe_numerics;

    using type_indices = mp_iota_c<mp_size<test_types>::value>;
    using value_indices = mp_iota_c<mp_size<test_values>::value>;

    // generate all combinations of types <- value
    using l = mp_product<mp_list,type_indices,value_indices>;
    //boost::safe_numerics::utility::print_types<l> lp;

    // filter out the invalid ones
    using l1 = mp_copy_if<l, p>;
    //boost::safe_numerics::utility::print_types<l1> l1p;

    // verify that all valid ones compile without error
    static_assert(mp_all_of<l1, test>(), "testing all valid casts");
    return 0;
}


/**
   \file
   \author Shin'ichiro Nakaoka
*/

#include "RangeSensor.h"
#include <boost/make_shared.hpp>

using namespace std;
using namespace cnoid;

namespace {
const double PI = 3.14159265358979323846;
}

RangeSensor::RangeSensor()
{
    on_ = true;
    isRangeDataSetAsState_ = false;

    yawRange_ = PI / 2.0;
    yawResolution_ = 100;

    pitchRange_ = 0.0;
    pitchResolution_ = 1;

    minDistance_ = 0.01;
    maxDistance_ = 10.0;

    frameRate_ = 10.0;

    rangeData_ = boost::make_shared<RangeData>();
}

void RangeSensor::copyStateFrom(const RangeSensor& other)
{
    VisionSensor::copyStateFrom(other);

    on_ = other.on_;
    yawResolution_ = other.yawResolution_;
    pitchResolution_ = other.pitchResolution_;
    yawRange_ = other.yawRange_;
    pitchRange_ = other.pitchRange_;
    minDistance_ = other.minDistance_;
    maxDistance_ = other.maxDistance_;
    frameRate_ = other.frameRate_;

    if(other.isRangeDataSetAsState_){
        rangeData_ = other.rangeData_;
    } else {
        rangeData_ = boost::make_shared<RangeData>();
    }
}

void RangeSensor::copyStateFrom(const DeviceState& other)
{
    if(typeid(other) != typeid(RangeSensor)){
        throw std::invalid_argument("Type mismatch in the Device::copyStateFrom function");
    }
    copyStateFrom(static_cast<const RangeSensor&>(other));
}

RangeSensor::RangeSensor(const RangeSensor& org, bool copyAll)
    : VisionSensor(org, copyAll)
{
    isRangeDataSetAsState_ = org.isRangeDataSetAsState_;
    copyStateFrom(org);
}

DeviceState* RangeSensor::cloneState() const
{
    return new RangeSensor(*this, false);
}

Device* RangeSensor::clone() const
{
    return new RangeSensor(*this);
}

void RangeSensor::forEachActualType(boost::function<bool(const std::type_info& type)> func)
{
    if(!func(typeid(RangeSensor))){
        VisionSensor::forEachActualType(func);
    }
}

void RangeSensor::setYawRange(double angle)
{
    if(angle > 0.0){
        yawRange_ = angle;
    }
}

void RangeSensor::setYawResolution(int n)
{
    if(n >= 1 ){
        yawResolution_ = n;
    }
}

double RangeSensor::yawStep() const
{
    if(yawResolution_ >= 2){
        return yawRange_ / (yawResolution_ - 1);
    } else {
        return 0.0;
    }
}

void RangeSensor::setPitchRange(double angle)
{
    if(angle >= 0.0){
        pitchRange_ = angle;
    }
}

void RangeSensor::setPitchResolution(double n)
{
    if(n >= 1){
        pitchResolution_ = n;
    }
}

double RangeSensor::pitchStep() const
{
    if(pitchResolution_ >= 2){
        return pitchRange_ / (pitchResolution_ - 1);
    } else {
        return 0.0;
    }
}

void RangeSensor::setMaxDistance(double d)
{
    if(d > 0.0){
        if(d > minDistance_){
            maxDistance_ = d;
        } else {
            minDistance_ = d;
        }
    }
}

void RangeSensor::setMinDistance(double d)
{
    if(d > 0.0){
        if(d < maxDistance_){
            minDistance_ = d;
        } else {
            maxDistance_ = d;
        }
    }
}

void RangeSensor::setFrameRate(double r)
{
    if(r > 0.0){
        frameRate_ = r;
    }
}

RangeSensor::RangeData& RangeSensor::rangeData()
{
    if(rangeData_.use_count() > 1){
        rangeData_ = boost::make_shared<RangeData>(*rangeData_);
    }
    return *rangeData_;
}

RangeSensor::RangeData& RangeSensor::newRangeData()
{
    rangeData_ = boost::make_shared<RangeData>();
    return *rangeData_;
}

void RangeSensor::setRangeData(boost::shared_ptr<RangeData>& data)
{
    if(data.use_count() == 1){
        rangeData_ = data;
    } else {
        rangeData_ = boost::make_shared<RangeData>(*data);
    }
    data.reset();
}

void RangeSensor::clearState()
{
    if(rangeData_.use_count() == 1){
        rangeData_->clear();
    } else {
        rangeData_ = boost::make_shared<RangeData>();
    }
}

int RangeSensor::stateSize() const
{
    return 8;
}

const double* RangeSensor::readState(const double* buf)
{
    buf = VisionSensor::readState(buf);
    on_ = buf[0];
    yawRange_ = buf[1];
    yawResolution_ = buf[2];
    pitchRange_ = buf[3];
    pitchResolution_ = buf[4];
    minDistance_ = buf[5];
    maxDistance_ = buf[6];
    frameRate_ = buf[7];
    return buf + 8;
}

double* RangeSensor::writeState(double* out_buf) const
{
    out_buf = VisionSensor::writeState(out_buf);
    out_buf[0] = on_ ? 1.0 : 0.0;
    out_buf[1] = yawRange_;
    out_buf[2] = yawResolution_;
    out_buf[3] = pitchRange_;
    out_buf[4] = pitchResolution_;
    out_buf[5] = minDistance_;
    out_buf[6] = maxDistance_;
    out_buf[7] = frameRate_;
    return out_buf + 8;
}


// Copyright (C) 2004-2008 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/metis.hpp>
#include <fstream>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

using namespace boost;

/* An undirected graph with distance values stored on the vertices. */
typedef adjacency_list<vecS, vecS, undirectedS,
                       property<vertex_distance_t, std::size_t> >
  Graph;

int main(int argc, char* argv[])
{
  // Parse command-line options
  const char* filename = "weighted_graph.gr";
  if (argc > 1) filename = argv[1];

  // Open the METIS input file
  std::ifstream in(filename);
  assert (in.good());
  graph::metis_reader reader(in);

  // Load the graph using the default distribution
  Graph g(reader.begin(), reader.end(),
          reader.num_vertices());

  return 0;
}


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS_HPP

#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>

#endif


#include <algorithm>
#include <exception>
#include <iostream>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <frovedis.hpp>
#include <frovedis/core/dvector.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>
#include <frovedis/ml/tree/tree.hpp>

namespace po = boost::program_options;
using namespace frovedis;

template <typename T>
void do_train(
  const std::string& data_path,
  const std::string& label_path,
  const std::string& output_path,
  const std::unordered_map<size_t, size_t>& categorical_features_info,
  const tree::impurity_type impurity_type,
  const size_t max_depth,
  const size_t max_bins,
  const size_t num_iterations,
  const tree::loss_type loss,
  const T learning_rate,
  const bool binary_mode
) {
  time_spent timer(DEBUG);
  gradient_boosted_trees_model<T> model;

  auto strategy = tree::make_regression_strategy<T>()
    .set_categorical_features_info(categorical_features_info)
    .set_impurity_type(impurity_type)
    .set_max_depth(max_depth)
    .set_max_bins(max_bins)
    .set_num_iterations(num_iterations)
    .set_loss_type(loss)
    .set_learning_rate(learning_rate);
  auto builder = make_gradient_boosted_trees_builder(strategy);

  if (binary_mode) {
    timer.reset();
    auto dataset = make_rowmajor_matrix_loadbinary<T>(data_path);
    timer.show("load matrix: ");
    auto labels = make_dvector_loadbinary<T>(label_path);
    timer.show("load labels: ");
    model = builder.run(dataset, labels);
    timer.show("train model: ");
    model.savebinary(output_path);
    timer.show("save model:  ");
  } else {
    timer.reset();
    auto dataset = make_rowmajor_matrix_load<T>(data_path);
    timer.show("load matrix: ");
    auto labels = make_dvector_loadline(label_path).map(
      +[] (const std::string& line) -> T {
        return boost::lexical_cast<T>(line);
      }
    );
    timer.show("load labels: ");
    model = builder.run(dataset, labels);
    timer.show("train model: ");
    model.save(output_path);
    timer.show("save model:  ");
  }

  RLOG(TRACE) << std::endl << model;
}

template <typename T>
void do_train(const po::variables_map& argmap) {
  do_train<T>(
    argmap["input"].as<std::string>(),
    argmap["label"].as<std::string>(),
    argmap["output"].as<std::string>(),
    tree::parse_categorical_features_info(
      argmap["category"].as<std::string>(),
      argmap["default-cardinality"].as<size_t>()
    ),
    tree::get_impurity_type(argmap["impurity"].as<std::string>()),
    argmap["depth"].as<size_t>(),
    argmap["bins"].as<size_t>(),
    argmap["trees"].as<size_t>(),
    tree::get_loss_type(argmap["loss"].as<std::string>()),
    argmap["learning-rate"].as<double>(),
    argmap.count("binary")
  );
}

po::variables_map parse(int argc, char** argv) {
  po::options_description opt_desc("");
  opt_desc.add_options()
    ("help,h", "show this help message and exit");

  po::options_description reqarg_desc("required arguments");
  reqarg_desc.add_options()
    ("input,i", po::value<std::string>(), "an input matrix")
    ("label,l", po::value<std::string>(), "an input label")
    ("output,o", po::value<std::string>(), "an output model");

  po::options_description optarg_desc("optional arguments");
  optarg_desc.add_options()
    ("category", po::value<std::string>()->default_value(""),
     "categorical features' information (index: cardinality, ...)")
    ("default-cardinality", po::value<size_t>()->default_value(2),
     "a default cardinality of categorical features")
    ("impurity", po::value<std::string>()->default_value("default"),
     "an impurity function")
    ("depth,d", po::value<size_t>()->default_value(4),
     "the maximum depth")
    ("bins,b", po::value<size_t>()->default_value(32),
     "the maximum number of bins")
    ("trees,n", po::value<size_t>()->default_value(10),
     "the number of iterations")
    ("loss", po::value<std::string>()->default_value("default"),
     "a loss function")
    ("learning-rate,s", po::value<double>()->default_value(0.1),
     "a learning rate (shrinkage)")
    ("binary", "use binary input/output")
    ("double", "use double precision")
    ("verbose", "set log-level to DEBUG")
    ("trace", "set log-level to TRACE");

  opt_desc.add(reqarg_desc).add(optarg_desc);
  po::variables_map argmap;
  try {
    po::store(po::command_line_parser(argc, argv)
                .options(opt_desc)
//              .allow_unregistered()
                .run(),
              argmap);
    po::notify(argmap);
  } catch (const po::error_with_option_name& e) {
    std::cerr << e.what() << std::endl;
    finalizefrovedis(1);
  }

  // help message
  if (argmap.count("help")) {
    std::cerr << opt_desc;
    finalizefrovedis(0);
  }

  // check required arguments
  bool missing = false;
  for (const auto opt: reqarg_desc.options()) {
    const std::string& name = opt->long_name();
    if (!argmap.count(name)) {
      std::cerr << "option '--" << name << "' is required" << std::endl;
      missing = true;
    }
  }
  if (missing) { finalizefrovedis(1); }

  return argmap;
}

int main(int argc, char** argv) {
  use_frovedis use(argc, argv);

  const auto argmap = parse(argc, argv);
  if (argmap.count("verbose")) { set_loglevel(DEBUG); }
  if (argmap.count("trace")) { set_loglevel(TRACE); }

  try {
    if (argmap.count("double")) {
      do_train<double>(argmap);
    } else {
      do_train<float>(argmap);
    }
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    finalizefrovedis(1);
  }

  return 0;
}


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

#ifndef SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_
#define SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_

/**
 * @file
 * @brief Solve quadratiac programs with OSQP.
 */

#include <Eigen/Sparse>
#include <osqp/osqp.h>

#include "smooth/feedback/qp_solver.hpp"

namespace smooth::feedback {

/**
 * @brief Solve a QuadraticProgram with the OSQP solver
 *
 * @param pbm QuadraticProgram to solve
 * @param prm solver paramters
 * @param warmstart initial point to start iterating from
 *
 * @return solution
 *
 * @note This is a convenience interface that performs copies and memory allocation in each
 * call. For more fine-grained control use the low-level OSQP interface (https://osqp.org/).
 */
template<typename Problem>
QPSolution<-1, -1, double> solve_qp_osqp(
  const Problem & pbm,
  const QPSolverParams & prm,
  std::optional<std::reference_wrapper<const QPSolution<-1, -1, double>>> warmstart = {})
{
  // Covert to sparse matrices with OSQP indexing
  Eigen::SparseMatrix<double, Eigen::ColMajor, c_int> P;
  Eigen::SparseMatrix<double, Eigen::ColMajor, c_int> A;
  if constexpr (std::is_base_of_v<Eigen::SparseMatrixBase<decltype(pbm.A)>, decltype(pbm.A)>) {
    A = pbm.A;
    P = pbm.P.template triangularView<Eigen::Upper>();
  } else {
    A                   = pbm.A.sparseView();
    Eigen::MatrixXd Pup = pbm.P.template triangularView<Eigen::Upper>();
    P                   = Pup.sparseView();
    A.prune(1e-6);
    P.prune(1e-6);
  }
  A.makeCompressed();
  P.makeCompressed();

  OSQPSettings * settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
  osqp_set_default_settings(settings);

  settings->verbose            = prm.verbose;
  settings->sigma              = prm.sigma;
  settings->alpha              = prm.alpha;
  settings->rho                = prm.rho;
  settings->eps_abs            = prm.eps_abs;
  settings->eps_rel            = prm.eps_rel;
  settings->eps_prim_inf       = prm.eps_primal_inf;
  settings->eps_dual_inf       = prm.eps_dual_inf;
  settings->scaling            = prm.scaling;
  settings->check_termination  = prm.stop_check_iter;
  settings->polish             = prm.polish;
  settings->polish_refine_iter = prm.polish_iter;
  settings->delta              = prm.delta;

  settings->adaptive_rho       = false;
  settings->linsys_solver      = QDLDL_SOLVER;
  settings->scaled_termination = false;

  if (prm.max_iter) {
    settings->max_iter = prm.max_iter.value();
  } else {
    settings->max_iter = std::numeric_limits<c_int>::max();
  }
  if (prm.max_time) {
    settings->time_limit =
      duration_cast<std::chrono::duration<double>>(prm.max_time.value()).count();
  } else {
    settings->time_limit = 0;
  }

  OSQPData * data = (OSQPData *)c_malloc(sizeof(OSQPData));
  data->n         = A.cols();
  data->m         = A.rows();
  data->A         = csc_matrix(
    A.rows(), A.cols(), A.nonZeros(), A.valuePtr(), A.innerIndexPtr(), A.outerIndexPtr());
  data->q = const_cast<double *>(pbm.q.data());
  data->P = csc_matrix(
    P.rows(), P.cols(), P.nonZeros(), P.valuePtr(), P.innerIndexPtr(), P.outerIndexPtr());
  data->l = const_cast<double *>(pbm.l.data());
  data->u = const_cast<double *>(pbm.u.data());

  OSQPWorkspace * work;

  QPSolution<-1, -1, double> ret;
  ret.code = QPSolutionStatus::Unknown;

  c_int error = osqp_setup(&work, data, settings);

  if (warmstart) {
    osqp_warm_start(
      work, warmstart.value().get().primal.data(), warmstart.value().get().dual.data());
    settings->warm_start = 1;
  } else {
    settings->warm_start = 0;
  }

  if (!error) { error &= osqp_solve(work); }

  if (!error) {
    switch (work->info->status_val) {
    case OSQP_SOLVED: {
      ret.code = QPSolutionStatus::Optimal;
      break;
    }
    case OSQP_PRIMAL_INFEASIBLE: {
      ret.code = QPSolutionStatus::PrimalInfeasible;
      break;
    }
    case OSQP_DUAL_INFEASIBLE: {
      ret.code = QPSolutionStatus::DualInfeasible;
      break;
    }
    case OSQP_MAX_ITER_REACHED: {
      ret.code = QPSolutionStatus::MaxIterations;
      break;
    }
    case OSQP_TIME_LIMIT_REACHED: {
      ret.code = QPSolutionStatus::MaxTime;
      break;
    }
    default: {
      break;
    }
    }

    ret.iter      = work->info->iter;
    ret.primal    = Eigen::Map<const Eigen::Matrix<double, -1, 1>>(work->solution->x, data->n);
    ret.dual      = Eigen::Map<const Eigen::Matrix<double, -1, 1>>(work->solution->y, data->m);
    ret.objective = work->info->obj_val;
  }

  osqp_cleanup(work);

  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return ret;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__COMPAT__OSQP_HPP_


// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// 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 <boost/thread/locks.hpp>

#include "SurgSim/Devices/DeviceFilters/ForceScale.h"

#include "SurgSim/Math/Vector.h"

using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::Vector3d;

namespace SurgSim
{
namespace Devices
{

SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::ForceScale, ForceScale);

ForceScale::ForceScale(const std::string& name) :
	DeviceFilter(name),
	m_forceScale(1.0),
	m_torqueScale(1.0)
{
}

void ForceScale::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);

	*result = dataToFilter;

	Vector3d force;
	if (dataToFilter.vectors().get(DataStructures::Names::FORCE, &force))
	{
		force *= m_forceScale;
		result->vectors().set(DataStructures::Names::FORCE, force);
	}

	Vector3d torque;
	if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
	{
		torque *= m_torqueScale;
		result->vectors().set(DataStructures::Names::TORQUE, torque);
	}

	// Scale the spring's contribution to the force and torque. First three rows calculate force, last three for torque.
	DataGroup::DynamicMatrixType springJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
	{
		springJacobian.block<3,6>(0, 0) *= m_forceScale;
		springJacobian.block<3,6>(3, 0) *= m_torqueScale;
		result->matrices().set(DataStructures::Names::SPRING_JACOBIAN, springJacobian);
	}

	// Scale the damper's contribution to the force and torque. First three rows calculate force, last three for torque.
	DataGroup::DynamicMatrixType damperJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
	{
		damperJacobian.block<3,6>(0, 0) *= m_forceScale;
		damperJacobian.block<3,6>(3, 0) *= m_torqueScale;
		result->matrices().set(DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
	}
}

void ForceScale::setForceScale(double forceScale)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_forceScale = forceScale;
}

void ForceScale::setTorqueScale(double torqueScale)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_torqueScale = torqueScale;
}

};  // namespace Devices
};  // namespace SurgSim


#include <dlib/xml_parser.h>
#include <dlib/matrix.h>
#include <fstream>
#include <vector>
#include <stack>
#include <set>
#include <dlib/string.h>

using namespace std;
using namespace dlib;

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

// Only these computational layers have parameters
const std::set<string> comp_tags_with_params = {"fc", "fc_no_bias", "con", "affine_con", "affine_fc", "affine", "prelu"};

struct layer
{
    string type; // comp, loss, or input
    int idx;

    matrix<long,4,1> output_tensor_shape; // (N,K,NR,NC)

    string detail_name; // The name of the tag inside the layer tag. e.g. fc, con, max_pool, input_rgb_image.
    std::map<string,double> attributes;
    matrix<float> params;
    long tag_id = -1;   // If this isn't -1 then it means this layer was tagged, e.g. wrapped with tag2<> giving tag_id==2
    long skip_id = -1;  // If this isn't -1 then it means this layer draws its inputs from
                        // the most recent layer with tag_id==skip_id rather than its immediate predecessor.

    double attribute (const string& key) const
    {
        auto i = attributes.find(key);
        if (i != attributes.end())
            return i->second;
        else
            throw dlib::error("Layer doesn't have the requested attribute '" + key + "'.");
    }

    string caffe_layer_name() const
    {
        if (type == "input")
            return "data";
        else
            return detail_name+to_string(idx);
    }
};

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

std::vector<layer> parse_dlib_xml(
    const matrix<long,4,1>& input_tensor_shape,
    const string& xml_filename
);

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

template <typename iterator>
const layer& find_layer (
    iterator i,
    long tag_id
)
/*!
    requires
        - i is a reverse iterator pointing to a layer in the list of layers produced by parse_dlib_xml().
        - i is not an input layer.
    ensures
        - if (tag_id == -1) then
            - returns the previous layer (i.e. closer to the input) to layer i.
        - else
            - returns the previous layer (i.e. closer to the input) to layer i with the
              given tag_id.
!*/
{
    if (tag_id == -1)
    {
        return *(i-1);
    }
    else
    {
        while(true)
        {
            i--;
            // if we hit the end of the network before we found what we were looking for
            if (i->tag_id == tag_id)
                return *i;
            if (i->type == "input")
                throw dlib::error("Network definition is bad, a layer wanted to skip back to a non-existing layer.");
        }
    }
}

template <typename iterator>
const layer& find_input_layer (iterator i) { return find_layer(i, i->skip_id); }

template <typename iterator>
string find_layer_caffe_name (
    iterator i,
    long tag_id
)
{
    return find_layer(i,tag_id).caffe_layer_name();
}

template <typename iterator>
string find_input_layer_caffe_name (iterator i) { return find_input_layer(i).caffe_layer_name(); }

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

template <typename iterator>
void compute_caffe_padding_size_for_pooling_layer(
    const iterator& i,
    long& pad_x,
    long& pad_y
)
/*!
    requires
        - i is a reverse iterator pointing to a layer in the list of layers produced by parse_dlib_xml().
        - i is not an input layer.
    ensures
        - Caffe is funny about how it computes the output sizes from pooling layers.
          Rather than using the normal formula for output row/column sizes used by all the
          other layers (and what dlib uses everywhere),
            floor((bottom_size + 2*pad - kernel_size) / stride) + 1
          it instead uses:
            ceil((bottom_size + 2*pad - kernel_size) / stride) + 1

          These are the same except when the stride!=1.  In that case we need to figure out
          how to change the padding value so that the output size of the caffe padding
          layer will match the output size of the dlib padding layer.   That is what this
          function does.
!*/
{
    const long dlib_output_nr = i->output_tensor_shape(2);
    const long dlib_output_nc = i->output_tensor_shape(3);
    const long bottom_nr = find_input_layer(i).output_tensor_shape(2);
    const long bottom_nc = find_input_layer(i).output_tensor_shape(3);
    const long padding_x = (long)i->attribute("padding_x");
    const long padding_y = (long)i->attribute("padding_y");
    const long stride_x = (long)i->attribute("stride_x");
    const long stride_y = (long)i->attribute("stride_y");
    long kernel_w = i->attribute("nc");
    long kernel_h = i->attribute("nr");

    if (kernel_w == 0)
        kernel_w = bottom_nc;
    if (kernel_h == 0)
        kernel_h = bottom_nr;

    // The correct padding for caffe could be anything in the range [0,padding_x].  So
    // check what gives the correct output size and use that.
    for (pad_x = 0; pad_x <= padding_x; ++pad_x)
    {
        long caffe_out_size = ceil((bottom_nc + 2.0*pad_x - kernel_w)/(double)stride_x) + 1;
        if (caffe_out_size == dlib_output_nc)
            break;
    }
    if (pad_x == padding_x+1)
    {
        std::ostringstream sout;
        sout << "No conversion between dlib pooling layer parameters and caffe pooling layer parameters found for layer " << to_string(i->idx) << endl;
        sout << "dlib_output_nc: " << dlib_output_nc << endl;
        sout << "bottom_nc:      " << bottom_nc << endl;
        sout << "padding_x:      " << padding_x << endl;
        sout << "stride_x:       " << stride_x << endl;
        sout << "kernel_w:       " << kernel_w << endl;
        sout << "pad_x:          " << pad_x << endl;
        throw dlib::error(sout.str());
    }

    for (pad_y = 0; pad_y <= padding_y; ++pad_y)
    {
        long caffe_out_size = ceil((bottom_nr + 2.0*pad_y - kernel_h)/(double)stride_y) + 1;
        if (caffe_out_size == dlib_output_nr)
            break;
    }
    if (pad_y == padding_y+1)
    {
        std::ostringstream sout;
        sout << "No conversion between dlib pooling layer parameters and caffe pooling layer parameters found for layer " << to_string(i->idx) << endl;
        sout << "dlib_output_nr: " << dlib_output_nr << endl;
        sout << "bottom_nr:      " << bottom_nr << endl;
        sout << "padding_y:      " << padding_y << endl;
        sout << "stride_y:       " << stride_y << endl;
        sout << "kernel_h:       " << kernel_h << endl;
        sout << "pad_y:          " << pad_y << endl;
        throw dlib::error(sout.str());
    }
}

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

void convert_dlib_xml_to_caffe_python_code(
    const string& xml_filename,
    const long N,
    const long K,
    const long NR,
    const long NC
)
{
    const string out_filename = left_substr(xml_filename,".") + "_dlib_to_caffe_model.py";
    const string out_weights_filename = left_substr(xml_filename,".") + "_dlib_to_caffe_model.weights";
    cout << "Writing python part of model to " << out_filename << endl;
    cout << "Writing weights part of model to " << out_weights_filename << endl;
    ofstream fout(out_filename);
    fout.precision(9);
    const auto layers = parse_dlib_xml({N,K,NR,NC}, xml_filename);

    fout << "#\n";
    fout << "# !!! This file was automatically generated by dlib's tools/convert_dlib_nets_to_caffe utility.     !!!\n";
    fout << "# !!! It contains all the information from a dlib DNN network and lets you save it as a cafe model. !!!\n";
    fout << "#\n";
    fout << "import caffe " << endl;
    fout << "from caffe import layers as L, params as P" << endl;
    fout << "import numpy as np" << endl;

    // dlib nets don't commit to a batch size, so just use 1 as the default
    fout << "\n# Input tensor dimensions" << endl;
    fout << "input_batch_size = " << N << ";" << endl;
    if (layers.back().detail_name == "input_rgb_image")
    {
        fout << "input_num_channels = 3;" << endl;
        fout << "input_num_rows = "<<NR<<";" << endl;
        fout << "input_num_cols = "<<NC<<";" << endl;
        if (K != 3)
            throw dlib::error("The dlib model requires input tensors with NUM_CHANNELS==3, but the dtoc command line specified NUM_CHANNELS=="+to_string(K));
    }
    else if (layers.back().detail_name == "input_rgb_image_sized")
    {
        fout << "input_num_channels = 3;" << endl;
        fout << "input_num_rows = " << layers.back().attribute("nr") << ";" << endl;
        fout << "input_num_cols = " << layers.back().attribute("nc") << ";" << endl;
        if (NR != layers.back().attribute("nr"))
            throw dlib::error("The dlib model requires input tensors with NUM_ROWS=="+to_string((long)layers.back().attribute("nr"))+", but the dtoc command line specified NUM_ROWS=="+to_string(NR));
        if (NC != layers.back().attribute("nc"))
            throw dlib::error("The dlib model requires input tensors with NUM_COLUMNS=="+to_string((long)layers.back().attribute("nc"))+", but the dtoc command line specified NUM_COLUMNS=="+to_string(NC));
        if (K != 3)
            throw dlib::error("The dlib model requires input tensors with NUM_CHANNELS==3, but the dtoc command line specified NUM_CHANNELS=="+to_string(K));
    }
    else if (layers.back().detail_name == "input")
    {
        fout << "input_num_channels = 1;" << endl;
        fout << "input_num_rows = "<<NR<<";" << endl;
        fout << "input_num_cols = "<<NC<<";" << endl;
        if (K != 1)
            throw dlib::error("The dlib model requires input tensors with NUM_CHANNELS==1, but the dtoc command line specified NUM_CHANNELS=="+to_string(K));
    }
    else
    {
        throw dlib::error("No known transformation from dlib's " + layers.back().detail_name + " layer to caffe.");
    }
    fout << endl;
    fout << "# Call this function to write the dlib DNN model out to file as a pair of caffe\n";
    fout << "# definition and weight files.  You can then use the network by loading it with\n";
    fout << "# this statement: \n";
    fout << "#    net = caffe.Net(def_file, weights_file, caffe.TEST);\n";
    fout << "#\n";
    fout << "def save_as_caffe_model(def_file, weights_file):\n";
    fout << "    with open(def_file, 'w') as f: f.write(str(make_netspec()));\n";
    fout << "    net = caffe.Net(def_file, caffe.TEST);\n";
    fout << "    set_network_weights(net);\n";
    fout << "    net.save(weights_file);\n\n";
    fout << "###############################################################################\n";
    fout << "#         EVERYTHING BELOW HERE DEFINES THE DLIB MODEL PARAMETERS             #\n";
    fout << "###############################################################################\n\n\n";

    // -----------------------------------------------------------------------------------
    //  The next block of code outputs python code that defines the network architecture.
    // -----------------------------------------------------------------------------------

    fout << "def make_netspec():" << endl;
    fout << "    # For reference, the only \"documentation\" about caffe layer parameters seems to be this page:\n";
    fout << "    # https://github.com/BVLC/caffe/blob/master/src/caffe/proto/caffe.proto\n" << endl;
    fout << "    n = caffe.NetSpec(); " << endl;
    fout << "    n.data,n.label = L.MemoryData(batch_size=input_batch_size, channels=input_num_channels, height=input_num_rows, width=input_num_cols, ntop=2)" << endl;
    // iterate the layers starting with the input layer
    for (auto i = layers.rbegin(); i != layers.rend(); ++i)
    {
        // skip input and loss layers
        if (i->type == "loss" || i->type == "input")
            continue;

        if (i->detail_name == "con")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Convolution(n." << find_input_layer_caffe_name(i);
            fout << ", num_output=" << i->attribute("num_filters");
            fout << ", kernel_w=" << i->attribute("nc");
            fout << ", kernel_h=" << i->attribute("nr");
            fout << ", stride_w=" << i->attribute("stride_x");
            fout << ", stride_h=" << i->attribute("stride_y");
            fout << ", pad_w=" << i->attribute("padding_x");
            fout << ", pad_h=" << i->attribute("padding_y");
            fout << ");\n";
        }
        else if (i->detail_name == "relu")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.ReLU(n." << find_input_layer_caffe_name(i);
            fout << ");\n";
        }
        else if (i->detail_name == "sig")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Sigmoid(n." << find_input_layer_caffe_name(i);
            fout << ");\n";
        }
        else if (i->detail_name == "prelu")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.PReLU(n." << find_input_layer_caffe_name(i);
            fout << ", channel_shared=True";
            fout << ");\n";
        }
        else if (i->detail_name == "max_pool")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Pooling(n." << find_input_layer_caffe_name(i);
            fout << ", pool=P.Pooling.MAX";
            if (i->attribute("nc")==0)
            {
                fout << ", global_pooling=True";
            }
            else
            {
                fout << ", kernel_w=" << i->attribute("nc");
                fout << ", kernel_h=" << i->attribute("nr");
            }

            fout << ", stride_w=" << i->attribute("stride_x");
            fout << ", stride_h=" << i->attribute("stride_y");
            long pad_x, pad_y;
            compute_caffe_padding_size_for_pooling_layer(i, pad_x, pad_y);
            fout << ", pad_w=" << pad_x;
            fout << ", pad_h=" << pad_y;
            fout << ");\n";
        }
        else if (i->detail_name == "avg_pool")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Pooling(n." << find_input_layer_caffe_name(i);
            fout << ", pool=P.Pooling.AVE";
            if (i->attribute("nc")==0)
            {
                fout << ", global_pooling=True";
            }
            else
            {
                fout << ", kernel_w=" << i->attribute("nc");
                fout << ", kernel_h=" << i->attribute("nr");
            }
            if (i->attribute("padding_x") != 0 || i->attribute("padding_y") != 0)
            {
                throw dlib::error("dlib and caffe implement pooling with non-zero padding differently, so you can't convert a "
                    "network with such pooling layers.");
            }

            fout << ", stride_w=" << i->attribute("stride_x");
            fout << ", stride_h=" << i->attribute("stride_y");
            long pad_x, pad_y;
            compute_caffe_padding_size_for_pooling_layer(i, pad_x, pad_y);
            fout << ", pad_w=" << pad_x;
            fout << ", pad_h=" << pad_y;
            fout << ");\n";
        }
        else if (i->detail_name == "fc")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.InnerProduct(n." << find_input_layer_caffe_name(i);
            fout << ", num_output=" << i->attribute("num_outputs");
            fout << ", bias_term=True";
            fout << ");\n";
        }
        else if (i->detail_name == "fc_no_bias")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.InnerProduct(n." << find_input_layer_caffe_name(i);
            fout << ", num_output=" << i->attribute("num_outputs");
            fout << ", bias_term=False";
            fout << ");\n";
        }
        else if (i->detail_name == "bn_con" || i->detail_name == "bn_fc")
        {
            throw dlib::error("Conversion from dlib's batch norm layers to caffe's isn't supported.  Instead, "
                "you should put your dlib network into 'test mode' by switching batch norm layers to affine layers. "
                "Then you can convert that 'test mode' network to caffe.");
        }
        else if (i->detail_name == "affine_con")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Scale(n." << find_input_layer_caffe_name(i);
            fout << ", bias_term=True";
            fout << ");\n";
        }
        else if (i->detail_name == "affine_fc")
        {
            fout << "    n." << i->caffe_layer_name() << " = L.Scale(n." << find_input_layer_caffe_name(i);
            fout << ", bias_term=True";
            fout << ");\n";
        }
        else if (i->detail_name == "add_prev")
        {
            auto in_shape1 = find_input_layer(i).output_tensor_shape;
            auto in_shape2 = find_layer(i,i->attribute("tag")).output_tensor_shape;
            if (in_shape1 != in_shape2)
            {
                // if only the number of channels differs then we will use a dummy layer to
                // pad with zeros.  But otherwise we will throw an error.
                if (in_shape1(0) == in_shape2(0) &&
                    in_shape1(2) == in_shape2(2) &&
                    in_shape1(3) == in_shape2(3))
                {
                    fout << "    n." << i->caffe_layer_name() << "_zeropad = L.DummyData(num=" << in_shape1(0);
                    fout << ", channels="<<std::abs(in_shape1(1)-in_shape2(1));
                    fout << ", height="<<in_shape1(2);
                    fout << ", width="<<in_shape1(3);
                    fout << ");\n";

                    string smaller_layer = find_input_layer_caffe_name(i);
                    string bigger_layer = find_layer_caffe_name(i, i->attribute("tag"));
                    if (in_shape1(1) > in_shape2(1))
                        swap(smaller_layer, bigger_layer);

                    fout << "    n." << i->caffe_layer_name() << "_concat = L.Concat(n." << smaller_layer;
                    fout << ", n." << i->caffe_layer_name() << "_zeropad";
                    fout << ");\n";

                    fout << "    n." << i->caffe_layer_name() << " = L.Eltwise(n." << i->caffe_layer_name() << "_concat";
                    fout << ", n." << bigger_layer;
                    fout << ", operation=P.Eltwise.SUM";
                    fout << ");\n";
                }
                else
                {
                    std::ostringstream sout;
                    sout << "The dlib network contained an add_prev layer (layer idx " << i->idx << ") that adds two previous ";
                    sout << "layers with different output tensor dimensions.  Caffe's equivalent layer, Eltwise, doesn't support ";
                    sout << "adding layers together with different dimensions.  In the special case where the only difference is ";
                    sout << "in the number of channels, this converter program will add a dummy layer that outputs a tensor full of zeros ";
                    sout << "and concat it appropriately so this will work.  However, this network you are converting has tensor dimensions ";
                    sout << "different in values other than the number of channels.  In particular, here are the two tensor shapes (batch size, channels, rows, cols): ";
                    std::ostringstream sout2;
                    sout2 << wrap_string(sout.str()) << endl;
                    sout2 << trans(in_shape1);
                    sout2 << trans(in_shape2);
                    throw dlib::error(sout2.str());
                }
            }
            else
            {
                fout << "    n." << i->caffe_layer_name() << " = L.Eltwise(n." << find_input_layer_caffe_name(i);
                fout << ", n." << find_layer_caffe_name(i, i->attribute("tag"));
                fout << ", operation=P.Eltwise.SUM";
                fout << ");\n";
            }
        }
        else
        {
            throw dlib::error("No known transformation from dlib's " + i->detail_name + " layer to caffe.");
        }
    }
    fout << "    return n.to_proto();\n\n" << endl;

    // -----------------------------------------------------------------------------------
    //  The next block of code outputs python code that populates all the filter weights.
    // -----------------------------------------------------------------------------------

    ofstream fweights(out_weights_filename, ios::binary);
    fout << "def set_network_weights(net):\n";
    fout << "    # populate network parameters\n";
    fout << "    f = open('"<<out_weights_filename<<"', 'rb');\n";
    // iterate the layers starting with the input layer
    for (auto i = layers.rbegin(); i != layers.rend(); ++i)
    {
        // skip input and loss layers
        if (i->type == "loss" || i->type == "input")
            continue;

        if (i->detail_name == "con")
        {
            const long num_filters = i->attribute("num_filters");
            matrix<float> weights = trans(rowm(i->params,range(0,i->params.size()-num_filters-1)));
            matrix<float> biases  = trans(rowm(i->params,range(i->params.size()-num_filters, i->params.size()-1)));
            fweights.write((char*)&weights(0,0), weights.size()*sizeof(float));
            fweights.write((char*)&biases(0,0), biases.size()*sizeof(float));

            // main filter weights
            fout << "    p = np.fromfile(f, dtype='float32', count="<<weights.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][0].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][0].data[:] = p;\n";

            // biases
            fout << "    p = np.fromfile(f, dtype='float32', count="<<biases.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][1].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][1].data[:] = p;\n";
        }
        else if (i->detail_name == "fc")
        {
            matrix<float> weights = trans(rowm(i->params, range(0,i->params.nr()-2)));
            matrix<float> biases  = rowm(i->params, i->params.nr()-1);
            fweights.write((char*)&weights(0,0), weights.size()*sizeof(float));
            fweights.write((char*)&biases(0,0), biases.size()*sizeof(float));

            // main filter weights
            fout << "    p = np.fromfile(f, dtype='float32', count="<<weights.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][0].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][0].data[:] = p;\n";

            // biases
            fout << "    p = np.fromfile(f, dtype='float32', count="<<biases.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][1].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][1].data[:] = p;\n";
        }
        else if (i->detail_name == "fc_no_bias")
        {
            matrix<float> weights = trans(i->params);
            fweights.write((char*)&weights(0,0), weights.size()*sizeof(float));

            // main filter weights
            fout << "    p = np.fromfile(f, dtype='float32', count="<<weights.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][0].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][0].data[:] = p;\n";
        }
        else if (i->detail_name == "affine_con" || i->detail_name == "affine_fc")
        {
            const long dims = i->params.size()/2;
            matrix<float> gamma = trans(rowm(i->params,range(0,dims-1)));
            matrix<float> beta  = trans(rowm(i->params,range(dims, 2*dims-1)));
            fweights.write((char*)&gamma(0,0), gamma.size()*sizeof(float));
            fweights.write((char*)&beta(0,0), beta.size()*sizeof(float));

            // set gamma weights
            fout << "    p = np.fromfile(f, dtype='float32', count="<<gamma.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][0].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][0].data[:] = p;\n";

            // set beta weights
            fout << "    p = np.fromfile(f, dtype='float32', count="<<beta.size()<<");\n";
            fout << "    p.shape = net.params['"<<i->caffe_layer_name()<<"'][1].data.shape;\n";
            fout << "    net.params['"<<i->caffe_layer_name()<<"'][1].data[:] = p;\n";
        }
        else if (i->detail_name == "prelu")
        {
            const double param = i->params(0);

            // main filter weights
            fout << "    tmp = net.params['"<<i->caffe_layer_name()<<"'][0].data.view();\n";
            fout << "    tmp.shape = 1;\n";
            fout << "    tmp[0] = "<<param<<";\n";
        }
    }

}

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

int main(int argc, char** argv) try
{
    if (argc != 6)
    {
        cout << "To use this program, give it an xml file generated by dlib::net_to_xml() " << endl;
        cout << "and then 4 numbers that indicate the input tensor size.  It will convert " << endl;
        cout << "the xml file into a python file that outputs a caffe model containing the dlib model." << endl;
        cout << "For example, you might run this program like this: " << endl;
        cout << "   ./dtoc lenet.xml 1 1 28 28" << endl;
        cout << "would convert the lenet.xml model into a caffe model with an input tensor of shape(1,1,28,28)" << endl;
        cout << "where the shape values are (num samples in batch, num channels, num rows, num columns)." << endl;
        return 0;
    }

    const long N = sa = argv[2];
    const long K = sa = argv[3];
    const long NR = sa = argv[4];
    const long NC = sa = argv[5];

    convert_dlib_xml_to_caffe_python_code(argv[1], N, K, NR, NC);

    return 0;
}
catch(std::exception& e)
{
    cout << "\n\n*************** ERROR CONVERTING TO CAFFE ***************\n" << e.what() << endl;
    return 1;
}

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

class doc_handler : public document_handler
{
public:
    std::vector<layer> layers;
    bool seen_first_tag = false;

    layer next_layer;
    std::stack<string> current_tag;
    long tag_id = -1;

    virtual void start_document (
    )
    {
        layers.clear();
        seen_first_tag = false;
        tag_id = -1;
    }

    virtual void end_document (
    ) { }

    virtual void start_element (
        const unsigned long /*line_number*/,
        const std::string& name,
        const dlib::attribute_list& atts
    )
    {
        if (!seen_first_tag)
        {
            if (name != "net")
                throw dlib::error("The top level XML tag must be a 'net' tag.");
            seen_first_tag = true;
        }

        if (name == "layer")
        {
            next_layer = layer();
            if (atts["type"] == "skip")
            {
                // Don't make a new layer, just apply the tag id to the previous layer
                if (layers.size() == 0)
                    throw dlib::error("A skip layer was found as the first layer, but the first layer should be an input layer.");
                layers.back().skip_id = sa = atts["id"];

                // We intentionally leave next_layer empty so the end_element() callback
                // don't add it as another layer when called.
            }
            else if (atts["type"] == "tag")
            {
                // Don't make a new layer, just remember the tag id so we can apply it on
                // the next layer.
                tag_id = sa = atts["id"];

                // We intentionally leave next_layer empty so the end_element() callback
                // don't add it as another layer when called.
            }
            else
            {
                next_layer.idx = sa = atts["idx"];
                next_layer.type = atts["type"];
                if (tag_id != -1)
                {
                    next_layer.tag_id = tag_id;
                    tag_id = -1;
                }
            }
        }
        else if (current_tag.size() != 0 && current_tag.top() == "layer")
        {
            next_layer.detail_name = name;
            // copy all the XML tag's attributes into the layer struct
            atts.reset();
            while (atts.move_next())
                next_layer.attributes[atts.element().key()] = sa = atts.element().value();
        }

        current_tag.push(name);
    }

    virtual void end_element (
        const unsigned long /*line_number*/,
        const std::string& name
    )
    {
        current_tag.pop();
        if (name == "layer" && next_layer.type.size() != 0)
            layers.push_back(next_layer);
    }

    virtual void characters (
        const std::string& data
    )
    {
        if (current_tag.size() == 0)
            return;

        if (comp_tags_with_params.count(current_tag.top()) != 0)
        {
            istringstream sin(data);
            sin >> next_layer.params;
        }

    }

    virtual void processing_instruction (
        const unsigned long /*line_number*/,
        const std::string& /*target*/,
        const std::string& /*data*/
    )
    {
    }
};

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

void compute_output_tensor_shapes(const matrix<long,4,1>& input_tensor_shape, std::vector<layer>& layers)
{
    DLIB_CASSERT(layers.back().type == "input");
    layers.back().output_tensor_shape = input_tensor_shape;
    for (auto i = ++layers.rbegin(); i != layers.rend(); ++i)
    {
        const auto input_shape = find_input_layer(i).output_tensor_shape;
        if (i->type == "comp")
        {
            if (i->detail_name == "fc" || i->detail_name == "fc_no_bias")
            {
                long num_outputs = i->attribute("num_outputs");
                i->output_tensor_shape = {input_shape(0), num_outputs, 1, 1};
            }
            else if (i->detail_name == "con")
            {
                long num_filters = i->attribute("num_filters");
                long filter_nc = i->attribute("nc");
                long filter_nr = i->attribute("nr");
                long stride_x = i->attribute("stride_x");
                long stride_y = i->attribute("stride_y");
                long padding_x = i->attribute("padding_x");
                long padding_y = i->attribute("padding_y");
                long nr = 1+(input_shape(2) + 2*padding_y - filter_nr)/stride_y;
                long nc = 1+(input_shape(3) + 2*padding_x - filter_nc)/stride_x;
                i->output_tensor_shape = {input_shape(0), num_filters, nr, nc};
            }
            else if (i->detail_name == "max_pool" || i->detail_name == "avg_pool")
            {
                long filter_nc = i->attribute("nc");
                long filter_nr = i->attribute("nr");
                long stride_x = i->attribute("stride_x");
                long stride_y = i->attribute("stride_y");
                long padding_x = i->attribute("padding_x");
                long padding_y = i->attribute("padding_y");
                if (filter_nc != 0)
                {
                    long nr = 1+(input_shape(2) + 2*padding_y - filter_nr)/stride_y;
                    long nc = 1+(input_shape(3) + 2*padding_x - filter_nc)/stride_x;
                    i->output_tensor_shape = {input_shape(0), input_shape(1), nr, nc};
                }
                else // if we are filtering the whole input down to one thing
                {
                    i->output_tensor_shape = {input_shape(0), input_shape(1), 1, 1};
                }
            }
            else if (i->detail_name == "add_prev")
            {
                auto aux_shape = find_layer(i, i->attribute("tag")).output_tensor_shape;
                for (long j = 0; j < input_shape.size(); ++j)
                    i->output_tensor_shape(j) = std::max(input_shape(j), aux_shape(j));
            }
            else
            {
                i->output_tensor_shape = input_shape;
            }
        }
        else
        {
            i->output_tensor_shape = input_shape;
        }

    }
}

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

std::vector<layer> parse_dlib_xml(
    const matrix<long,4,1>& input_tensor_shape,
    const string& xml_filename
)
{
    doc_handler dh;
    parse_xml(xml_filename, dh);
    if (dh.layers.size() == 0)
        throw dlib::error("No layers found in XML file!");

    if (dh.layers.back().type != "input")
        throw dlib::error("The network in the XML file is missing an input layer!");

    compute_output_tensor_shapes(input_tensor_shape, dh.layers);

    return dh.layers;
}

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


#include "drake/solvers/fbstab/components/mpc_variable.h"

#include <cmath>
#include <memory>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/mpc_data.h"

namespace drake {
namespace solvers {
namespace fbstab {

using VectorXd = Eigen::VectorXd;
using MatrixXd = Eigen::MatrixXd;

MpcVariable::MpcVariable(int N, int nx, int nu, int nc) {
  if (N <= 0 || nx <= 0 || nu <= 0 || nc <= 0) {
    throw std::runtime_error(
        "All size inputs to MpcVariable::MpcVariable must be >= 1.");
  }
  N_ = N;
  nx_ = nx;
  nu_ = nu;
  nc_ = nc;

  nz_ = (N_ + 1) * (nx_ + nu_);
  nl_ = (N_ + 1) * nx_;
  nv_ = (N_ + 1) * nc_;

  z_storage_ = std::make_unique<VectorXd>(nz_);
  l_storage_ = std::make_unique<VectorXd>(nl_);
  v_storage_ = std::make_unique<VectorXd>(nv_);
  y_storage_ = std::make_unique<VectorXd>(nv_);

  z_ = z_storage_.get();
  l_ = l_storage_.get();
  v_ = v_storage_.get();
  y_ = y_storage_.get();

  z_->setConstant(0.0);
  l_->setConstant(0.0);
  v_->setConstant(0.0);
  y_->setConstant(0.0);
}

MpcVariable::MpcVariable(VectorXd* z, VectorXd* l, VectorXd* v, VectorXd* y) {
  if (z == nullptr || l == nullptr || v == nullptr || y == nullptr) {
    throw std::runtime_error(
        "Inputs to MpcVariable::MpcVariable cannot be null.");
  }
  if (z->size() == 0 || l->size() == 0 || v->size() == 0 || y->size() == 0) {
    throw std::runtime_error(
        "All size inputs to MpcVariable::MpcVariable must be >= 1.");
  }
  if (v->size() != y->size()) {
    throw std::runtime_error(
        "In MpcVariable::MpcVariable, y and v must be the same size");
  }

  nz_ = z->size();
  nl_ = l->size();
  nv_ = v->size();

  z_ = z;
  l_ = l;
  v_ = v;
  y_ = y;
}

void MpcVariable::Fill(double a) {
  z_->setConstant(a);
  l_->setConstant(a);
  v_->setConstant(a);
  InitializeConstraintMargin();
}

void MpcVariable::InitializeConstraintMargin() {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "Cannot call MpcVariable::InitializeConstraintMargin unless data is "
        "linked.");
  }
  // y = b - A*z
  y_->setConstant(0.0);
  data_->axpyb(1.0, y_);
  data_->gemvA(*z_, -1.0, 1.0, y_);
}

void MpcVariable::axpy(double a, const MpcVariable& x) {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "Cannot call MpcVariable::axpy unless data is linked.");
  }

  z_->noalias() += a * (*x.z_);
  l_->noalias() += a * (*x.l_);
  v_->noalias() += a * (*x.v_);

  // y <- y + a*(x.y - b)
  y_->noalias() += a * (*x.y_);
  data_->axpyb(-a, y_);
}

void MpcVariable::Copy(const MpcVariable& x) {
  *z_ = *x.z_;
  *l_ = *x.l_;
  *v_ = *x.v_;
  *y_ = *x.y_;
  data_ = x.data_;
}

const MpcData* MpcVariable::data() const {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "In MpcVariable::data: tried to access problem data before it's "
        "linked.");
  }
  return data_;
}

void MpcVariable::ProjectDuals() { *v_ = v_->cwiseMax(0); }

double MpcVariable::Norm() const {
  const double t1 = z_->norm();
  const double t2 = l_->norm();
  const double t3 = v_->norm();

  return sqrt(t1 * t1 + t2 * t2 + t3 * t3);
}

bool MpcVariable::SameSize(const MpcVariable& x, const MpcVariable& y) {
  return (x.nz_ == y.nz_ && x.nl_ == y.nl_ && x.nv_ == y.nv_);
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


#define BOOST_TEST_MODULE pcraster com iraster
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "com_raster.h"

BOOST_AUTO_TEST_CASE(outside)
{
  using namespace com;

  Raster<UINT1> v(4,5,8);

  BOOST_CHECK(!v.isOutside( 0, 0));
  BOOST_CHECK( v.isOutside(-1, 0));
  BOOST_CHECK( v.isOutside(-1, -1));
  BOOST_CHECK( v.isOutside( 0, -1));
  BOOST_CHECK( v.isOutside( 4,  5));
  BOOST_CHECK( v.isOutside( 3,  6));
  BOOST_CHECK( v.isOutside( 4,  2));

  BOOST_CHECK(v.cell(3,4)==8);

  v.setMV(0,2);

  BOOST_CHECK(v.isMV(0,2));
  BOOST_CHECK(!v.isMV(3,4));
}

BOOST_AUTO_TEST_CASE(get_cell)
{
  using namespace com;

  Raster<UINT1> v(4,5,8);
  v.setMV(0,2);

  BOOST_CHECK(!v.isMV(3,4));

  {
  int r=0,c=0;
  UINT1 result=10;
  BOOST_CHECK(v.cell(result,r,c));
  BOOST_CHECK(result==8);
  }

  {
  size_t r=0,c=0;
  UINT1 result=10;
  BOOST_CHECK(v.cell(result,r,c));
  BOOST_CHECK(result==8);
  }

  {
  int r=-1,c=0;
  UINT1 result=10;
  BOOST_CHECK(!v.cell(result,r,c));
  BOOST_CHECK(result==10); // untouched
  }
  {
   int r=0,c=2;
   UINT1 result=10;
   BOOST_CHECK(v.isMV(r,c));
   BOOST_CHECK(!v.cell(result,r,c));
   BOOST_CHECK(result==10);
  }

  {
   size_t r=0,c=2;
   UINT1 result=10;
   BOOST_CHECK(v.isMV(r,c));
   BOOST_CHECK(!v.cell(result,r,c));
   BOOST_CHECK(result==10);
  }
}


/**
 * \file FourthOrderFilter.hxx
 */

#include "FourthOrderFilter.h"
#include <ATK/EQ/IIRFilter.h>

#include <boost/math/constants/constants.hpp>

#include <cassert>
#include <cmath>

namespace ATK
{
  template <typename DataType_>
  FourthOrderBaseCoefficients<DataType_>::FourthOrderBaseCoefficients(gsl::index nb_channels)
    :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void FourthOrderBaseCoefficients<DataType_>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);
  }

  template <typename DataType_>
  void FourthOrderBaseCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename FourthOrderBaseCoefficients<DataType_>::CoeffDataType FourthOrderBaseCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }
}


#include <boost/graph/maximum_adjacency_search.hpp>


#include "igl/readOBJ.h"
#include "igl/writeOBJ.h"
// #include "igl/decimate.h"

#include <Eigen/Core>

#include "pythonlike.h"

#include <cstdlib> // exit()
#include <iostream>
#include <cassert>
#include <cstdio> // printf()

#include <igl/seam_edges.h>
#include "decimate.h"
#include "quadric_error_metric.h"
#include <igl/writeDMAT.h>

// An anonymous namespace. This hides these symbols from other modules.
namespace {

void usage( const char* argv0 )
{
    std::cerr << "Usage: " << argv0 << " <path/to/input.obj> num-vertices     <target_number_of_vertices>  [--strict] [<strictness>]" << std::endl;
    std::cerr << "Usage: " << argv0 << " <path/to/input.obj> percent-vertices <target_percent_of_vertices> [--strict] [<strictness>]" << std::endl;
    exit(-1);
}

int count_seam_edge_num(const EdgeMap& seam_vertex_edges)
{
	int count = 0;
	for(auto & v : seam_vertex_edges) {
		count += v.second.size();
	}
	return count / 2;
}

enum SeamAwareDegree
{
	NoUVShapePreserving,
	UVShapePreserving,
	Seamless
};

/*
Decimates a triangle mesh down to a target number of vertices,
preserving the UV parameterization.
TODO Q: Should we do a version that does not preserve the UV parameterization exactly,
        but instead returns a sequence of TC/FTC that can be used to transform a UV
        point between the parameterizations of the decimated and undecimated mesh?
Input parameters:
    V: The 3D positions of the input mesh (3 columns)
    TC: The 2D texture coordinates of the input mesh (2 columns)
    F: Indices into `V` for the three vertices of each triangle.
    FTC: Indices into `TC` for the three vertices of each triangle.
Output parameters:
    Vout: The 3D positions of the decimated mesh (3 columns),
          where #vertices is as close as possible to `target_num_vertices`)
    TCout: The texture coordinates of the decimated mesh (2 columns)
    Fout: Indices into `Vout` for the three vertices of each triangle.
    FTCout: Indices into `TCout` for the three vertices of each triangle.
Returns:
    True if the routine succeeded, false if an error occurred.
Notes:
    The output mesh will a vertex count as close as possible to `target_num_vertices`.
    The decimated mesh should never have fewer vertices than `target_num_vertices`.
*/
template <typename DerivedV, typename DerivedF, typename DerivedT>
bool decimate_down_to(
    const Eigen::PlainObjectBase<DerivedV>& V,
    const Eigen::PlainObjectBase<DerivedF>& F,
    const Eigen::PlainObjectBase<DerivedT>& TC,
    const Eigen::PlainObjectBase<DerivedF>& FT,
    int target_num_vertices,
    Eigen::MatrixXd& V_out,
    Eigen::MatrixXi& F_out,
    Eigen::MatrixXd& TC_out,
    Eigen::MatrixXi& FT_out,
    int seam_aware_degree
    )
{
#define DEBUG_DECIMATE_DOWN_TO
    assert( target_num_vertices > 0 );
    assert( target_num_vertices < V.rows() );

    /// 3D triangle mesh with UVs.
    // 3D
    assert( V.cols() == 3 );
    // triangle mesh
    assert( F.cols() == 3 );
    // UVs
    assert( TC.cols() == 2 );
    assert( FT.cols() == 3 );
    assert( FT.cols() == F.cols() );

    // Print information about seams.
    Eigen::MatrixXi seams, boundaries, foldovers;
    igl::seam_edges( V, TC, F, FT, seams, boundaries, foldovers );
#ifdef DEBUG_DECIMATE_DOWN_TO
    std::cout << "seams: " << seams.rows() << "\n";
    std::cout << seams << std::endl;
    std::cout << "boundaries: " << boundaries.rows() << "\n";
    std::cout << boundaries << std::endl;
    std::cout << "foldovers: " << foldovers.rows() << "\n";
    std::cout << foldovers << std::endl;
#endif

    // Collect all vertex indices involved in seams.
    std::unordered_set< int > seam_vertex_indices;
    // Also collect the edges in terms of position vertex indices themselves.
    EdgeMap seam_vertex_edges;
    {
		for( int i = 0; i < seams.rows(); ++i ) {
		    const int v1 = F( seams( i, 0 ),   seams( i, 1 ) );
		    const int v2 = F( seams( i, 0 ), ( seams( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
			// The vertices on both sides should match:
			assert( seam_vertex_indices.count( F( seams( i, 2 ),   seams( i, 3 ) ) ) );
			assert( seam_vertex_indices.count( F( seams( i, 2 ), ( seams( i, 3 ) + 1 ) % 3 ) ) );
		}
		for( int i = 0; i < boundaries.rows(); ++i ) {
		    const int v1 = F( boundaries( i, 0 ),   boundaries( i, 1 ) );
		    const int v2 = F( boundaries( i, 0 ), ( boundaries( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
		}
		for( int i = 0; i < foldovers.rows(); ++i ) {
		    const int v1 = F( foldovers( i, 0 ),   foldovers( i, 1 ) );
		    const int v2 = F( foldovers( i, 0 ), ( foldovers( i, 1 ) + 1 ) % 3 );
			seam_vertex_indices.insert( v1 );
			seam_vertex_indices.insert( v2 );
			insert_edge( seam_vertex_edges, v1, v2 );
			// The vertices on both sides should match:
			assert( seam_vertex_indices.count( F( foldovers( i, 2 ),   foldovers( i, 3 ) ) ) );
			assert( seam_vertex_indices.count( F( foldovers( i, 2 ), ( foldovers( i, 3 ) + 1 ) % 3 ) ) );
		}

	    std::cout << "# seam vertices: " << seam_vertex_indices.size() << std::endl;
		std::cout << "# seam edges: " << count_seam_edge_num(seam_vertex_edges) << std::endl;
    }

    // Compute the per-vertex quadric error metric.
    std::vector< Eigen::MatrixXd > Q;
    bool success = false;
    Eigen::VectorXi J;

	MapV5d hash_Q;
	half_edge_qslim_5d(V,F,TC,FT,hash_Q);
	std::cout << "computing initial metrics finished\n" << std::endl;
	success = decimate_halfedge_5d(
		V, F,
		TC, FT,
		seam_vertex_edges,
		hash_Q,
		target_num_vertices,
		seam_aware_degree,
		V_out, F_out,
		TC_out, FT_out
		);
	std::cout << "#seams after decimation: " << count_seam_edge_num(seam_vertex_edges) << std::endl;
    std::cout << "#interior foldeover: " << interior_foldovers.size() << std::endl;
    std::cout << "#exterior foldeover: " << exterior_foldovers.size() << std::endl;
    return success;
}
}

int main( int argc, char* argv[] ) {
    std::vector<std::string> args( argv + 1, argv + argc );
    std::string strictness;
    int seam_aware_degree = int( SeamAwareDegree::Seamless );
    const bool found_strictness = pythonlike::get_optional_parameter( args, "--strict", strictness );
	if ( found_strictness ) {
		seam_aware_degree = atoi(strictness.c_str());
	}

    if( args.size() != 3 && args.size() != 4 )	usage( argv[0] );
    std::string input_path, command, command_parameter;
    pythonlike::unpack( args.begin(), input_path, command, command_parameter );
    args.erase( args.begin(), args.begin() + 3 );

    // Does the input path exist?
    Eigen::MatrixXd V, TC, CN;
    Eigen::MatrixXi F, FT, FN;
    if( !igl::readOBJ( input_path, V, TC, CN, F, FT, FN ) ) {
        std::cerr << "ERROR: Could not read OBJ: " << input_path << std::endl;
        usage( argv[0] );
    }

    std::cout << "Loaded a mesh with " << V.rows() << " vertices and " << F.rows() << " faces: " << input_path << std::endl;

    // Get the target number of vertices.
    int target_num_vertices = 0;
    if( command == "num-vertices" ) {
        // strto<> returns 0 upon failure, which is fine, since that is invalid input for us.
        target_num_vertices = pythonlike::strto< int >( command_parameter );
    }
    else if( command == "percent-vertices" ) {
        const double percent = pythonlike::strto< double >( command_parameter );
        target_num_vertices = lround( ( percent * V.rows() )/100. );
        std::cout << command_parameter << "% of " << std::to_string( V.rows() ) << " input vertices is " << std::to_string( target_num_vertices ) << " output vertices." << std::endl;
        // Ugh, printf() requires me to specify the types of integers versus longs.
        // printf( "%.2f%% of %d input vertices is %d output vertices.", percent, V.rows(), target_num_vertices );
    }
    else {
        std::cerr << "ERROR: Unknown command: " << command << std::endl;
        usage( argv[0] );
    }

    // Check that the target number of vertices is positive and fewer than the input number of vertices.
    if( target_num_vertices <= 0 ) {
        std::cerr << "ERROR: Target number of vertices must be a positive integer: " << argv[4] << std::endl;
        usage( argv[0] );
    }
    if( target_num_vertices >= V.rows() ) {
    	std::string output_path = pythonlike::os_path_splitext( input_path ).first + "-decimated_to_" + std::to_string( V.rows() ) + "_vertices.obj";
        if( !igl::writeOBJ( output_path, V, F, CN, FN, TC, FT ) ) {
			std::cerr << "ERROR: Could not write OBJ: " << output_path << std::endl;
			usage( argv[0] );
		}
   		std::cout << "Wrote: " << output_path << std::endl;
        std::cerr << "ERROR: Target number of vertices must be smaller than the input number of vertices: " << argv[4] << std::endl;
        return 0;
    }

    // Make the default output path.
    std::string output_path = pythonlike::os_path_splitext( input_path ).first + "-decimated_to_" + std::to_string( target_num_vertices ) + "_vertices.obj";
    if( !args.empty() ) {
        output_path = args.front();
        args.erase( args.begin() );
    }

    // We should have consumed all arguments.
    if( !args.empty() ) usage( argv[0] );

    // Decimate!
    Eigen::MatrixXd V_out, TC_out, CN_out;
    Eigen::MatrixXi F_out, FT_out, FN_out;
    const bool success = decimate_down_to( V, F, TC, FT, target_num_vertices, V_out, F_out, TC_out, FT_out, seam_aware_degree );
    if( !success ) {
        std::cerr << "WARNING: decimate_down_to() returned false (target number of vertices may have been unachievable)." << std::endl;
    }

    if( !igl::writeOBJ( output_path, V_out, F_out, CN_out, FN_out, TC_out, FT_out ) ) {
        std::cerr << "ERROR: Could not write OBJ: " << output_path << std::endl;
        usage( argv[0] );
    }
    std::cout << "Wrote: " << output_path << std::endl;

    return 0;
}


#include "HashTableHandler.h"
#include "SseHandler.h"
#include <stdio.h>
#include <string>
#include <map>
#include <fstream>
#include "llvm/Support/raw_ostream.h"
#include <cstdio>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <array>
#include <regex>
#include "helper.h"
#include "klee/ExecutionState.h"
#include "Executor.h"
#include <cmath>

#include <boost/lambda/lambda.hpp>
#include <boost/math/distributions/beta.hpp>

using namespace boost::lambda;
using namespace boost::math;

using namespace klee;
using namespace std;
using namespace llvm;

SseHandler::SseHandler(Executor* _executor, unsigned _line,
                       double _confidence, double _interval,
                       unsigned _maxSample,
                       unsigned _isEnabled):
executor(_executor),
targetLine(_line),
confidence(_confidence),
interval(_interval),
maxSample(_maxSample),
isEnabled(_isEnabled)
{
   LOG(LOG_MASK_SSE, "new sse handler created, line=%u, confi=%f, interval=%f",
       targetLine, confidence, interval);
}

SseHandler::SseHandler(SseHandler &_handler):
   executor(_handler.executor),
   targetLine(_handler.targetLine),
   confidence(_handler.confidence),
   interval(_handler.interval),
   maxSample(_handler.maxSample)
{

}

SseHandler::~SseHandler()
{

}

void SseHandler::sampleOnce(ExecutionState &state)
{
   // keep running this state, do not schedule any other state
   // TODO: avoid forking new states.
   state.isSample = true;
   state.isSampleDone = false;

   while (!state.isSampleDone) {
      //LOG(LOG_MASK_SSE, "to run an ins = %f", state.getPathProb());

      KInstruction *ki = state.pc;

      executor->stepInstruction(state);
      executor->executeInstruction(state, ki);

      //LOG(LOG_MASK_SSE, "finish an ins = %f", state.getPathProb());
   }
}

void SseHandler::run(ExecutionState &initialState)
{
   vector<unsigned> res;
   LOG(LOG_MASK_SSE, "SSE starts to take max %d samples", maxSample);

   // Beta(a,b) distribution
   // unsigned a = 1;
   // unsigned b = 1;
   double a = 1;
   double b = 1;

   static double explored = 0.0;

   for (unsigned i=0; i < maxSample; i ++) {
      LOG(LOG_MASK_SSE, "start taking the %dth sample", i);

      // make a copy of the initial state state
      ExecutionState *nextState = new ExecutionState(initialState);
      sampleOnce(*nextState);

      LOG(LOG_MASK_SSE, "sample %d trigger target event %d times",
          i, nextState->sampleRes);
      res.push_back(nextState->sampleRes);

      if (nextState->sampleRes > 0) {
         a += 1;
      } else {
         b += 1;
      }

      double esti = a / (a + b);

      double left = esti - interval < 0? 0 : esti - interval;
      double right = esti + interval >1? 1 : esti + interval;

      double p = ibeta(a, b, right) - ibeta(a, b, left);

      LOG(LOG_MASK_SSE, "Beta(%g, %g), left=%g, right=%g, p=%g",
          a, b, left, right, p);

      // If Informed Sampling is enabled, we relax the confidence level
      // based on the explored space.
      if (isEnabled) {
         explored += nextState->getPathProb();
         confidence = (confidence - explored) / (1.0 - explored);
         LOG(LOG_MASK_SSE, "pathprob=%g, explored updated to %g, confidence "
             "level updated to %g", nextState->getPathProb(), explored, confidence);
      }

      delete nextState;

      if (p >= confidence) {
         LOG(LOG_MASK_SSE, "satisfied the confidence! p=%g, mu=%g",
             p, esti);
         break;
      } else {
         LOG(LOG_MASK_SSE, "not satisfying the confidence! p=%g, mu=%g",
             p, esti);
      }
   }

   /*
   for (int i=0; i < res.size(); i++) {
      LOG(LOG_MASK_SSE, "sample %d trigger target event %d times",
          i, res[i]);
   }
   */

   LOG(LOG_MASK_SSE, "sampled %ld states, done", res.size());
}


// Copyright (c) 2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "test/test_syscoin_services.h"
#include "data/utxo.json.h"
#include "utiltime.h"
#include "rpc/server.h"
#include <boost/test/unit_test.hpp>
#include <univalue.h>
using namespace std;
int currentTx = 0;
extern UniValue read_json(const std::string& jsondata);
BOOST_FIXTURE_TEST_SUITE (syscoin_snapshot_tests, SyscoinMainNetSetup)
struct PaymentAmount
{
	std::string address;
	std::string alias;
	CAmount amount;
};
void SendSnapShotPayment(const std::string &strSend, const std::string &asset, const std::string &alias, const std::string &memo)
{
	currentTx++;
	std::string strSendMany = "assetsend " + asset + " " + strSend + "}]\" " + memo + " ''";
	UniValue r;
	BOOST_CHECK_THROW(r = CallRPC("mainnet1", strSendMany, false), runtime_error);
}
void GenerateAirDrop(const std::vector<PaymentAmount> &paymentAmounts, const CAmount& nTotal)
{
	UniValue r;
	// air drop in percentage / 100
	double airDropPct = 0.05;
	string assetName = "asset";
	string aliasName = "alias";
	string memo = assetName+"-AIRDROP";
	BOOST_CHECK_NO_THROW(r = CallRPC("mainnet1", "assetinfo " + assetName, false));

	if (!find_value(r.get_obj(), "inputs").get_array().empty())
	{
		printf("This script does not support airdropping input range based assets *yet*\n");
		return;
	}

	// set up max supply of asset, if its infinite use total supply as the gauge for the airDropPct percentage airdrop
	CAmount nMaxSupply = AmountFromValue(find_value(r.get_obj(), "max_supply"));
	if(nMaxSupply <= 0)
		nMaxSupply = AmountFromValue(find_value(r.get_obj(), "total_supply"));

	// total supply being airdropped
	nMaxSupply *= airDropPct;

	// generate snapshot payments and let it mature
	// import private key of alias
	// drop shares on aliases
	int numberOfTxPerBlock = 250;
	int totalTx = 0;
	CAmount nTotalSent  =0;
	std::string sendManyString = "";
	for(int i =0;i<paymentAmounts.size();i++)
	{
		double fractionOwnershipInSys = paymentAmounts[i].amount / nTotal;
		CAmount nAmountToAirDrop = nMaxSupply * fractionOwnershipInSys;

		if(sendManyString != "")
			sendManyString += ",";
		//"\"[{\\\"aliasto\\\":\\\"jagassetallocationsend2\\\",\\\"amount\\\":0.1}]\""
		sendManyString += "\"[{\\\"aliasto\\\":\\\"" + paymentAmounts[i].alias + "\\\",\\\"amount\\\":" + ValueFromAmount(nAmountToAirDrop).write();
		nTotalSent += paymentAmounts[i].amount;
		totalTx++;
		if(i != 0 && (i%numberOfTxPerBlock) == 0)
		{
			printf("strSendAsset #%d, total %s, num txs %d\n", currentTx, ValueFromAmount(nTotalSent).write(), totalTx);
			SendSnapShotPayment(sendManyString);
			sendManyString = "";
			nTotalSent = 0;
		}
	}
	if(sendManyString != "")
	{
		printf("FINAL strSendAsset #%d, total %s, num txs %d\n", currentTx, ValueFromAmount(nTotalSent).write(), totalTx);
		SendSnapShotPayment(sendManyString);

	}

}
void GetUTXOs(std::vector<PaymentAmount> &paymentAmounts, CAmount& nTotal)
{
	int countTx = 0;
	int rejectTx = 0;
    UniValue tests = read_json(std::string(json_tests::utxo, json_tests::utxo + sizeof(json_tests::utxo)));
    for (unsigned int idx = 0; idx < tests.size(); idx++) {
        UniValue test = tests[idx];
        std::string strTest = test.write();
        if (test.size() < 2) // Allow for extra stuff (useful for comments)
        {
            BOOST_ERROR("Bad test: " << strTest);
            continue;
        }
		PaymentAmount payment;
        payment.address  = test[0].get_str();
		CAmount amountInSys1 = test[1].get_int64();
		// don't transfer less than 1 coin utxo's
		if(amountInSys1 <= COIN)
		{
			rejectTx++;
			continue;
		}
		nTotal += amountInSys1;
		BOOST_CHECK_NO_THROW(r = CallRPC("mainnet1", "validateaddress " + test[0].get_str(), false));
		BOOST_CHECK(find_value(r.get_obj(), "alias").get_str() !+ "");
		countTx++;
		payment.alias = find_value(r.get_obj(), "alias").get_str();
        payment.amount = amountInSys1;
		paymentAmounts.push_back(payment);
    }
	printf("Read %d total utxo sets, rejected %d, valid aliases %d, total amount %lld\n", rejectTx+countTx, rejectTx, countTx);
}
bool IsMainNetAlreadyCreated()
{
	int height;
	UniValue r;
	BOOST_CHECK_NO_THROW(r = CallRPC("mainnet1", "getinfo", false));
	height = find_value(r.get_obj(), "blocks").get_int();
	return height > 1;
}
BOOST_AUTO_TEST_CASE (generate_and_verify_snapshot)
{
	CAmount nTotal = 0;
	std::vector<PaymentAmount> paymentAmounts;
	GetUTXOs(paymentAmounts, nTotal);
	if(!IsMainNetAlreadyCreated())
	{
		GenerateAirDrop(paymentAmounts, nTotal);
	}
}
BOOST_AUTO_TEST_SUITE_END ()