//          Copyright Rein Halbersma 2010-2021.
// 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 <dctl/algo/search/ply.hpp>
#include <dctl/algo/search/score.hpp>
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_CASE, BOOST_CHECK, BOOST_CHECK_EQUAL, BOOST_CHECK_LT, BOOST_AUTO_TEST_SUITE_END

using namespace dctl::algo::search;

BOOST_AUTO_TEST_SUITE(SearchScore)

BOOST_AUTO_TEST_CASE(IsLoss)
{
        // minus infinity is not a loss
        BOOST_CHECK(!is_loss(-infinity()));

        // losses start one above minus infinity
        BOOST_CHECK_EQUAL(-infinity() + 1, loss_min());

        // scores in the interval [loss_min, loss_max) are losses
        for (auto v = loss_min(); v < loss_max(); ++v)
                BOOST_CHECK(is_loss(v));

        // all other scores in the [loss_max, +INF] interval are not losses
        for (auto v = loss_max(); v <= infinity(); ++v)
                BOOST_CHECK(!is_loss(v));
}

BOOST_AUTO_TEST_CASE(FiniteLoss)
{
        // scores in the interval [loss_min, loss_max) are finite
        for (auto v = loss_min(); v < loss_max(); ++v)
                BOOST_CHECK(is_finite(v));
}

BOOST_AUTO_TEST_CASE(NegativeLoss)
{
        // scores in the interval [loss_min, loss_max) are negative
        for (auto v = loss_min(); v < loss_max(); ++v)
                BOOST_CHECK_LT(v, 0);
}

BOOST_AUTO_TEST_CASE(IsWin)
{
        // infinity is not a win
        BOOST_CHECK(!is_win(infinity()));

        // wins start at one below infinity
        BOOST_CHECK_EQUAL(infinity() - 1, win_min());

        // scores in the interval (win_max, win_min] are wins
        for (auto v = win_min(); v > win_max(); --v)
                BOOST_CHECK(is_win(v));

        // all other scores in the interval [-INF, win_max] are not wins
        for (auto v = -infinity(); v <= win_max(); ++v)
                BOOST_CHECK(!is_win(v));
}

BOOST_AUTO_TEST_CASE(FiniteWin)
{
        // scores in the interval (win_max, win_min] are finite
        for (auto v = win_min(); v > win_max(); --v)
                BOOST_CHECK(is_finite(v));
}

BOOST_AUTO_TEST_CASE(PositiveWin)
{
        // scores in the interval (win_max, win_min] are positive
        for (auto v = win_min(); v > win_max(); --v)
                BOOST_CHECK_GT(v, 0);
}

BOOST_AUTO_TEST_CASE(WinEqualMinusLoss)
{
        for (auto i = 0; i < MAX_MATE_MOVES; ++i)
                BOOST_CHECK_EQUAL(win_value(i), -loss_value(i));
}

BOOST_AUTO_TEST_CASE(MinMaxDistance)
{
        BOOST_CHECK_EQUAL(loss_min(), loss_value(0));
        BOOST_CHECK_EQUAL(loss_max(), loss_value(MAX_MATE_MOVES));
        BOOST_CHECK_EQUAL( win_min(),  win_value(0));
        BOOST_CHECK_EQUAL( win_max(),  win_value(MAX_MATE_MOVES));
}

BOOST_AUTO_TEST_CASE(PlyValueInverse)
{
        for (auto i = 0; i < MAX_MATE_MOVES; ++i) {
                BOOST_CHECK_EQUAL(i, loss_ply(loss_value(i)));
                BOOST_CHECK_EQUAL(i,  win_ply( win_value(i)));
        }
}

BOOST_AUTO_TEST_CASE(Stretch)
{
        // loss and win values are "stretched" one step towards the edges of the [-INF, +INF] interval
        for (auto i = 1; i < MAX_MATE_MOVES; ++i) {
                BOOST_CHECK_EQUAL(loss_value(i - 1), stretch(loss_value(i)));
                BOOST_CHECK_EQUAL( win_value(i - 1), stretch( win_value(i)));
        }

        // heuristic scores are invariant
        for (auto v = loss_max(); v <= win_max(); ++v)
                BOOST_CHECK_EQUAL(v, stretch(v));

        // infinite scores are invariant
        BOOST_CHECK_EQUAL(-infinity(), stretch(loss_value(0)));
        BOOST_CHECK_EQUAL( infinity(), stretch( win_value(0)));
}

BOOST_AUTO_TEST_CASE(Squeeze)
{
        // loss and win values are "squeezed" one step towards the center of the [-INF, +INF] interval
        for (auto i = 0; i < MAX_MATE_MOVES; ++i) {
                BOOST_CHECK_EQUAL(loss_value(i + 1), squeeze(loss_value(i)));
                BOOST_CHECK_EQUAL( win_value(i + 1), squeeze( win_value(i)));
        }

        // heuristic scores are invariant
        for (auto v = loss_max(); v <= win_max(); ++v)
                BOOST_CHECK_EQUAL(v, squeeze(v));

        // infinite scores are invariant
        BOOST_CHECK_EQUAL(-infinity(), squeeze(-infinity()));
        BOOST_CHECK_EQUAL( infinity(), squeeze( infinity()));
}

BOOST_AUTO_TEST_SUITE_END()


#include "haptic_teleoperation/VirtualForceBrf.h"
#include <Eigen/Eigen>

VirtualForceBrf::VirtualForceBrf(ros::NodeHandle & n_):ForceField(n_)
{std::cout << "child constructor" << std::endl ; }

void  VirtualForceBrf::setGain( double g)
{
   gain = g ;
}

double  VirtualForceBrf::getGain(){ return  gain; }

Eigen::Vector3d VirtualForceBrf::getForcePoint (geometry_msgs::Point32 & c_current, Eigen::Vector3d robot_velocity ) {

    Eigen::Vector3d obsVector;
    obsVector(0) = c_current.x ;
    obsVector(1)=  c_current.y ;
    obsVector(2) = c_current.z ;

//    obsVector(0) =  c_current.x * cos(yaw) - c_current.y * sin(yaw)  + CurrentRobotPose(0)  ;
//    obsVector(1)=  c_current.x * sin(yaw) + c_current.y * cos(yaw) + CurrentRobotPose(1)  ;
//    obsVector(2) = c_current.z + CurrentRobotPose(2) ;

    double obsMag = sqrt(pow(c_current.x, 2) + pow(c_current.y , 2) + pow(c_current.z , 2)) ;

    Eigen::Vector3d obsVecNorm = obsVector / obsMag ;

double a_max = 1.0 ;

    // ******************* force in x ******************************* //
    double v_ix = robot_velocity(0) ;
    float dstopx = (v_ix * v_ix) / (2*a_max ) ;
    float dresx ;
    if ( v_ix <= 0 ) dresx = obsMag + dstopx ;

    else dresx = obsMag - dstopx ;

    double f_x ;
    if ( dresx <= 0 || (1+v_ix)/dresx >= 1/gain)
        f_x =  1.0;
    else if ( (1+v_ix)/dresx <= 0)
        f_x  =  0.0 ;
    else
    {
        //std::cout << "risk vector" << gain *(1+v_i)/ dresx  << std::endl ;
        f_x =  (getGain()*((1+v_ix)/ dresx));
    }
// ************************* force in y ****************************** //
//   double v_y = robot_velocity(0) ;
//   float dstop = (v_y * v_y) / (2*a_max ) ;
//   float dres ;
//    if ( v_y <= 0 ) dres = obsVector(1) + dstop ;
//    else dres = obsVector(1) - dstop ;

//    double f_y ;

//    if ( dres <= 0 || (1+v_y)/dres >= 1/gain)
//        f_y =   1.0;
//    else if ( (1+v_y)/dres <= 0)
//        f_y  = 0.0 ;
//    else
//    {
//        //std::cout << "risk vector" << gain *(1+v_y)/ dres  << std::endl ;
//        f_y =  (getGain()*(1+v_y)/ dres);
//    }

// double f_z = 0.0 ;
//    Eigen::Vector3d  forceMag;
//    forceMag(0) = f_x ;
//    forceMag(1)=  f_y; //c_current.y ;
//    forceMag(2) = f_z; //c_current.z;

    //forceMag << f_x , f_y , 0.0 ;
   // Eigen::Vector3d  f= -obsVecNorm *forceMag.norm() ;
    Eigen::Vector3d  f= -obsVecNorm * f_x;

//    std::cout << "f_x  " << f_x << "  F_y " << f_y << std::endl ;
 //   std::cout << "f_x  " << f(0) << "  F_y " << f(1) << std::endl ;

    return f ;

}


//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef CLOTHO_MARGIN_DETAILS_HPP_
#define CLOTHO_MARGIN_DETAILS_HPP_

#include <boost/dynamic_bitset.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace ac = boost::accumulators;

namespace clotho {
namespace genetics {

template < class SizeType >
class margin_details {
public:

    typedef SizeType size_type;
    typedef size_type * result_type;

    typedef boost::dynamic_bitset<>                                     row_subset_type;

    margin_details() :
        m_column_margin( NULL )
        , m_row_margin( NULL )
        , m_column_margin_size(0)
        , m_column_allocated_size(0)
        , m_row_margin_size(0)
        , m_row_allocated_size(0)
    {}

    void recordResults( boost::property_tree::ptree & log ) {

        typedef ac::accumulator_set< size_type, ac::stats< ac::tag::min, ac::tag::mean, ac::tag::max, ac::tag::variance, ac::tag::median, ac::tag::count > > accumulator_t;

        accumulator_t   col_accum, row_accum;

        boost::property_tree::ptree dist, lfreq;

        // maximum frequency for an allele is bounded by the number of sequences (rows) in the population
        std::vector< size_type > freq( m_indices.size(), 0 );

        // evaluate allele (column) statistics
        size_type i = 0;
        while( i < m_column_margin_size ) {
            size_type v = m_column_margin[i++];
            clotho::utility::add_value_array( dist, v );
            col_accum( v );
            freq[v]++;
        }

        clotho::utility::add_value_array(lfreq, freq.begin(), freq.end() );

        log.put_child( "distribution", dist );
        log.put_child( "frequency_distribution", lfreq );

        log.put( "stats.sequences_per_allele.min", ac::min( col_accum ) );
        log.put( "stats.sequences_per_allele.max", ac::max( col_accum ) );
        log.put( "stats.sequences_per_allele.mean", ac::mean( col_accum ) );
        log.put( "stats.sequences_per_allele.median", ac::median( col_accum ) );
        log.put( "stats.sequences_per_allele.variance", ac::variance( col_accum ) );
        log.put( "stats.sequences_per_allele.total", ac::count(col_accum) );

        // evaluate sequence (row) statistics
        i = 0;
        while( i < m_row_margin_size ) {
            if( m_indices.test(i) ) {
                row_accum( m_row_margin[ i ] );
            }
            ++i;
        }

        log.put( "stats.alleles_per_sequence.min", ac::min( row_accum ) );
        log.put( "stats.alleles_per_sequence.max", ac::max( row_accum ) );
        log.put( "stats.alleles_per_sequence.mean", ac::mean( row_accum ) );
        log.put( "stats.alleles_per_sequence.median", ac::median( row_accum ) );
        log.put( "stats.alleles_per_sequence.variance", ac::variance( row_accum ) );
        log.put( "stats.alleles_per_sequence.total", ac::count(row_accum) );
    }

    std::shared_ptr< std::vector< size_type > > getResults() {
        if( m_column_margin == NULL || m_column_margin_size == 0 ) {
            return std::shared_ptr< std::vector< size_type > >();
        }
        std::shared_ptr< std::vector< size_type > > t( new std::vector< size_type >( m_column_margin, m_column_margin + m_column_margin_size) );
        return t;
    }

    virtual ~margin_details() {
        if( m_column_margin != NULL ) {
            delete [] m_column_margin;
        }

        if( m_row_margin != NULL ) {
            delete [] m_row_margin;
        }
    }

protected:
/**
 *
 * Grows vectors if necessary
 * Always clears the count vectors to be all zeros
 *
 */
    void resize( size_type M, size_type N ) {

        if( M > m_column_allocated_size ) {
            if( m_column_margin != NULL ) {
                delete [] m_column_margin;
            }

            m_column_margin = new size_type[ M ];

            m_column_allocated_size = M;
        }

        m_column_margin_size = M;

        memset( m_column_margin, 0, sizeof(size_type) * m_column_allocated_size );

        m_indices.resize( N );
        if( N > m_row_allocated_size ) {
            if( m_row_margin != NULL ) {
                delete [] m_row_margin;
            }

            m_row_margin = new size_type[ N ];
            m_row_allocated_size = N;
        }

        m_row_margin_size = N;

        memset( m_row_margin, 0, sizeof(size_type) * m_row_allocated_size );
    }

    result_type         m_column_margin;
    result_type         m_row_margin;

    row_subset_type     m_indices;

    size_type              m_column_margin_size, m_column_allocated_size;
    size_type              m_row_margin_size, m_row_allocated_size;
};

}   // namespace genetics
}   // namespace clotho
#endif  // CLOTHO_MARGIN_DETAILS_HPP_


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

const std::vector<std::pair<int, int>> attack_offsets{
    {-1, -2},
    {-1, 2},
    {1, -2},
    {1, 2},
    {-2, -1},
    {-2, 1},
    {2, -1},
    {2, 1},
};

class EdgeAdder
{
  Graph &G;

public:
  explicit EdgeAdder(Graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity)
  {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void testcase()
{
  int n;
  std::cin >> n;
  assert(n >= 1 && n <= (1 << 6));

  const auto square_is_in_range = [n](int i, int j) { return i >= 0 && i < n && j >= 0 && j < n; };
  const auto square_is_white = [](int i, int j) { return (i + j) % 2 == 0; };

  std::vector<std::vector<bool>> is_present_by_field(n, std::vector<bool>(n));
  for (int i = 0; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      int raw;
      std::cin >> raw;
      assert(raw == 0 || raw == 1);
      is_present_by_field.at(i).at(j) = bool(raw);
    }
  }

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_sink = next_free_node++;
  const auto get_node_for_square = [next_free_node, n, square_is_in_range](int i, int j) {
    assert(square_is_in_range(i, j));
    return next_free_node + i * n + j;
  };
  next_free_node += n * n;
  const int num_nodes = next_free_node;

  Graph G(num_nodes);
  EdgeAdder adder(G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  for (int i = 0; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      if (!is_present_by_field.at(i).at(j))
      {
        continue;
      }

      if (square_is_white(i, j))
      {
        adder.add_edge(node_source, get_node_for_square(i, j), 1);
      }
      else
      {
        adder.add_edge(get_node_for_square(i, j), node_sink, 1);
        continue;
      }

      for (const auto offset : attack_offsets)
      {
        const int attack_i = i + offset.first;
        const int attack_j = j + offset.second;
        if (!square_is_in_range(attack_i, attack_j) || !is_present_by_field.at(attack_i).at(attack_j))
        {
          continue;
        }
        assert(!square_is_white(attack_i, attack_j));
        adder.add_edge(get_node_for_square(i, j), get_node_for_square(attack_i, attack_j), 1);
      }
    }
  }

  boost::push_relabel_max_flow(G, node_source, node_sink);

  std::vector<bool> visited_by_node(num_nodes, false);
  visited_by_node.at(node_source) = true;
  std::deque<int> queue{node_source};
  while (!queue.empty())
  {
    const int node = queue.front();
    queue.pop_front();
    for (auto it = boost::out_edges(node, G); it.first != it.second; it.first++)
    {
      const int next_node = boost::target(*it.first, G);
      if (rc_map[*it.first] > 0 && !visited_by_node.at(next_node))
      {
        visited_by_node.at(next_node) = true;
        queue.push_back(next_node);
      }
    }
  }

  int max_knights = 0;
  for (int i = 0; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      if (is_present_by_field.at(i).at(j) && square_is_white(i, j) == visited_by_node.at(get_node_for_square(i, j)))
      {
        max_knights++;
      }
    }
  }

  std::cout << max_knights << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include "exactjudge.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <string>
#include <complex>

namespace
{
    void validate(PASTNode astnode, ParsersHelper& ph, const std::string& fnname)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error(fnname+" should have exactly one argument");
        auto secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
          throw std::runtime_error("The argument of "+fnname+" must be Complex");
    }
}

namespace HT
{
    void isexact(PASTNode astnode, ParsersHelper& ph)
    {
        validate(astnode, ph, "exact?");
        auto w (boost::get<ComplexType>((*astnode->ch.rbegin())->token.info));
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        astnode->token.info = BooleanType( w.exact());
        astnode->remove();
    }
    void isinexact(PASTNode astnode, ParsersHelper& ph)
    {
        validate(astnode, ph, "inexact?");
        auto w (boost::get<ComplexType>((*astnode->ch.rbegin())->token.info));
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        astnode->token.info = BooleanType(!w.exact());
        astnode->remove();
    }

}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SQRS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQRS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing sqrs capabilities

    Computes the saturated square of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = sqrs(x);
    @endcode

    is similar to:

    @code
    T r = x*x > Valmax ? Valmax : x*x;
    @endcode

  **/
  const boost::dispatch::functor<tag::sqrs_> sqrs = {};
} }
#endif

#include <boost/simd/function/scalar/sqrs.hpp>
#include <boost/simd/function/simd/sqrs.hpp>

#endif


#include <cmath>

#include <iostream>
#include <random>
#include <thread>

#include "gflags/gflags.h"
#include "glog/logging.h"

#include "driver/engine.hpp"
//#include "lib/abstract_data_loader.hpp"
//#include "lib/labeled_sample.hpp"
//#include "lib/parser.hpp"
#include "app/logitstic_regression.hpp"

#include <boost/algorithm/string.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/irange.hpp>
#include <fstream>
#include <lib/svm_loader.hpp>
//#include "lib/svm_sample.hpp"

using namespace csci5570;

DEFINE_string(config_file, "", "The config file path");
DEFINE_string(my_id, "", "Local node id");
DEFINE_string(input, "", "The hdfs input url");

void get_nodes_from_config(std::string config_file, std::vector<Node>& nodes) {
  std::ifstream infile(config_file);
  std::string line;
  while (std::getline(infile, line)) {
    std::vector<std::string> cols;
    boost::split(cols, line, [](char c){return c == ':';});
    uint32_t node_id = std::stoi(cols[0]);
    std::string host_name = cols[1];
    boost::trim(host_name);
    int port = std::stoi(cols[2]);
    nodes.push_back(Node({node_id, host_name, port}));
  }
}

int main(int argc, char** argv) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = 0;
  FLAGS_colorlogtostderr = true;
  using DataStore = std::vector<lib::SVMSample>;
  DataStore data_store;

  LOG(INFO) << FLAGS_config_file;
  LOG(INFO) << FLAGS_my_id;
  LOG(INFO) << FLAGS_input;

  std::vector<Node> nodes;
  get_nodes_from_config(FLAGS_config_file, nodes);
  uint32_t my_id = std::stoi(FLAGS_my_id);
  auto node = std::find_if(nodes.begin(), nodes.end(), [my_id](Node& n){return n.id == my_id;});
  if (node == nodes.end()) {
    LOG(INFO) << "My_id not in nodes list.";
    return -1;
  }

  /*
   * Begin IO config
   */
//  std::string url = "hdfs:///datasets/classification/a9";
  std::string url = FLAGS_input;
  std::string hdfs_namenode = "proj10";                        // Do not change
  std::string master_host = "proj10";  // Set to worker name
  std::string worker_host = node->hostname;  // Set to worker name
  uint32_t id = node->id;
  int hdfs_namenode_port = 9000;
  int master_port = 19817;  // use a random port number to avoid collision with other users
  const uint32_t n_features = 100;
  /*
   * End IO config
   */

  lib::Parser<lib::SVMSample, DataStore> parser;
  lib::DataLoader<lib::SVMSample, DataStore> data_loader;
  data_loader.load(url, hdfs_namenode, master_host, worker_host, hdfs_namenode_port, master_port, n_features,
                                parser, &data_store, id, nodes.size());

//  lib::SVMSample sample;
//  sample.x_ = std::vector<std::pair<int, int>>({{0, 2}, {3, 1}});
//  sample.y_ = -1;
//  data_store.push_back(sample);
//  lib::SVMSample sample1;
//  sample1.x_ = std::vector<std::pair<int, int>>({{2, 2}, {3, 1}});
//  sample1.y_ = 1;
//  data_store.push_back(sample1);

  Engine engine(*node, nodes);

  // 1. Start system
  engine.StartEverything();

  // 1.1 Create table
  const auto kTableId = engine.CreateTable<double>(ModelType::ASP, StorageType::Map);  // table 0

  // 1.2 Load data
  engine.Barrier();

  // 2. Start training task
  MLTask task;
  std::vector<WorkerAlloc> worker_alloc;
  for (auto node : nodes) {
    worker_alloc.push_back({node.id, 1});  // node_id, worker_num
  }
  task.SetWorkerAlloc(worker_alloc);
  task.SetTables({kTableId});     // Use table 0
  task.SetLambda([kTableId, &data_store](const Info& info) {
    LOG(INFO) << info.DebugString();
    // algorithm helper
    LogisticRegression<double> lr(&data_store, 0.00001);
    // key for parameters
    std::vector<Key> keys;
    lr.get_keys(keys);
    LOG(INFO) << "parameter size: " << keys.size();

    KVClientTable<double> table = info.CreateKVClientTable<double>(kTableId);

    for (int i = 0; i < 10e2; ++i) {
      // parameters from server
      std::vector<double> theta;
      table.Get(keys, &theta);
      lr.update_theta(keys, theta);
      std::vector<double> grad;
      lr.compute_gradient(grad);
      table.Add(keys, grad);
      table.Clock();
      if(i % 5 == 0) {
        LOG(INFO) << "Current accuracy: " << lr.test_acc();
        LOG(INFO) << "Current loss: " << lr.get_loss();
      }
    }
    // print theta
    /*
    std::vector<double> theta;
    table.Get(keys, &theta);
    std::stringstream ss;
    for(auto t : theta) {
      ss << t;
      ss << " ";
    }
    LOG(INFO) << ss.str();
    */
    LOG(INFO) << "Task completed.";
  });

  engine.Run(task);

  // 3. Stop
  engine.StopEverything();
  return 0;
}


#include <assert.h>
#include <limits>

#include <Eigen/Core>

#include "MultiviewGeometry.h"
#include "Math.h"
#include "Stl.h"
#include "Conversion.h"
#include "Log.h"

namespace VISFS {

bool isFinite(const cv::Point3f & _point) {
    return uIsFinite(_point.x) && uIsFinite(_point.y) && uIsFinite(_point.z);
}

cv::Point3f transformPoint(const cv::Point3f & _points, const GeometricCamera & _model) {
    Eigen::Vector4d tempPoint = _model.getTansformImageToRobot() * Eigen::Vector4d(_points.x, _points.y, _points.z, 1.f);
    return cv::Point3f(tempPoint[0], tempPoint[1], tempPoint[2]);
}

cv::Point3f transformPoint(const cv::Point3f & _points, const Eigen::Isometry3d & _transform) {
    Eigen::Vector4d tempPoint = _transform * Eigen::Vector4d(_points.x, _points.y, _points.z, 1.f);
    return cv::Point3f(tempPoint[0], tempPoint[1], tempPoint[2]);
}

std::vector<float> computeReprojErrors(
	const std::vector<cv::Point3f> & _objectPoints,
	const std::vector<cv::Point2f> & _imagePoints,
	const cv::Mat & _cameraModel,
	const cv::Mat & _distCoeffs,
	const cv::Mat & _rvec,
	const cv::Mat & _tvec,
	const float _reProjErrorThreshold,
	std::vector<std::size_t> & _inliers) {
    assert(_objectPoints.size() == _imagePoints.size());
    std::size_t cnt = _objectPoints.size();
    std::vector<cv::Point2f> projPoints;
    cv::projectPoints(_objectPoints, _rvec, _tvec, _cameraModel, _distCoeffs, projPoints);

    _inliers.resize(cnt, 0);
    std::vector<float> error(cnt);
    std::size_t oi = 0;
    for (std::size_t i = 0; i < cnt; ++i) {
        float e = static_cast<float>(cv::norm(_imagePoints[i] - projPoints[i]));
        if (e <= _reProjErrorThreshold) {
            _inliers[oi] = i;
            error[oi++] = e;
        }
    }
    _inliers.resize(oi);
    error.resize(oi);
    return error;
}

std::vector<cv::Point3f> generateKeyPoints3DStereo(const std::vector<cv::KeyPoint> & _kptsLeft, const std::vector<cv::KeyPoint> & _kptsRight_, const GeometricCamera & _cameraLeft, const GeometricCamera & _cameraRight, float _minDepth, float _maxDepth) {
    assert(_kptsLeft.size() == _kptsRight_.size());

    std::vector<cv::Point3f> kpts3D;
    kpts3D.resize(_kptsLeft.size());
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    for (std::size_t i = 0; i < _kptsLeft.size(); ++i) {
        cv::Point3f point(badPoint, badPoint, badPoint);
        float disparity = _kptsLeft[i].pt.x - _kptsRight_[i].pt.x;
        if (disparity != 0.f) {
            cv::Point3f tempPoint = projectDisparityTo3D(_kptsLeft[i].pt, disparity, _cameraLeft, _cameraRight);
            if (isFinite(tempPoint) && (_minDepth < 0.f || tempPoint.z > _minDepth) && (_maxDepth <= 0.f || tempPoint.z <= _maxDepth)) {
                point = tempPoint;  // point in camera coordinate system.
                point = transformPoint(point, _cameraLeft);
            }
        }
        kpts3D.at(i) = point;
    }
    return kpts3D;
}

cv::Point3f projectDisparityTo3D(const cv::Point2f & _corner, float _disparity, const GeometricCamera & _cameraLeft, const GeometricCamera & _cameraRight) {
    cv::Mat leftK = _cameraLeft.cvKfloat();
    cv::Mat rightK = _cameraRight.cvKfloat();
    float baseLine = _cameraLeft.getBaseLine();
    if (_disparity > 0.f && baseLine > 0.f && leftK.at<float>(0, 0) > 0.f) {
        float c = 0.f;
        if (leftK.at<float>(0, 2) > 0.f && rightK.at<float>(0, 2) > 0.f) {
            c = rightK.at<float>(0, 2) - leftK.at<float>(0, 2);
        }
        float W = baseLine/(_disparity + c);
        return cv::Point3f((_corner.x - leftK.at<float>(0, 2))*W, (_corner.y - leftK.at<float>(1, 2))*W, leftK.at<float>(0, 0)*W);
    }
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    return cv::Point3f(badPoint, badPoint, badPoint);
}

Eigen::Isometry3d estimateMotion3DTo2D(
    const std::map<std::size_t, cv::Point3f> & _words3dFrom,
    const std::map<std::size_t, cv::KeyPoint> & _words2dTo,
    const GeometricCamera & _cameraModel,
    int _minInliers,
    int _iterations,
    double _reProjError,
    int _flagPnP,
    int _refineIterations,
    const std::map<std::size_t, cv::Point3f> & _words3dTo,
    cv::Mat & _covariance,
    std::vector<std::size_t> & _matchesOut,
    std::vector<std::size_t> & _inliersOut,
	const Eigen::Isometry3d & _guess) {
    Eigen::Isometry3d transform(Eigen::Matrix4d::Zero());
    std::vector<std::size_t> matches, inliers;
    _covariance = cv::Mat::eye(6, 6, CV_64FC1);

    // find correspondences
    std::vector<size_t> ids = uKeys(_words2dTo);
    std::vector<cv::Point3f> objectPoints(ids.size());
    std::vector<cv::Point2f> imagePoints(ids.size());
    std::size_t oi = 0;
    matches.resize(ids.size());
    for (std::size_t i = 0; i < ids.size(); ++i) {
        std::map<std::size_t, cv::Point3f>::const_iterator iter = _words3dFrom.find(ids[i]);
        if (iter != _words3dFrom.end() && isFinite(iter->second)) {
            const cv::Point3f & pt = iter->second;
            objectPoints[oi] = pt;
            imagePoints[oi] = _words2dTo.find(ids[i])->second.pt;
            matches[oi++] = ids[i];
        }
    }
    objectPoints.resize(oi);
    imagePoints.resize(oi);
    matches.resize(oi);

    if (static_cast<int>(matches.size()) >= _minInliers) {
        cv::Mat K = _cameraModel.cvKdouble();
        cv::Mat D = _cameraModel.cvDdouble();
        Eigen::Isometry3d guessCameraFrame = (_guess * _cameraModel.getTansformImageToRobot()).inverse();
		cv::Mat R = (cv::Mat_<double>(3,3) <<
				guessCameraFrame(0, 0), guessCameraFrame(0, 1), guessCameraFrame(0, 2),
				guessCameraFrame(1, 0), guessCameraFrame(1, 1), guessCameraFrame(1, 2),
				guessCameraFrame(2, 0), guessCameraFrame(2, 1), guessCameraFrame(2, 2));
		cv::Mat rvec(1,3, CV_64FC1);
		cv::Rodrigues(R, rvec);
        Eigen::Vector3d eigent = guessCameraFrame.translation();
        cv::Mat tvec = (cv::Mat_<double>(1,3) << eigent.x(), eigent.y(), eigent.z());

        // Calculate
        VISFS::solvePnPRansac(objectPoints, imagePoints, K, D, rvec, tvec, false, _iterations, _reProjError, _minInliers, inliers, _flagPnP, _refineIterations);

        if (static_cast<int>(inliers.size()) >= _minInliers) {
            cv::Rodrigues(rvec, R);
            Eigen::Matrix3d rotation;
            rotation << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
						R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
						R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2);
            Eigen::Vector3d translate(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
            Eigen::Isometry3d pnp = Eigen::Isometry3d::Identity();
            pnp.prerotate(rotation);
            pnp.pretranslate(translate);
            transform = (_cameraModel.getTansformImageToRobot() * pnp).inverse();

            // compute variance (like in PCL computeVariance() method of sac_model.h)
            if (_words3dTo.size()) {
                std::vector<float> errSqrdDists(inliers.size());
                std::vector<float> errSqrdAngles(inliers.size());
                oi = 0;
                for (std::size_t i = 0; i < inliers.size(); ++i) {
                    std::map<std::size_t, cv::Point3f>::const_iterator iter = _words3dTo.find(matches[inliers[i]]);
                    if (iter != _words3dTo.end() && isFinite(iter->second)) {
                        const cv::Point3f & objPoint = objectPoints[inliers[i]];
                        cv::Point3f newPoint = transformPoint(iter->second, transform);
                        errSqrdDists[oi] = uNormSquared(objPoint.x-newPoint.x, objPoint.y-newPoint.y, objPoint.z-newPoint.z);

                        Eigen::Vector4f v1(objPoint.x-transform.translation().x(), objPoint.y-transform.translation().y(), objPoint.z-transform.translation().z(), 0);
                        Eigen::Vector4f v2(newPoint.x-transform.translation().x(), newPoint.y-transform.translation().y(), newPoint.z-transform.translation().z(), 0);
                        errSqrdAngles[oi++] = getAngle3D(v1, v2);
                    }
                }
                errSqrdDists.resize(oi);
                errSqrdAngles.resize(oi);

                if (errSqrdDists.size()) {
                    std::sort(errSqrdDists.begin(), errSqrdDists.end());
                    double medianErrSqr = 2.1981 * static_cast<double>(errSqrdDists[errSqrdDists.size() >> 1]);
                    assert(uIsFinite(medianErrSqr));
                    _covariance(cv::Range(0, 3), cv::Range(0, 3)) *= medianErrSqr;
                    std::sort(errSqrdAngles.begin(), errSqrdAngles.end());
                    medianErrSqr = 2.1981 * static_cast<double>(errSqrdAngles[errSqrdAngles.size() >> 1]);
                    assert(uIsFinite(medianErrSqr));
                    _covariance(cv::Range(3, 6), cv::Range(3, 6)) *= medianErrSqr;
                } else {
                    LOG_ERROR << "Not enough close points to compute covariance!";
                }

                if (static_cast<float>(oi) / static_cast<float>(inliers.size()) < 0.2f) {
                    LOG_WARN << "A very low number of inliers have valid depth " << oi << " / " << inliers.size() << " , the transform returned may be wrong!";
                }
            } else {
                // compute variance, which is the rms of reprojection errors
                std::vector<cv::Point2f> imagePointsReproj;
                cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
                float err = 0.f;
                for (std::size_t i = 0; i < inliers.size(); ++i) {
                    err += uNormSquared(imagePoints.at(inliers[i]).x-imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y-imagePointsReproj.at(inliers[i]).y);
                }
                assert(uIsFinite(err));
                _covariance *= std::sqrt(err/static_cast<float>(inliers.size()));
            }
        }
    }

    _matchesOut = matches;
    _inliersOut.resize(inliers.size());
    for (std::size_t i = 0; i < inliers.size(); ++i) {
        _inliersOut.at(i) = matches[inliers[i]];
    }

    return transform;
}

void solvePnPRansac(
	const std::vector<cv::Point3f> & _objectPoints,
	const std::vector<cv::Point2f> & _imagePoints,
	const cv::Mat & _cameraMatrix,
	const cv::Mat & _distCoeffs,
	cv::Mat & _rvec,
	cv::Mat & _tvec,
	bool _useExtrinsicGuess,
	int _iterationsCount,
	float _reprojectionError,
	int _minInliersCount,
	std::vector<std::size_t> & _inliers,
	int _flags,
	int _refineIterations,
	float _refineSigma) {
    if (_minInliersCount < 4) {
        _minInliersCount = 4;
    }
    std::vector<int> inliersInt;
    cv::solvePnPRansac(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec,
                        _useExtrinsicGuess, _iterationsCount, _reprojectionError, 0.99, inliersInt, _flags);
    float inlierThreshold = _reprojectionError;
    if (static_cast<int>(inliersInt.size()) >= _minInliersCount && _refineIterations > 0) {
        float errorThreshold = inlierThreshold;
        int refineIterations = 0;
        bool inlierChanged = false, oscillating = false;
        _inliers = uIntVector2Ul(inliersInt);
        std::vector<std::size_t> newInliers, prevInliers = _inliers, inliersSizes;
        cv::Mat newModelRvec = _rvec;
        cv::Mat newModelTvec = _tvec;

        do {
            // Get inliers from current model.
            std::vector<cv::Point3f> oPointsInliers(prevInliers.size());
            std::vector<cv::Point2f> iPointsInliers(prevInliers.size());
            for (std::size_t i = 0; i < prevInliers.size(); ++i) {
                oPointsInliers[i] = _objectPoints[prevInliers[i]];
                iPointsInliers[i] = _imagePoints[prevInliers[i]];
            }
            // Optimize the model coefficients.
            cv::solvePnP(oPointsInliers, iPointsInliers, _cameraMatrix, _distCoeffs, newModelRvec, newModelTvec, true, _flags);
            inliersSizes.push_back(prevInliers.size());

            // Select the new inliers based on the optimized coefficients and new threshold.
            std::vector<float> error = computeReprojErrors(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, newModelRvec, newModelTvec, errorThreshold, newInliers);
            if (static_cast<int>(newInliers.size()) < _minInliersCount) {
                ++refineIterations;
                if (refineIterations >= _refineIterations) {
                    break;
                }
                continue;
            }

            // Estimate the variance and the new threshold.
            float mean = uMean(error);
            float variance = uVariance(error, mean);
            errorThreshold = std::min(inlierThreshold, _refineSigma*sqrt(variance));

            inlierChanged = false;
            std::swap(prevInliers, newInliers);
            // If the number of inliers changed, then we are still optimizing.
            if (newInliers.size() != prevInliers.size()) {
                // Check if the number of inliers is oscillating in between two values
                if (static_cast<int>(inliersSizes.size()) >= _minInliersCount) {
                    if (inliersSizes[inliersSizes.size()-1] == inliersSizes[inliersSizes.size()-3] &&
                        inliersSizes[inliersSizes.size()-2] == inliersSizes[inliersSizes.size()-4]) {
                        oscillating = true;
                        break;
                    }
                }
                inlierChanged = true;
                continue;
            }
            // Check the value of the inlier set.
            for (std::size_t i = 0; i < prevInliers.size(); ++i) {
                // If the value of the inliers changed, then we are still optimizing
                if (prevInliers[i] != newInliers[i]) {
                    inlierChanged = true;
                    break;
                }
            }
        } while (inlierChanged && ++refineIterations < _refineIterations);

        // If the new set of inliers is empty, we didn't do a good job refineing.
        if (static_cast<int>(prevInliers.size()) < _minInliersCount) {
            LOG_ERROR << "RANSAC refineModel: Refinement failed: got very low inliers " << prevInliers.size() << ".";
        }
        if (oscillating) {
            LOG_WARN << "RANSAC refineModel: Detected oscillations in the model refinement.";
        }

        std::swap(_inliers, newInliers);
        _rvec = newModelRvec;
        _tvec = newModelTvec;
    }

}

cv::Mat computeCovariance(
	const std::map<std::size_t, cv::Point3f> & _pointsInCoor1,
	const std::map<std::size_t, cv::Point3f> & _pointsInCoor2,
	const Eigen::Isometry3d & _transformCoor2ToCoor1,
	const std::vector<std::size_t> & _inliers) {
        cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
    if (_inliers.empty()) {
        LOG_ERROR << "The corresponding index is empty. return huge covariance.";
        covariance *= 9999.0;
        return covariance;
    } else if (_pointsInCoor1.empty() || _pointsInCoor2.empty()) {
        LOG_ERROR << "The input points are empty. return huge covariance.";
        covariance *= 9999.0;
        return covariance;
    } else {
        std::vector<float> errSqrdDists(_inliers.size());
        std::vector<float> errSqrdAngles(_inliers.size());
        std::size_t oi = 0;
        for (std::size_t i = 0; i < _inliers.size(); ++i) {
            std::map<std::size_t, cv::Point3f>::const_iterator iter1 = _pointsInCoor1.find(_inliers[i]);
            std::map<std::size_t, cv::Point3f>::const_iterator iter2 = _pointsInCoor2.find(_inliers[i]);
            if (iter1 != _pointsInCoor1.end() && isFinite(iter1->second) && iter2 != _pointsInCoor2.end() && isFinite(iter2->second)) {
                const cv::Point3f & ptCoor1 = iter1->second;
                const cv::Point3f & ptCoor2Proj = transformPoint(iter2->second, _transformCoor2ToCoor1);
                errSqrdDists[oi] = uNormSquared(ptCoor1.x - ptCoor2Proj.x, ptCoor1.y - ptCoor2Proj.y, ptCoor1.z- ptCoor2Proj.z);

                auto translationCoor2TtoCoor1 = _transformCoor2ToCoor1.translation();
                Eigen::Vector4f v1(ptCoor1.x- translationCoor2TtoCoor1.x(), ptCoor1.y - translationCoor2TtoCoor1.y(), ptCoor1.z - translationCoor2TtoCoor1.z(), 0);
                Eigen::Vector4f v2(ptCoor2Proj.x - translationCoor2TtoCoor1.x(), ptCoor2Proj.y - translationCoor2TtoCoor1.y(), ptCoor2Proj.z - translationCoor2TtoCoor1.z(), 0);
                errSqrdAngles[oi++] = getAngle3D(v1, v2);
            }
        }
        errSqrdDists.resize(oi);
        errSqrdAngles.resize(oi);

        if (errSqrdDists.size()) {
            std::sort(errSqrdDists.begin(), errSqrdDists.end());
            double medianErrSqr = 2.1981 * static_cast<double>(errSqrdDists[errSqrdDists.size() >> 1]);
            assert(uIsFinite(medianErrSqr));
            covariance(cv::Range(0, 3), cv::Range(0, 3)) *= medianErrSqr;
            std::sort(errSqrdAngles.begin(), errSqrdAngles.end());
            medianErrSqr = 2.1981 * static_cast<double>(errSqrdAngles[errSqrdAngles.size() >> 1]);
            assert(uIsFinite(medianErrSqr));
            covariance(cv::Range(3, 6), cv::Range(3, 6)) *= medianErrSqr;
        } else {
            LOG_WARN << "Not enough close points to compute covariance!";
        }

        if (static_cast<float>(oi) / static_cast<float>(_inliers.size()) < 0.2f) {
            LOG_WARN << "A very low number of inliers have valid depth " << oi << " / " << _inliers.size() << " , the transform returned may be wrong!";
        }
    }
}

}   // namespace


#include "CartographicMapping.hpp"
#include <proj_api.h>
#include <boost/assign/list_of.hpp>
#include <boost/algorithm/string.hpp>
#include <base-logging/Logging.hpp>

namespace templ {
namespace utils {

const double CartographicMapping::RADIUS_MOON_IN_M = 1737.1E03;
const double CartographicMapping::RADIUS_EARTH_IN_M = 6371E03;

std::map<CartographicMapping::Type, std::string> CartographicMapping::TypeTxt = boost::assign::map_list_of
    (UNKNOWN, "UNKNOWN")
    (EARTH, "EARTH")
    (MOON, "MOON")
    ;

CartographicMapping::CartographicMapping(const std::string& radiusTypeName)
{
    mType = getTypeByName(radiusTypeName);
}

CartographicMapping::CartographicMapping(Type type)
    : mType(type)
{
}

base::Point CartographicMapping::latitudeLongitudeToMetric(const base::Point& point) const
{
    projPJ pj_merc, pj_latlong;

    switch(mType)
    {
        case MOON:
        {
            {
                std::stringstream ss;
                ss << "+proj=merc +ellps=sphere";
                ss << " +a=" << RADIUS_MOON_IN_M;
                ss << " +b=" << RADIUS_MOON_IN_M;
                ss << " +units=m";
                ss << " +lat_ts=" << point.x();
                if (!(pj_merc = pj_init_plus(ss.str().c_str())) )
                {
                    throw std::runtime_error("templ::utils::CartographMapping::latitudeLongitudeToMetric: "
                            " could not init mercator projection for moon");
                }
            }
            {
                std::stringstream ss;
                ss << "+proj=latlong +ellps=sphere";
                ss << " +a=" << RADIUS_MOON_IN_M;
                ss << " +b=" << RADIUS_MOON_IN_M;
                ss << " +units=m";
                if (!(pj_latlong = pj_init_plus(ss.str().c_str())) )
                {
                    throw std::runtime_error("templ::utils::CartographMapping::latitudeLongitudeToMetric: "
                            " could not init latitude/longitude projection for moon");
                }
            }
            break;
        }
        case EARTH:
        case UNKNOWN:
        {
            if (!(pj_merc = pj_init_plus("+proj=merc +ellps=clrk66 +lat_ts=33")) )
            {
                throw std::runtime_error("templ::utils::CartographMapping::latitudeLongitudeToMetric: "
                        " could not init mercator projection");
            }
            if (!(pj_latlong = pj_init_plus("+proj=latlong +ellps=clrk66")) )
            {
                throw std::runtime_error("templ::utils::CartographMapping::latitudeLongitudeToMetric: "
                        " could not init latitude/longitude projection");
            }
        }
        break;
    }

    double x = point.x();
    double y = point.y();

    x *= DEG_TO_RAD;
    y *= DEG_TO_RAD;

    pj_transform(pj_latlong, pj_merc, 1, 1, &x, &y, NULL);
    return base::Point(x,y,0.0);
}

CartographicMapping::Type CartographicMapping::getTypeByName(const std::string& _name)
{
    std::string name = _name;
    boost::to_upper(name);

    std::map<CartographicMapping::Type, std::string>::const_iterator cit = CartographicMapping::TypeTxt.begin();
    for(; cit != CartographicMapping::TypeTxt.end(); ++cit)
    {
        if(cit->second == name)
        {
            return cit->first;
        }
    }

    throw std::invalid_argument("templ::utils::CartographMapping::getTypeByName: could not find radius type named: '" + _name + "'");
}

} // end namespace utils
} // end namespace templ


#include <boost/math/distributions.hpp>
#include <vector>
#include <iostream>
#include <limits>
#include <fstream>
#include <algorithm>
#include <random>

#include "strategies.hpp"

using namespace marketsim;

class simplebuy: public tsimplestrategy
{
public:
    simplebuy(const std::string& name,tvolume delta, ttime timeinterval) :
        tsimplestrategy(name, timeinterval,false),
        fdelta(delta)
    {}
private:
    virtual tsimpleorderprofile simpleevent(const tmarketinfo& ,
                                     const ttradinghistory& )
    {
        return buymarket(fdelta);
    }
    tvolume fdelta;
};

class trendist: public tsimplestrategy
{
public:
    trendist(const std::string& name, tvolume delta, ttime timeinterval)
        : tsimplestrategy(name, timeinterval),  fdelta(delta)
    {}
private:
    virtual tsimpleorderprofile simpleevent(const tmarketinfo& mi,
                                     const ttradinghistory& )
    {
        if(mi.t > 2*timeinterval())
        {
            double p0 = mi.history.p(mi.t);
            double p1 = mi.history.p(mi.t-timeinterval());
            if(!isnan(p0) && !isnan(p1) && fabs(p0-p1) > std::numeric_limits<double>::epsilon())
                return p0 > p1 ? buymarket(fdelta) : sellmarket(fdelta);
        }
        return noorder();
    }
    tvolume fdelta;
};

/// \p evaluate: true if the strategies are to be run several times

void example(bool evaluate)
{
    // orpp::sys::seed(788897);

    /// strategy lt buys/sells on aveage 1 stocks ecach second on average
    tliquiditytaker lt("lt", 1, 1);

    /// maket maker starts with price 500 and his target inventory is 5000,
    /// the bid and ask size is 1
    tstollmarketmaker mm("mm", 500, 5000, 1,0.01);
    zistrategy zi("zi", 10,0.5,1,100);
    trendist tr("trendist", 1,1);
    simplebuy sb("simplebuy",1,0.1);

    /// MS changed 100 to 10
    tsimulation::addstrategy(mm, {100000,5000});
    tsimulation::addstrategy(lt, {100000,5000});

    tsimulation::addstrategy(zi, {100000,5000});
    tsimulation::addstrategy(sb, {1000,10});
    tsimulation::setlogging(true);

    //        tsimulation::setlogging(true);
    //doba simulacie

    if(evaluate)
    {
        auto r= tsimulation::evaluate(100,10);
        for(unsigned i=0; i<tsimulation::numstrategies(); i++)
            orpp::sys::log() << "Stragegy " << tsimulation::results().tradinghistory[i].name()
                  << ": " << r[i].meanvalue << ", sterr: " << r[i].standarderr << std::endl;
    }
    else
    {
        tsimulation::run(100);
        auto results = tsimulation::results();

        for(auto& r: results.tradinghistory)
        {
            r.output(orpp::sys::log(),ttradinghistory::ewallet);// etrades);
            orpp::sys::log() << "Value: "
                  << r.wallet().money()
                        + results.history.p(std::numeric_limits<double>::max()) *  r.wallet().stocks() << std::endl;
        }

        results.history.output(orpp::sys::log(),1);
    }
}

int main()
{
    try
    {
        example(false);
        example(true);
        return 0;
    }
    catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    catch (const char* m)
    {
           std::cerr << m << std::endl;
           return 1;
    }
    catch (const std::string& m)
    {
           std::cerr << m << std::endl;
           return 1;
    }
    catch(...)
    {
        std::cerr << "unknown exception" << std::endl;
        return 1;
    }

    return 0;
}


/*
 *  Created on: Aug, 2015
 *      Author: Thomas Faeulhammer
 *
 */

#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>

namespace po = boost::program_options;

Eigen::Matrix4f readMatrixFromFile(const std::string &file, int padding = 0);
Eigen::Matrix4f RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector3f &trans);
Eigen::Matrix4f RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans);

Eigen::Matrix4f readMatrixFromFile(const std::string &file, int padding) {
  // check if file exists
  boost::filesystem::path path = file;
  if (!(boost::filesystem::exists(path) && boost::filesystem::is_regular_file(path)))
    throw std::runtime_error("Given file path to read Matrix does not exist!");

  std::ifstream in(file.c_str(), std::ifstream::in);

  char linebuf[1024];
  in.getline(linebuf, 1024);
  std::string line(linebuf);
  std::vector<std::string> strs_2;
  boost::split(strs_2, line, boost::is_any_of(" "));

  Eigen::Matrix4f matrix;
  for (int i = 0; i < 16; i++)
    matrix(i / 4, i % 4) = static_cast<float>(atof(strs_2[padding + i].c_str()));

  return matrix;
}

Eigen::Matrix4f RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans) {
  Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
  tf.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
  tf.block<4, 1>(0, 3) = trans;
  return tf;
}

int main(int argc, char **argv) {
  typedef pcl::PointXYZRGB PointT;

  std::string path_s, path_p, path_m;
  po::options_description desc(
      "Visualize scene with object hypotheses in a given pose\n======================================\n**Required "
      "options");
  desc.add_options()("help,h", "produce help message")("scene_path,s", po::value<std::string>(&path_s)->required(),
                                                       "Filename of the point cloud of the scene")(
      "model_path,m", po::value<std::string>(&path_m)->required(),
      "Filename of the point cloud of the model (e.g. 3D_model.pcd)")(
      "model_pose,p", po::value<std::string>(&path_p)->required(),
      "Filename of the 4x4 object pose stored in row-major order");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return false;
  }
  try {
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
    return false;
  }

  pcl::visualization::PCLVisualizer vis;

  pcl::PointCloud<PointT>::Ptr scene_cloud(new pcl::PointCloud<PointT>);
  pcl::io::loadPCDFile(path_s, *scene_cloud);

  pcl::PointCloud<PointT>::Ptr model_cloud(new pcl::PointCloud<PointT>);
  pcl::io::loadPCDFile(path_m, *model_cloud);

  const Eigen::Matrix4f transform = readMatrixFromFile(path_p);
  const Eigen::Matrix4f scene_tf = RotTrans2Mat4f(scene_cloud->sensor_orientation_, scene_cloud->sensor_origin_);
  pcl::transformPointCloud(*model_cloud, *model_cloud, scene_tf * transform);

  vis.addPointCloud(scene_cloud, "original_cloud");
  vis.addPointCloud(model_cloud, "model_cloud");
  vis.spin();
  return true;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file crossassetstateprocess.hpp
    \brief crossasset model state process
    \ingroup crossassetmodel
    \ingroup processes
*/

#ifndef quantext_crossasset_stateprocess_hpp
#define quantext_crossasset_stateprocess_hpp

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/stochasticprocess.hpp>

#include <boost/unordered_map.hpp>

namespace QuantExt {
using namespace QuantLib;

class CrossAssetModel;

//! Cross Asset Model State Process
/*! \ingroup crossassetmodel
 \ingroup processes
 */
class CrossAssetStateProcess : public StochasticProcess {
public:
    enum discretization { exact, euler };

    CrossAssetStateProcess(const CrossAssetModel* const model, discretization disc,
                           SalvagingAlgorithm::Type salvaging = SalvagingAlgorithm::Spectral);

    /*! StochasticProcess interface */
    Size size() const;
    Disposable<Array> initialValues() const;
    Disposable<Array> drift(Time t, const Array& x) const;
    Disposable<Matrix> diffusion(Time t, const Array& x) const;

    /*! specific members */
    void flushCache() const;

protected:
    virtual Disposable<Matrix> diffusionImpl(Time t, const Array& x) const;

    const CrossAssetModel* const model_;
    SalvagingAlgorithm::Type salvaging_;

    class ExactDiscretization : public StochasticProcess::discretization {
    public:
        ExactDiscretization(const CrossAssetModel* const model,
                            SalvagingAlgorithm::Type salvaging = SalvagingAlgorithm::Spectral);
        virtual Disposable<Array> drift(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;
        virtual Disposable<Matrix> diffusion(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;
        virtual Disposable<Matrix> covariance(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;
        void flushCache() const;

    protected:
        virtual Disposable<Array> driftImpl1(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;
        virtual Disposable<Array> driftImpl2(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;
        virtual Disposable<Matrix> covarianceImpl(const StochasticProcess&, Time t0, const Array& x0, Time dt) const;

        const CrossAssetModel* const model_;
        SalvagingAlgorithm::Type salvaging_;

        // cache for exact discretization
        struct cache_key {
            double t0, dt;
            bool operator==(const cache_key& o) const { return (t0 == o.t0) && (dt == o.dt); }
        };

        struct cache_hasher : std::unary_function<cache_key, std::size_t> {
            std::size_t operator()(cache_key const& x) const {
                std::size_t seed = 0;
                boost::hash_combine(seed, x.t0);
                boost::hash_combine(seed, x.dt);
                return seed;
            }
        };
        mutable boost::unordered_map<cache_key, Array, cache_hasher> cache_m_;
        mutable boost::unordered_map<cache_key, Matrix, cache_hasher> cache_v_, cache_d_;
    }; // ExactDiscretization

    // cache for process drift and diffusion (e.g. used in Euler discretization)
    struct cache_hasher : std::unary_function<double, std::size_t> {
        std::size_t operator()(double const& x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x);
            return seed;
        }
    };

    mutable boost::unordered_map<double, Array, cache_hasher> cache_m_;
    mutable boost::unordered_map<double, Matrix, cache_hasher> cache_v_, cache_d_;
}; // CrossAssetStateProcess

} // namespace QuantExt

#endif


/**
 *  @file    Performances.hpp
 *  @brief   Check performances for different methods.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef PERFORMANCES_H
#define PERFORMANCES_H

#include <vector>
#include <set>
#include <array>
#include <algorithm>
#include <iterator>
#include <Eigen/SparseCore>
#include "spdlog/spdlog.h"

namespace bench {

namespace performances {

const unsigned int n = 10000;
const Eigen::VectorXd u = Eigen::VectorXd::Random(n + 1);

/**
* Computes a simple looped difference.
* @return The computed vector.
*/
Eigen::VectorXd loop();
/**
* Computes a vectorized looped difference.
* @return The computed vector.
*/
Eigen::VectorXd vectorized_loop();
/**
*  Initialize vector: left boundary of a rectangular grid
*
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The vector containing the grid nodes belonging to the left boundary.
*/
std::vector<int> left_v(int nx, int ny);
/**
*  Initialize vector: bottom boundary of a rectangular grid
*
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The vector containing the grid nodes belonging to the bottom
*  boundary.
*/
std::vector<int> bottom_v(int nx, int ny);
/**
*  Intersection of vectors: bottom left corner
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return the bottom-left corner (the node at the intersection bewteen the
*  left and bottom lines)
*/
int bottom_left_corner_v(int nx, int ny);
/**
*  Same as above but with a different algorithm.
*/
int bottom_left_corner_v_1(int nx, int ny);
/**
*  Union of vectors: bottom and left lines.
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return the nodes at the union bewteen the left and bottom lines.
*/
std::vector<int> union_v(int nx, int ny);
/**
*  Initialize set: left boundary of a rectangular grid
*
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The vector containing the grid nodes belonging to the left boundary.
*/
std::set<int> left_s(int nx, int ny);
/**
*  Initialize set: bottom boundary of a rectangular grid
*
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The vector containing the grid nodes belonging to the bottom
*  boundary.
*/
std::set<int> bottom_s(int nx, int ny);
/**
*  Intersection of sets: bottom left corner
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return the bottom-left corner (the node at the intersection bewteen the
*  left and bottom lines)
*/
int bottom_left_corner_s(int nx, int ny);
/**
*  Union of sets: bottom and left lines.
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return the nodes at the union bewteen the left and bottom lines.
*/
std::vector<int> union_s(int nx, int ny);
/**
*  Change value of a vector u at given indices with a set.
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The new vector.
*/
Eigen::VectorXd uleft_s(int nx, int ny);

/**
*  Change value of a vector u at given indices with a vector.
*  @param nx The number of division along the x-axis.
*  @param ny The number of dicision along the y-axis.
*  @return The new vector.
*/
Eigen::VectorXd uleft_v(int nx, int ny);

}  // namespace performances
}  // namespace bench

#endif  // PERFORMANCES_H


#define BOOST_TEST_MODULE graphical_model_test

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <vigra/multi_array.hxx>

#include "pgmlink/feature.h"
#include "pgmlink/pgm.h"
#include "pgmlink/traxels.h"
#include "pgmlink/event.h"

using namespace pgmlink;
using namespace std;
using namespace boost;
using namespace vigra;
using namespace opengm;

BOOST_AUTO_TEST_CASE( ogm_model_building )
{
   pgm::OpengmModel gm;
   gm.addVariable(2);
   gm.addVariable(2);
   gm.addVariable(2);

   size_t vv[] = {1,2,3,4,5,6,7,8};
   for(size_t variable1 = 0; variable1 < gm.numberOfVariables(); ++variable1)
   for(size_t variable2 = variable1 + 1; variable2 < gm.numberOfVariables(); ++variable2)
   for(size_t variable3 = variable2 + 1; variable3 < gm.numberOfVariables(); ++variable3) {
      const size_t shape[] = {
         gm.numberOfLabels(variable1),
         gm.numberOfLabels(variable2),
         gm.numberOfLabels(variable3)
      };
      pgm::ExplicitFunction f(shape, shape + 3);
      for(size_t state1 = 0; state1 < gm.numberOfLabels(variable1); ++state1)
      for(size_t state2 = 0; state2 < gm.numberOfLabels(variable2); ++state2)
      for(size_t state3 = 0; state3 < gm.numberOfLabels(variable3); ++state3) {
         f(0,0,0)=float(vv[0]);
	 f(1,0,0)=float(vv[1]);
	 f(0,1,0)=float(vv[2]);
	 f(1,1,0)=float(vv[3]);
	 f(0,0,1)=float(vv[4]);
	 f(1,0,1)=float(vv[5]);
	 f(0,1,1)=float(vv[6]);
	 f(1,1,1)=float(vv[7]);
      }
      pgm::OpengmModel::FunctionIdentifier id = gm.addFunction(f);
      size_t variableIndexSequence[] = {variable1, variable2, variable3};
      gm.addFactor(id, variableIndexSequence, variableIndexSequence + 3);
      BOOST_CHECK_EQUAL(f(0,0,0), 1);
      BOOST_CHECK_EQUAL(f(1,0,0), 2);
      BOOST_CHECK_EQUAL(f(0,1,0), 3);
      BOOST_CHECK_EQUAL(f(1,1,0), 4);
      BOOST_CHECK_EQUAL(f(0,0,1), 5);
      BOOST_CHECK_EQUAL(f(1,0,1), 6);
      BOOST_CHECK_EQUAL(f(0,1,1), 7);
      BOOST_CHECK_EQUAL(f(1,1,1), 8);
   }
}
// EOF


#include <ros/ros.h>
#include <atraj.h>
#include "atraj.cpp"

#include <Eigen/Dense>

#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <pc_asctec_sim/pc_traj_cmd.h>
#include <pc_asctec_sim/pc_state.h>

#include <sensor_msgs/Joy.h>
#include <visualization_msgs/Marker.h>

#include <math.h>
#include <string.h>

#define obsNumb 10

#define repel_pts 3.0	//always integer
#define repel 0.7       //radius
#define repel_t 5.0
#define repel_rings 5
#define end_off 0.5

#define real_r  0.14    //11 inches diameter
#define quad_r 0.27     //21 inches diameter

#define freq 10
#define maxV 0.25
#define maxVZ 0.2
#define maxA maxV

using namespace std;

struct obs {
	string frame;
	float x;
	float y;
	float r;

	obs():frame(""), x(0), y(0), r(0) {}
}OBS;

struct Isection {

	double t;
	int spline;
	Isection * next;

}ISECTION;

enum states {
waitStart,
waitHover,
waitA,
waitB
};

states state = waitStart;

bool isDone = true;
bool nextTraj = false;
bool isFlying = false;

obs * obs_ptr = new obs[obsNumb];
int points;

Trajectory_Accel * path_gen = new Trajectory_Accel(1.0);

string world, quad_name, obs_frame;
pc_asctec_sim::pc_state q_st;
pc_asctec_sim::pc_traj_cmd raw;
pc_asctec_sim::CMatrix C;

ros::Publisher traj_pub, start_pub, obs_pub, tiki_pub, quad_pub;
ros::Subscriber rawtraj_sub, traj_sub, joy_sub, state_sub;

void stateCallback(const pc_asctec_sim::pc_state::ConstPtr& msg)
{
	q_st = *msg;
}

void trajCallback(const std_msgs::Bool::ConstPtr& msg)
{
	isDone = msg->data;
}

void rawtrajCallback(const pc_asctec_sim::pc_traj_cmd::ConstPtr& msg)
{
	NEW_PATH temp;
	temp.state = q_st;
	temp.cmd = *msg;
	points = msg->points;

	path_gen->getPathConstants(&temp,&C);
}

void joyCallback(const sensor_msgs::Joy::ConstPtr& msg)
{
	if(msg->buttons[0]) {
		isFlying = !isFlying;

	}else if(msg->buttons[1]) {
		if(state != waitStart) {
			nextTraj = true;
		}
	}
}

float limit(float value, float upper, float lower)
{
	float temp = value;
	if(value > upper) {
		temp = upper;

	}else if(value < lower) {
		temp = lower;
	}

	return temp;
}

float getTravTime(float xst, float x, float yst, float y, float zst, float z)
{
	float tTravel = sqrt(pow(xst - x,2) + pow(yst - y,2) + pow(zst - z,2)) / maxV;
	return limit(tTravel, 12, 0.1);
}

float getTravTimeZ(float xst, float x, float yst, float y, float zst, float z)
{
	float tTravel = sqrt(pow(xst - x,2) + pow(yst - y,2) + pow(zst - z,2)) / maxVZ;
	return limit(tTravel, 20, 0.1);
}
/*
struct Isection * getLastNode(struct Isection * node)
{
	if(node->next) {
		return getLastNode(node->next);
	}else {
		return node;
	}
}

void addNewINode(double t, int spline)
{
	struct Isection * next = new Isection;
	next->t = t;
	next->spline = spline;

	getLastNode()->next = next;
}

void getRoots(pc_asctec_sim::pc_traj_cmd * path, pc_asctec_sim::pc_state * q_st)
{
	NEW_PATH temp;
	temp.state = q_st;
	temp.cmd = *path;
	points = temp.cmd.points;
	C = *path_gen->getPathConstants(&temp,&C);

	Eigen::MatrixXd companion = Eigen::MatrixXd::Constant(10,10,0);
	for(int i=0; i<9; i++) {
		companion(i+1,i) = 1;
	}

	int sp = 0;
	for(int i=0; i<points; i++) {
		double A10 = -(pow(C.c5x[i],2) + pow(C.c5y[i],2));

		companion(0,9) = (pow((C.c0x[i] - ob_ptr->x),2) + pow((C.c0y[i] - ob_ptr->y),2) - pow(ob_ptr->r,2))/A10;
		companion(1,9) = (2*C.c0x[i]*C.c1x[i] - 2*C.c1x[i]*ob_ptr->x + 2*C.c0y[i]*C.c1y[i] - 2*C.c1y[i]*ob_ptr->y)/A10;
		companion(2,9) = (2*C.c0x[i]*C.c2x[i] + pow(C.c1x[i],2) - 2*C.c2x[i]*ob_ptr->x + 2*C.c0y[i]*C.c2y[i] + pow(C.c1y[i],2) - 2*C.c2y[i]*ob_ptr->y)/A10;
		companion(3,9) = (2*C.c0x[i]*C.c3x[i] + 2*C.c1x[i]*C.c2x[i] - 2*C.c3x[i]*ob_ptr->x + 2*C.c0y[i]*C.c3y[i] + 2*C.c1y[i]*C.c2y[i] - 2*C.c3y[i]*ob_ptr->y)/A10;
		companion(4,9) = (2*C.c0x[i]*C.c4x[i] + 2*C.c1x[i]*C.c3x[i] + pow(C.c2x[i],2) - 2*C.c4x[i]*ob_ptr->x + 2*C.c0y[i]*C.c4y[i] + 2*C.c1y[i]*C.c3y[i] + pow(C.c2y[i],2) - 2*C.c4y[i]*ob_ptr->y)/A10;
		companion(5,9) = (2*C.c0x[i]*C.c5x[i] + 2*C.c1x[i]*C.c4x[i] + 2*C.c2x[i]*C.c3x[i] - 2*C.c5x[i]*ob_ptr->x + 2*C.c0y[i]*C.c5y[i] + 2*C.c1y[i]*C.c4y[i] + 2*C.c2y[i]*C.c3y[i] - 2*C.c5y[i]*ob_ptr->y)/A10;
		companion(6,9) = (2*C.c1x[i]*C.c5x[i] + 2*C.c2x[i]*C.c4x[i] + pow(C.c3x[i],2) + 2*C.c1y[i]*C.c5y[i] + 2*C.c2y[i]*C.c4y[i] + pow(C.c3y[i],2))/A10;
		companion(7,9) = (2*C.c2x[i]*C.c5x[i] + 2*C.c3x[i]*C.c4x[i] + 2*C.c2y[i]*C.c5y[i] + 2*C.c3y[i]*C.c4y[i])/A10;
		companion(8,9) = (2*C.c3x[i]*C.c5x[i] + pow(C.c4x[i],2) + 2*C.c3y[i]*C.c5y[i] + pow(C.c4y[i],2))/A10;
		companion(9,9) = (2*C.c4x[i]*C.c5x[i] + 2*C.c4y[i]*C.c5y[i])/A10;

		Eigen::EigenSolver<Eigen::MatrixXd> result(companion);

		int j = 0;
		for(int k=0; k<10; k++) {
			complex<double> lambda = result.eigenvalues()[k];
			if(lambda.imag() == 0) {
				ob_ptr->I[j][sp] = lambda.real();
				j++;
				ob_ptr->spline[sp] = i;
				ob_ptr->check = true;
			}
		}
		sp++;
	}
}

double getObsAngle(struct obs * ob_ptr)
{
	//TODO
	float Ix1 = C.c0x[0
	float Ix2
	float Iy1
	float Iy2

	double a = sqrt(pow(Ix1 - Ix2,2) + pow(Iy1 - Iy2,2));
	double angle = acos((2*(pow(ob_ptr->r,2)) - pow(a,2))/(2*(pow(ob_ptr->r,2)));
	return angle;
}

pc_asctec_sim::pc_traj_cmd * injectObsPoints(struct obs * ob_ptr, pc_asctec_sim::pc_traj_cmd * path)
{
	//TODO
	getRoots(ob_ptr, path);
	double maxAng = getObsAngle(ob_ptr);
}

pc_asctec_sim::pc_traj_cmd * adaptTrajectory(pc_asctec_sim::pc_traj_cmd * path)
{
	for(int i=0; i<obsNumb; i++) {
		if((obs_ptr+i)->check) {
			path = injectObsPoints(obs_ptr+i, path);
		}
	}
	return path;
}

void updateObstacles(struct obs * obs_ptr, tf::TransformListener * listener)
{
	tf::StampedTransform transform;
	for(int i=0; i<obsNumb; i++) {
		if((obs_ptr+i)->frame != "") {
			listener->lookupTransform(world, (obs_ptr+i)->frame, ros::Time(0), transform);
			(obs_ptr+i)->x = transform.getOrigin().x();
			(obs_ptr+i)->y = transform.getOrigin().y();
		}
	}
}
*/
void sendAvoidABNew(float wait, float x, float y, float z, float yaw, tf::StampedTransform * transform)
{
	/* Aversion trajectory from -y to +y
	 * Default trajectory to inject from is x = 0 from -1.5 <= y <= 1.5
	 * gamma: angle portion of avoidance circle
	 * theta: angle of first point relative to trajectory
	 * avoidR: avoid radius length
	 */

	float obsx = transform->getOrigin().x();
	float obsy = transform->getOrigin().y();
	float theta = asin(abs(obsx)/repel);
	float gamma = M_PI - 2*theta;

	pc_asctec_sim::pc_traj_cmd cmd;
	//Set first point of obstacle radius
	cmd.x[0] = 0.0;
	cmd.y[0] = obsy - repel*cos(theta) - end_off;
	cmd.vy[0] = maxV;
	cmd.z[0] = 1.0;
	cmd.duration[0] = getTravTime(q_st.x, cmd.x[0], q_st.y, cmd.y[0], q_st.z, cmd.z[0]);

	//Set mid points of obstacle radius
	if(obsx > 0) {
		//Set second point of obstacle radius
		cmd.x[1] = -0.1*cos(theta);
		cmd.y[1] = obsy - repel*cos(theta) - end_off/8;
		cmd.vy[1] = maxV;
		cmd.z[1] = 1.0;
		cmd.duration[1] = getTravTime(cmd.x[0], cmd.x[1], cmd.y[0], cmd.y[1], cmd.z[0], cmd.z[1]);

		for(int i=1; i<=repel_pts; i++) {
			float newAng = theta + i*gamma/(repel_pts+1);
			cmd.x[i+1] = obsx - repel*sin(newAng);
			cmd.vx[i+1] = -maxV*sin(M_PI/2 * i/(repel_pts-1) + M_PI/2);
			cmd.ax[i+1] = -maxV*cos(M_PI/2 * i/(repel_pts-1) + M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.y[i+1] = obsy - repel*cos(newAng);
			cmd.vy[i+1] = maxV*cos(M_PI/2 * i/(repel_pts-1) - M_PI/2);
			cmd.ay[i+1] = -maxV*sin(M_PI/2 * i/(repel_pts-1) - M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.z[i+1] = 1.0;
			cmd.duration[i+1] = getTravTime(cmd.x[i], cmd.x[i+1], cmd.y[i], cmd.y[i+1], cmd.z[i], cmd.z[i+1]);
		}

		//Set second to last point of obstacle radius
		cmd.x[repel_pts+2] = -0.1*cos(theta);
		cmd.y[repel_pts+2] = obsy + repel*cos(theta) + end_off/8;
		cmd.vy[repel_pts+2] = maxV;
		cmd.z[repel_pts+2] = 1.0;
		cmd.duration[repel_pts+2] = getTravTime(cmd.x[repel_pts+1], cmd.x[repel_pts+2], cmd.y[repel_pts+1], cmd.y[repel_pts+2], cmd.z[repel_pts+1], cmd.z[repel_pts+2]);

	}else {
		//Set second point of obstacle radius
		cmd.x[1] = 0.1*cos(theta);
		cmd.y[1] = obsy - repel*cos(theta) - end_off/8;
		cmd.vy[1] = maxV;
		cmd.z[1] = 1.0;
		cmd.duration[1] = getTravTime(cmd.x[0], cmd.x[1], cmd.y[0], cmd.y[1], cmd.z[0], cmd.z[1]);

		for(int i=1; i<=repel_pts; i++) {
			float newAng = theta + i*gamma/(repel_pts+1);
			cmd.x[i+1] = obsx + repel*sin(newAng);
			cmd.vx[i+1] = maxV*sin(M_PI/2 * i/(repel_pts-1) + M_PI/2);
			cmd.ax[i+1] = maxV*cos(M_PI/2 * i/(repel_pts-1) + M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.y[i+1] = obsy - repel*cos(newAng);
			cmd.vy[i+1] = maxV*cos(M_PI/2 * i/(repel_pts-1) - M_PI/2);
			cmd.ay[i+1] = -maxV*sin(M_PI/2 * i/(repel_pts-1) - M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.z[i+1] = 1.0;
			cmd.duration[i+1] = getTravTime(cmd.x[i], cmd.x[i+1], cmd.y[i], cmd.y[i+1], cmd.z[i], cmd.z[i+1]);
		}

		//Set second to last point of obstacle radius
		cmd.x[repel_pts+2] = 0.1*cos(theta);
		cmd.y[repel_pts+2] = obsy + repel*cos(theta) + end_off/8;
		cmd.vy[repel_pts+2] = maxV;
		cmd.z[repel_pts+2] = 1.0;
		cmd.duration[repel_pts+2] = getTravTime(cmd.x[repel_pts+1], cmd.x[repel_pts+2], cmd.y[repel_pts+1], cmd.y[repel_pts+2], cmd.z[repel_pts+1], cmd.z[repel_pts+2]);
	}

	//Set final point of obstacle radius
	cmd.x[repel_pts+3] = 0.0;
	cmd.y[repel_pts+3] = obsy + repel*cos(theta) + end_off;
	cmd.vy[repel_pts+3] = maxV;
	cmd.z[repel_pts+3] = 1.0;
	cmd.duration[repel_pts+3] = getTravTime(cmd.x[repel_pts+2], cmd.x[repel_pts+3], cmd.y[repel_pts+2], cmd.y[repel_pts+3], cmd.z[repel_pts+2], cmd.z[repel_pts+3]);

	//Set end point of path
	cmd.x[repel_pts+4] = x;
	cmd.y[repel_pts+4] = y;
	cmd.z[repel_pts+4] = z;
	cmd.yaw[repel_pts+4] = yaw;
	cmd.wait_time[repel_pts+4] = wait;
	cmd.duration[repel_pts+4] = getTravTime(cmd.x[repel_pts+3], cmd.x[repel_pts+4], cmd.y[repel_pts+3], cmd.y[repel_pts+4], cmd.z[repel_pts+3], cmd.z[repel_pts+4]);

	cmd.points = repel_pts+5;
	isDone = false;
	traj_pub.publish(cmd);
}

void sendAvoidBANew(float wait, float x, float y, float z, float yaw, tf::StampedTransform * transform)
{
	/* Aversion trajectory from +y to -y
	 * Default trajectory to inject from is x = 0 from -1.5 <= y <= 1.5
	 * gamma: angle portion of avoidance circle
	 * theta: angle of first point relative to trajectory
	 */

	float obsx = transform->getOrigin().x();
	float obsy = transform->getOrigin().y();
	float theta = asin(abs(obsx)/repel);
	float gamma = M_PI - 2*theta;

	pc_asctec_sim::pc_traj_cmd cmd;
	//Set first point of obstacle radius
	cmd.x[0] = 0.0;
	cmd.y[0] = obsy + repel*cos(theta) + end_off;
	cmd.vy[0] = -maxV;
	cmd.z[0] = 1.0;
	cmd.duration[0] = getTravTime(q_st.x, cmd.x[0], q_st.y, cmd.y[0], q_st.z, cmd.z[0]);

	//Set mid points of obstacle radius
	if(obsx > 0) {
		//Set second point of obstacle radius
		cmd.x[1] = -0.1*cos(theta);
		cmd.y[1] = obsy + repel*cos(theta) + end_off/8;
		cmd.vy[1] = -maxV;
		cmd.z[1] = 1.0;
		cmd.duration[1] = getTravTime(cmd.x[0], cmd.x[1], cmd.y[0], cmd.y[1], cmd.z[0], cmd.z[1]);

		for(int i=1; i<=repel_pts; i++) {
			float newAng = theta + i*gamma/(repel_pts+1);
			cmd.x[i+1] = obsx - repel*sin(newAng);
			cmd.vx[i+1] = -maxV*sin(M_PI/2 * i/(repel_pts-1) + M_PI/2);
			cmd.ax[i+1] = -maxV*cos(M_PI/2 * i/(repel_pts-1) + M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.y[i+1] = obsy + repel*cos(newAng);
			cmd.vy[i+1] = -maxV*cos(M_PI/2 * i/(repel_pts-1) - M_PI/2);
			cmd.ay[i+1] = maxV*sin(M_PI/2 * i/(repel_pts-1) - M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.z[i+1] = 1.0;
			cmd.duration[i+1] = getTravTime(cmd.x[i], cmd.x[i+1], cmd.y[i], cmd.y[i+1], cmd.z[i], cmd.z[i+1]);
		}

		//Set second to last point of obstacle radius
		cmd.x[repel_pts+2] = -0.1*cos(theta);
		cmd.y[repel_pts+2] = obsy - repel*cos(theta) - end_off/8;
		cmd.vy[repel_pts+2] = -maxV;
		cmd.z[repel_pts+2] = 1.0;
		cmd.duration[repel_pts+2] = getTravTime(cmd.x[repel_pts+1], cmd.x[repel_pts+2], cmd.y[repel_pts+1], cmd.y[repel_pts+2], cmd.z[repel_pts+1], cmd.z[repel_pts+2]);

	}else {
		//Set second point of obstacle radius
		cmd.x[1] = 0.1*cos(theta);
		cmd.y[1] = obsy + repel*cos(theta) + end_off/8;
		cmd.vy[1] = -maxV;
		cmd.z[1] = 1.0;
		cmd.duration[1] = getTravTime(cmd.x[0], cmd.x[1], cmd.y[0], cmd.y[1], cmd.z[0], cmd.z[1]);

		for(int i=1; i<=repel_pts; i++) {
			float newAng = theta + i*gamma/(repel_pts+1);
			cmd.x[i+1] = obsx + repel*sin(newAng);
			cmd.vx[i+1] = maxV*sin(M_PI/2 * i/(repel_pts-1) + M_PI/2);
			cmd.ax[i+1] = maxV*cos(M_PI/2 * i/(repel_pts-1) + M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.y[i+1] = obsy + repel*cos(newAng);
			cmd.vy[i+1] = -maxV*cos(M_PI/2 * i/(repel_pts-1) - M_PI/2);
			cmd.ay[i+1] = maxV*sin(M_PI/2 * i/(repel_pts-1) - M_PI/2) * M_PI/(2*(repel_pts-1));

			cmd.z[i+1] = 1.0;
			cmd.duration[i+1] = getTravTime(cmd.x[i], cmd.x[i+1], cmd.y[i], cmd.y[i+1], cmd.z[i], cmd.z[i+1]);
		}

		//Set second to last point of obstacle radius
		cmd.x[repel_pts+2] = 0.1*cos(theta);
		cmd.y[repel_pts+2] = obsy - repel*cos(theta) - end_off/8;
		cmd.vy[repel_pts+2] = -maxV;
		cmd.z[repel_pts+2] = 1.0;
		cmd.duration[repel_pts+2] = getTravTime(cmd.x[repel_pts+1], cmd.x[repel_pts+2], cmd.y[repel_pts+1], cmd.y[repel_pts+2], cmd.z[repel_pts+1], cmd.z[repel_pts+2]);
	}

	//Set final point of obstacle radius
	cmd.x[repel_pts+3] = 0.0;
	cmd.y[repel_pts+3] = obsy - repel*cos(theta) - end_off;
	cmd.vy[repel_pts+3] = -maxV;
	cmd.z[repel_pts+3] = 1.0;
	cmd.duration[repel_pts+3] = getTravTime(cmd.x[repel_pts+2], cmd.x[repel_pts+3], cmd.y[repel_pts+2], cmd.y[repel_pts+3], cmd.z[repel_pts+2], cmd.z[repel_pts+3]);

	//Set end point of path
	cmd.x[repel_pts+4] = x;
	cmd.y[repel_pts+4] = y;
	cmd.z[repel_pts+4] = z;
	cmd.yaw[repel_pts+4] = yaw;
	cmd.wait_time[repel_pts+4] = wait;
	cmd.duration[repel_pts+4] = getTravTime(cmd.x[repel_pts+3], cmd.x[repel_pts+4], cmd.y[repel_pts+3], cmd.y[repel_pts+4], cmd.z[repel_pts+3], cmd.z[repel_pts+4]);

	cmd.points = repel_pts+5;
	isDone = false;
	traj_pub.publish(cmd);
}

void sendTrajectory(float wait, float x, float y, float z, float yaw)
{
	pc_asctec_sim::pc_traj_cmd cmd;
	cmd.x[0] = x;
	cmd.y[0] = y;
	cmd.z[0] = z;
	cmd.yaw[0] = yaw;
	cmd.wait_time[0] = wait;
	cmd.duration[0] = getTravTime(q_st.x, x, q_st.y, y, q_st.z, z);
	cmd.points = 1;

	isDone = false;
	traj_pub.publish(cmd);
}

void sendRiseTrajectory()
{
	pc_asctec_sim::pc_traj_cmd cmd;
	cmd.x[0] = 0;
	cmd.y[0] = 0;
	cmd.z[0] = 1;
	cmd.yaw[0] = 0;
	cmd.wait_time[0] = 0;
	cmd.duration[0] = getTravTimeZ(q_st.x, 0, q_st.y, 0, q_st.z, 1);
	cmd.points = 1;

	isDone = false;
	traj_pub.publish(cmd);
}

void sendLandTrajectory()
{
	pc_asctec_sim::pc_traj_cmd cmd;
	cmd.x[0] = 0.0;
	cmd.y[0] = 0.0;
	cmd.z[0] = 1.0;
	cmd.yaw[0] = 0.0;
	cmd.wait_time[0] = 0.5;
	cmd.duration[0] = getTravTime(q_st.x, 0, q_st.y, 0, q_st.z, 1);

	cmd.x[1] = 0.0;
	cmd.y[1] = 0.0;
	cmd.z[1] = 0.0;
	cmd.yaw[1] = 0.0;
	cmd.wait_time[1] = 0.0;
	cmd.duration[1] = 3;

	cmd.points = 2;

	isDone = false;
	traj_pub.publish(cmd);
}

bool obstacleExists(tf::StampedTransform * transform)
{
	/* Obstacle is observed if line traj intersects;
	 * obsx+rad > trajx (0) & obsx-rad < trajx (0)
	 */

	if(transform->getOrigin().x()+repel > 0 && transform->getOrigin().x()-repel < 0) {
		ROS_INFO("Obstacled detected, adapting trajectory");
		return true;
	}
	return false;
}

bool targetPointOpen(float x, float y, tf::StampedTransform * transform)
{
	float obsx = transform->getOrigin().x();
	float obsy = transform->getOrigin().y();
	float dist = sqrt(pow((obsx-x),2) + pow((obsy-y),2));

	if(dist > repel) {
		return true;
	}else {
		return false;
	}
}

void showRange(float x, float y, float z)
{
	visualization_msgs::Marker ring;
	geometry_msgs::Point vis_ring;

	ring.header.frame_id = world;
	ring.header.stamp = ros::Time::now();
	ring.id = 2;
	ring.action = visualization_msgs::Marker::ADD;
	ring.type = visualization_msgs::Marker::LINE_LIST;
	ring.color.a = 1.0;
	ring.color.g = 1.0;

	ring.scale.x = 0.05;
	ring.scale.y = 0.05;

	for(int j=0; j<=repel_rings; j++) {
		float i = 0.0;
		while(i < 2*M_PI) {
			vis_ring.x = cos(i) * repel + x;
			vis_ring.y = sin(i) * repel + y;
			vis_ring.z = z*j/repel_rings;
			ring.points.push_back(vis_ring);
			i += 0.05;

			vis_ring.x = cos(i) * repel + x;
			vis_ring.y = sin(i) * repel + y;
			vis_ring.z = z*j/repel_rings;
			ring.points.push_back(vis_ring);
		}
	}

	obs_pub.publish(ring);
}

void showRealRadius(float x, float y, float z)
{
	visualization_msgs::Marker ring;
	geometry_msgs::Point vis_ring;

	ring.header.frame_id = world;
	ring.header.stamp = ros::Time::now();
	ring.id = 2;
	ring.action = visualization_msgs::Marker::ADD;
	ring.type = visualization_msgs::Marker::LINE_LIST;
	ring.color.a = 1.0;
	ring.color.r = 1.0;

	ring.scale.x = 0.05;
	ring.scale.y = 0.05;

	for(int j=0; j<=repel_rings; j++) {
		float i = 0.0;
		while(i < 2*M_PI) {
			vis_ring.x = cos(i) * real_r + x;
			vis_ring.y = sin(i) * real_r + y;
			vis_ring.z = z*j/repel_rings;
			ring.points.push_back(vis_ring);
			i += 0.05;

			vis_ring.x = cos(i) * real_r + x;
			vis_ring.y = sin(i) * real_r + y;
			vis_ring.z = z*j/repel_rings;
			ring.points.push_back(vis_ring);
		}
	}

	tiki_pub.publish(ring);
}

void showQuadRadius(float x, float y, float z)
{
	visualization_msgs::Marker ring;
	geometry_msgs::Point vis_ring;

	ring.header.frame_id = world;
	ring.header.stamp = ros::Time::now();
	ring.id = 2;
	ring.action = visualization_msgs::Marker::ADD;
	ring.type = visualization_msgs::Marker::LINE_LIST;
	ring.color.a = 1.0;
	ring.color.b = 1.0;

	ring.scale.x = 0.05;
	ring.scale.y = 0.05;

	float i = 0.0;
	while(i < 2*M_PI) {
		vis_ring.x = cos(i) * quad_r + x;
		vis_ring.y = sin(i) * quad_r + y;
		vis_ring.z = z;
		ring.points.push_back(vis_ring);
		i += 0.05;

		vis_ring.x = cos(i) * quad_r + x;
		vis_ring.y = sin(i) * quad_r + y;
		vis_ring.z = z;
		ring.points.push_back(vis_ring);
	}

	quad_pub.publish(ring);
}

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

	ros::init(argc, argv, "Obs_avoid");
	ros::NodeHandle nh;
	ros::Rate loop_rate = freq;

	tf::StampedTransform transform;
	tf::TransformListener listener;

	ros::param::get("~w_frame", world);
	ros::param::get("~q_name", quad_name);
	ros::param::get("~obs_frame", obs_frame);

	traj_pub = nh.advertise<pc_asctec_sim::pc_traj_cmd>(quad_name + "/traj_points", 10);
	start_pub = nh.advertise<std_msgs::Bool>(quad_name + "/start", 10);
	obs_pub = nh.advertise<visualization_msgs::Marker>(quad_name + "/obstacle", 10);
	tiki_pub = nh.advertise<visualization_msgs::Marker>(quad_name + "/tiki", 10);
	quad_pub = nh.advertise<visualization_msgs::Marker>(quad_name + "/quad_size", 10);

	traj_sub = nh.subscribe(quad_name + "/traj_end", 10, trajCallback);
	rawtraj_sub = nh.subscribe(quad_name + "/traj_points/raw", 10, rawtrajCallback);
	joy_sub = nh.subscribe("/joy", 10, joyCallback);
	state_sub = nh.subscribe(quad_name + "/state", 10, stateCallback);

	listener.waitForTransform(world, obs_frame, ros::Time(0), ros::Duration(3.0));

	ROS_INFO("Running: Obstacle Avoider");

	while(ros::ok()) {

		ros::spinOnce();
		listener.lookupTransform(world, obs_frame, ros::Time(0), transform);
		showRange(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
		showRealRadius(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
		showQuadRadius(q_st.x, q_st.y, q_st.z);

		obs_ptr->x = transform.getOrigin().x();
		obs_ptr->y = transform.getOrigin().y();
		obs_ptr->r = repel;

		switch(state) {
			case waitStart:
				if(isFlying) {
					if(targetPointOpen(0,0,&transform)) {
						ROS_INFO("Taking off!");
						state = waitHover;

						std_msgs::Bool start;
						start.data = true;
						start_pub.publish(start);

						sendRiseTrajectory();
					}else {
						ROS_INFO("Remove obstacle from center first!");
						isFlying = false;
					}
				}
				break;

			case waitHover:
				if(isDone) {
					if(nextTraj) {
						if(targetPointOpen(0,-2.0,&transform)) {
							ROS_INFO("Moving to point A");
							state = waitA;
							sendTrajectory(0,0,-2.0,1,0);
						}else {
							ROS_INFO("Remove obstacle from A first!");
						}
						nextTraj = false;
					}

					if(!isFlying) {
						ROS_INFO("Landing!");
						state = waitStart;

						sendLandTrajectory();
					}
				}
				break;

			case waitA:
				if(isDone) {
					if(nextTraj) {
						ROS_INFO("Observing obstacle(s) from A->B");
						nextTraj = false;
						state = waitB;

						if(obstacleExists(&transform)) {
							sendAvoidABNew(0,0,2.0,1,0,&transform);
							//cmd = *adaptTrajectory(obs_ptr,&cmd);

							/*pc_asctec_sim::pc_traj_cmd cmd;
							cmd.y[0] = 1.5;
							cmd.z[0] = 1.0;
							cmd.duration[0] = getTravTime(q_st.x, 0, q_st.y, 1.5, q_st.z, 1);
							cmd.points = 1;
							getRoots(obs_ptr, &cmd);
							*/

						}else {
							sendTrajectory(0,0,2.0,1,0);
						}
					}

					if(!isFlying) {
						if(targetPointOpen(0,0,&transform)) {
							ROS_INFO("Landing!");
							nextTraj = false;
							state = waitStart;

							sendLandTrajectory();
						}else {
							ROS_INFO("Not safe to land! Remove object from center first!");
							isFlying = true;
						}
					}
				}
				break;

			case waitB:
				if(isDone) {
					if(nextTraj) {
						ROS_INFO("Observing obstacle(s) from B->A");
						nextTraj = false;
						state = waitA;

						if(obstacleExists(&transform)) {
							sendAvoidBANew(0,0,-2.0,1,0,&transform);
							//cmd = *adaptTrajectory(obs_ptr,&cmd);
						}else {
							sendTrajectory(0,0,-2.0,1,0);
						}
					}

					if(!isFlying) {
						if(targetPointOpen(0,0,&transform)) {
							ROS_INFO("Landing!");
							nextTraj = false;
							state = waitStart;

							sendLandTrajectory();
						}else {
							ROS_INFO("Not safe to land! Remove object from center first!");
							isFlying = true;
						}
					}
				}
				break;
		}
	loop_rate.sleep();
	}

	return 0;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/vclip/vclip.h>
#include <OpenTissue/core/containers/mesh/common/util/mesh_make_box.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

template<typename mesh_type, typename coordsys_type,typename real_type>
void vclip_test_all_feature_pairs(mesh_type & A, mesh_type & B,coordsys_type const & AtoB,coordsys_type const & BtoA,real_type const & test_distance)
{
  typename coordsys_type::vector3_type p_a;
  typename coordsys_type::vector3_type p_b;

  unsigned int max_iterations = 500;

  OpenTissue::vclip::VClip vclip;

  real_type tol = 0.1;
  {
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb     = B.vertex_begin();
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb_end = B.vertex_end();
    for(;vb!=vb_end;++vb)
    {
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va     = A.vertex_begin();
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va_end = A.vertex_end();
      for(;va!=va_end;++va)
      {
        OpenTissue::vclip::Feature * seed_a = &( * va );
        OpenTissue::vclip::Feature * seed_b = &( * vb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb     = B.vertex_begin();
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb_end = B.vertex_end();
    for(;vb!=vb_end;++vb)
    {
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea     = A.halfedge_begin();
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea_end = A.halfedge_end();
      for(;ea!=ea_end;++ea)
      {
        OpenTissue::vclip::Feature * seed_a = &( * ea );
        OpenTissue::vclip::Feature * seed_b = &( * vb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb     = B.vertex_begin();
    OpenTissue::vclip::vclip_mesh_type::vertex_iterator vb_end = B.vertex_end();
    for(;vb!=vb_end;++vb)
    {
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa     = A.face_begin();
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa_end = A.face_end();
      for(;fa!=fa_end;++fa)
      {
        OpenTissue::vclip::Feature * seed_a = &( * fa );
        OpenTissue::vclip::Feature * seed_b = &( * vb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb     = B.halfedge_begin();
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb_end = B.halfedge_end();
    for(;eb!=eb_end;++eb)
    {
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va     = A.vertex_begin();
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va_end = A.vertex_end();
      for(;va!=va_end;++va)
      {
        OpenTissue::vclip::Feature * seed_a = &( * va );
        OpenTissue::vclip::Feature * seed_b = &( * eb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb     = B.halfedge_begin();
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb_end = B.halfedge_end();
    for(;eb!=eb_end;++eb)
    {
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea     = A.halfedge_begin();
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea_end = A.halfedge_end();
      for(;ea!=ea_end;++ea)
      {
        OpenTissue::vclip::Feature * seed_a = &( * ea );
        OpenTissue::vclip::Feature * seed_b = &( * eb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb     = B.halfedge_begin();
    OpenTissue::vclip::vclip_mesh_type::halfedge_iterator eb_end = B.halfedge_end();
    for(;eb!=eb_end;++eb)
    {
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa     = A.face_begin();
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa_end = A.face_end();
      for(;fa!=fa_end;++fa)
      {
        OpenTissue::vclip::Feature * seed_a = &( * fa );
        OpenTissue::vclip::Feature * seed_b = &( * eb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb     = B.face_begin();
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb_end = B.face_end();
    for(;fb!=fb_end;++fb)
    {
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va     = A.vertex_begin();
      OpenTissue::vclip::vclip_mesh_type::vertex_iterator va_end = A.vertex_end();
      for(;va!=va_end;++va)
      {
        OpenTissue::vclip::Feature * seed_a = &( * va );
        OpenTissue::vclip::Feature * seed_b = &( * fb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance>0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance<0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb     = B.face_begin();
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb_end = B.face_end();
    for(;fb!=fb_end;++fb)
    {
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea     = A.halfedge_begin();
      OpenTissue::vclip::vclip_mesh_type::halfedge_iterator ea_end = A.halfedge_end();
      for(;ea!=ea_end;++ea)
      {
        OpenTissue::vclip::Feature * seed_a = &( * ea );
        OpenTissue::vclip::Feature * seed_b = &( * fb );
        real_type distance = vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations);
        if(test_distance > 0)
          BOOST_CHECK_CLOSE( distance, test_distance, tol );
        else if(test_distance < 0)
          BOOST_CHECK( distance < 0 );
        else
          BOOST_CHECK( fabs(distance) < 10e-7 );
      }
    }
  }
  {
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb     = B.face_begin();
    OpenTissue::vclip::vclip_mesh_type::face_iterator fb_end = B.face_end();
    for(;fb!=fb_end;++fb)
    {
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa     = A.face_begin();
      OpenTissue::vclip::vclip_mesh_type::face_iterator fa_end = A.face_end();
      for(;fa!=fa_end;++fa)
      {
        OpenTissue::vclip::Feature * seed_a = &( * fa );
        OpenTissue::vclip::Feature * seed_b = &( * fb );
        BOOST_CHECK_THROW( vclip.run(A,B,AtoB,BtoA,&seed_a,&seed_b,p_a,p_b,max_iterations),  std::invalid_argument);
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE(opentissue_collision_vclip);

BOOST_AUTO_TEST_CASE(face_aligned_separated_boxes)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  OpenTissue::vclip::vclip_mesh_type A;
  OpenTissue::vclip::vclip_mesh_type B;

  OpenTissue::polymesh::PolyMesh<> tmp;
  OpenTissue::mesh::make_box(1.0,1.0,1.0, tmp);
  OpenTissue::mesh::make_box(1.0,1.0,1.0, tmp);

  OpenTissue::vclip::convert(tmp,A);
  OpenTissue::vclip::convert(tmp,B);

  coordsys_type  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.0), quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.0), quaternion_type());
  coordsys_type AtoB = model_update(Awcs,Bwcs);
  coordsys_type BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);

  Awcs = coordsys_type( vector3_type( 1.0,0.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type(-1.0,0.0,0.0), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);

  Awcs = coordsys_type( vector3_type( 0.0, 1.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0,-1.0,0.0), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);

  Awcs = coordsys_type( vector3_type( 0.0,-1.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0, 1.0,0.0), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);

  Awcs = coordsys_type( vector3_type( 0.0, 0.0,  1.0 ), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0, 0.0, -1.0 ), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);

  Awcs = coordsys_type( vector3_type( 0.0, 0.0, -1.0 ), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0, 0.0,  1.0 ), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);
}

BOOST_AUTO_TEST_CASE(non_aligned_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::value_traits                         value_traits;
  typedef math_types::real_type                            real_type;

  OpenTissue::vclip::vclip_mesh_type A;
  OpenTissue::vclip::vclip_mesh_type B;

  OpenTissue::polymesh::PolyMesh<> tmp;
  OpenTissue::mesh::make_box(1.0,1.0,1.0, tmp);
  OpenTissue::mesh::make_box(1.0,1.0,1.0, tmp);

  OpenTissue::vclip::convert(tmp,A);
  OpenTissue::vclip::convert(tmp,B);

  real_type sqrt_half = std::sqrt(0.5);

  real_type test_distance = 1.0 - (sqrt_half - 0.5);

  quaternion_type q;
  q.Rz( value_traits::pi()/4.0 );

  coordsys_type  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.0), quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.0), q);
  coordsys_type AtoB = model_update(Awcs,Bwcs);
  coordsys_type BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,test_distance);

  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.1,0.1), q);
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,test_distance);

  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.5), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.0), q);
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,test_distance);

  Awcs = coordsys_type( vector3_type(-1.0,0.0,-0.5), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.5), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA,1.0);
}

BOOST_AUTO_TEST_CASE(penetrating_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::value_traits                         value_traits;
  typedef math_types::real_type                            real_type;

  OpenTissue::vclip::vclip_mesh_type A;
  OpenTissue::vclip::vclip_mesh_type B;

  OpenTissue::polymesh::PolyMesh<> tmp;
  OpenTissue::mesh::make_box(1.0,1.0,1.0, tmp);
  OpenTissue::vclip::convert(tmp,A);

  OpenTissue::mesh::make_box(1.0,1.0, 1.0, tmp);
  OpenTissue::vclip::convert(tmp,B);

  coordsys_type  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector3_type( 0.25,0.25,0.25), quaternion_type());
  coordsys_type AtoB = model_update(Awcs,Bwcs);
  coordsys_type BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA, -1.0);

  OpenTissue::mesh::make_box(0.9,0.9, 0.9, tmp);
  OpenTissue::vclip::convert(tmp,B);

  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  AtoB = model_update(Awcs,Bwcs);
  BtoA = inverse(AtoB);
  vclip_test_all_feature_pairs(A,B,AtoB,BtoA, -1.0);
}

BOOST_AUTO_TEST_SUITE_END();