#include <iostream>                    // std::cout, std::cerr
#include <string>                      // std::string
#include <sstream>                     // std::stringstream
#include <tuple>                       // std::tuple
#include <vector>                      // std::vector
#include <chrono>                      // std::chrono
#include <utility>                     // std::pair
#include <iomanip>                     // std::setprecision
#include <cstdlib>                     // std::exit
// For External Library
#include <torch/torch.h>               // torch
#include <opencv2/opencv.hpp>          // cv::Mat
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "networks.hpp"                // YOLOv3
#include "detector.hpp"                // YOLODetector
#include "transforms.hpp"              // transforms_Compose, transforms::apply
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace po = boost::program_options;

// -------------------
// Demo Function
// -------------------
void demo(po::variables_map &vm, torch::Device &device, YOLOv3 &model, std::vector<transforms_Compose> &transformI, std::vector<transforms_Compose> &transformD, const std::vector<std::string> class_names, const std::vector<std::vector<std::tuple<float, float>>> anchors){

    constexpr double alpha = 0.1;  // current importance of moving average for calculating FPS
    constexpr std::pair<float, float> output_range = {0.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    int key;
    bool flag;
    double seconds, seconds_est, fps;
    std::string path;
    std::stringstream ss1, ss2;
    std::chrono::system_clock::time_point start, end;
    cv::Mat BGR, RGB;
    cv::Mat imageD, imageI, imageO;
    torch::Tensor tensorD, tensorI;
    std::vector<torch::Tensor> tensorO, tensorO_one;
    std::tuple<torch::Tensor, torch::Tensor, torch::Tensor> detect_result;
    cv::VideoCapture cap;

    // (1) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["demo_load_epoch"].as<std::string>() + ".pth";
    torch::load(model, path, device);

    // (2) Set Detector
    auto detector = YOLODetector(anchors, (long int)vm["class_num"].as<size_t>(), vm["prob_thresh"].as<float>(), vm["nms_thresh"].as<float>());
    std::vector<std::tuple<unsigned char, unsigned char, unsigned char>> label_palette = detector.get_label_palette();

    // (3) Set Camera Device
    if (vm["movie"].as<std::string>() == ""){
        cap.open(vm["cam_num"].as<size_t>());
        if (!cap.isOpened()){
            std::cerr << "Error : Couldn't open the camera '" << vm["cam_num"].as<size_t>() << "'." << std::endl;
            std::exit(1);
        }
        else{
            cap.set(cv::CAP_PROP_FRAME_WIDTH, vm["window_w"].as<size_t>());
            cap.set(cv::CAP_PROP_FRAME_HEIGHT, vm["window_h"].as<size_t>());
        }
    }
    else{
        cap.open(vm["movie"].as<std::string>());
        if (!cap.isOpened()){
            std::cerr << "Error : Couldn't open the movie '" << vm["movie"].as<std::string>() << "'." << std::endl;
            std::exit(1);
        }
    }

    // (4) Show Key Information
    std::cout << std::endl;
    std::cout << "<Key Information>" << std::endl;
    std::cout << "------------------------------" << std::endl;
    std::cout << "| key |        action        |" << std::endl;
    std::cout << "------------------------------" << std::endl;
    std::cout << "|  q  | Stop the camera.     |" << std::endl;
    std::cout << "------------------------------" << std::endl;
    std::cout << std::endl;

    // (5) Demo
    torch::NoGradGuard no_grad;
    model->eval();
    flag = true;
    fps = 0.0;
    start = std::chrono::system_clock::now();
    while (cap.read(BGR)){

        cv::cvtColor(BGR, RGB, cv::COLOR_BGR2RGB);  // {0,1,2} = {B,G,R} ===> {0,1,2} = {R,G,B}

        // (5.1) Set image for input
        RGB.copyTo(imageI);
        tensorI = transforms::apply(transformI, imageI).unsqueeze(/*dim=*/0).to(device);  // imageI{H_D,W_D,C} ==={Resize,ToTensor,etc.}===> tensorI{1,C,H,W}

        // (5.2) Set image for detection
        RGB.copyTo(imageD);
        tensorD = transforms::apply(transformD, imageD);  // imageD{H_D,W_D,3} ==={ToTensor,etc.}===> tensorD{3,H_D,W_D}

        // (5.3) Inference and Detection
        tensorO = model->forward(tensorI);  // {1,C,H,W} ===> {S,{1,G,G,FF}}
        /*************************************************************************/
        tensorO_one = std::vector<torch::Tensor>(tensorO.size());
        for (size_t i = 0; i < tensorO.size(); i++){
            tensorO_one.at(i) = tensorO.at(i)[0];
        }
        detect_result = detector(tensorO_one, {(float)tensorI.size(3), (float)tensorI.size(2)});  // tensorO_one{S,{G,G,FF}} ===> detect_result{ (ids{BB_n}, coords{BB_n,4}, probs{BB_n}) }
        /*************************************************************************/
        imageO = visualizer::draw_detections_des(tensorD.detach(), {std::get<0>(detect_result), std::get<1>(detect_result)}, std::get<2>(detect_result), class_names, label_palette, /*range=*/output_range);

        // (5.4) Calculate FPS
        end = std::chrono::system_clock::now();
        seconds = (double)std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() * 0.001;
        start = std::chrono::system_clock::now();
        switch (flag){
            case true:
                flag = false;
                seconds_est = seconds;
                fps = 1.0 / seconds;
                break;
            default:
                seconds_est = (1.0 - alpha) * seconds_est + alpha * seconds;
                fps = (1.0 - alpha) * fps + alpha * (1.0 / seconds);
        }
        ss1.str(""); ss1.clear(std::stringstream::goodbit);
        ss2.str(""); ss2.clear(std::stringstream::goodbit);
        ss1 << std::setprecision(3) << fps;
        ss2 << std::setprecision(3) << seconds;
        std::cout << "FPS: " << ss1.str() << "[frame/second] (" << ss2.str() << "[second/frame])" << std::endl;

        // (5.5) Show the image in which the objects were detected
        cv::imshow("demo", imageO);
        key = cv::waitKey(1);
        if (key == 'q') break;

    }

    // Post Processing
    cap.release();
    cv::destroyAllWindows();

    // End Processing
    return;

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_SIMD_META_VECTOR_OF_HPP_INCLUDED
#define BOOST_SIMD_SDK_SIMD_META_VECTOR_OF_HPP_INCLUDED

#include <boost/simd/sdk/simd/extensions.hpp>
#include <boost/simd/sdk/simd/native_fwd.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/fusion/include/fold.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/apply.hpp>

/* We use boost::mpl::sizeof_ rather than sizeof because MSVC has trouble
 * with sizeof of dependent names in SFINAE contexts */
#define BOOST_SIMD_LOCAL(z,d,r)                                         \
template<class T,std::size_t Card>                                      \
struct vector_of< T ,Card                                               \
                , typename boost::                                      \
                  enable_if_c < (   Card*sizeof(T)                      \
                                ==  boost::mpl::                        \
                                    sizeof_< typename meta::            \
                                             as_simd<T,r>::type         \
                                           >::value                     \
                                )                                       \
                              >::type                                   \
                >                                                       \
{                                                                       \
  typedef simd::native<T,r> type;                                       \
};                                                                      \
/**/

namespace boost { namespace simd { namespace details
{
  template<class F>
  struct max_value_impl
  {
    template<class Sig>
    struct result;

    template<class This, class T, class U>
    struct result<This(T, U)>
    {
      typedef typename dispatch::meta::strip<T>::type sT;
      typedef typename mpl::apply1<F, sT>::type vT;
      typedef typename dispatch::meta::strip<U>::type sU;
      typedef typename mpl::apply1<F, sU>::type vU;
      typedef typename mpl::if_< mpl::greater<vU, vT>
                               , sU
                               , sT
                               >::type type;
    };
  };

  template<class T, class F>
  struct max_value
       : fusion::result_of::
         fold< T
             , typename fusion::result_of::
               value_of< typename fusion::result_of::begin<T>::type
                       >::type
             , max_value_impl<F>
             >
  {
  };

  template<class T, std::size_t Card, class Enable=void>
  struct vector_of
  {
    typedef simd::native<T, tag::simd_emulation_< sizeof(T)*Card > > type;
  };

  template<class T, std::size_t Card>
  struct vector_of<T, Card, typename boost::enable_if< fusion::traits::is_sequence<T> >::type>
  {
    typedef typename max_value<T, mpl::sizeof_<mpl::_> >::type mT;
    typedef typename vector_of<mT, Card>::type nT;
    typedef simd::native<T, typename nT::extension_type> type;
  };

  #ifdef BOOST_SIMD_DETECTED
  BOOST_PP_SEQ_FOR_EACH(BOOST_SIMD_LOCAL,~,BOOST_SIMD_TAG_SEQ)
#endif

#undef BOOST_SIMD_LOCAL
} } }

namespace boost { namespace simd { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // vector_of<T,N> computes the proper type for pack of N elements of types T
  // Either <T,N> maps onto a SIMD types, either to array<T,N>
  //////////////////////////////////////////////////////////////////////////////
  template<class T,std::size_t Card>
  struct vector_of
   : details::vector_of<T, Card>
  {
  };

  // vectors must have at least 2 elements, vectors of 1 elements are scalar values
  template<class T>
  struct vector_of<T, 1u>
  {
    typedef T type;
  };

} } }

#endif


#include <sstream>
#include <fstream>
#include <stdexcept>
#include <iostream>
using std::cout;
using std::endl;

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace po = boost::program_options;

#include <expression/ExprLib.h>
#include <expression/TimeStepper.h>

#include "defs.h"
#include "test.h"

typedef Expr::ConstantExpr<VolT>  ConstExpr;

Expr::TagList
register_expressions( Expr::ExpressionFactory& exprFactory,
                      const int nvar,
                      const bool doDiffusion,
                      const bool coupledSrc,
                      const bool indepSrc )
{
  using Expr::Tag;
  using Expr::STATE_NONE;

  Expr::TagList rhsTags;

  for( int i=0; i<nvar; ++i ){

    const std::string varname = "var_" + boost::lexical_cast<std::string>(i);

    const Tag varTag( varname, Expr::STATE_N );
    const Tag rhsTag( varname+"_rhs", STATE_NONE );

    Tag fluxTag;
    if( doDiffusion ){
      fluxTag = Tag( varname+"_flux", STATE_NONE );
      exprFactory.register_expression( new FluxExpr::Builder( fluxTag, varTag, 1.0 ) );
    }
    else{
    }
    Tag busyTag;
    if( coupledSrc ){
      busyTag = Tag( varname+"_src", STATE_NONE );
      exprFactory.register_expression( new CoupledBusyWork::Builder(busyTag,nvar) );
    }
    else if( indepSrc ){
      busyTag = Tag( varname+"_src", STATE_NONE );
      exprFactory.register_expression( new BusyWork::Builder(busyTag,varTag,nvar) );
    }

    exprFactory.register_expression( new ConstExpr::Builder(varTag,i) );
    exprFactory.register_expression( new RHSExpr::Builder( rhsTag, fluxTag, busyTag ) );
    rhsTags.push_back( rhsTag );
  }
  return rhsTags;
}

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

void build_operators( PatchT& patch,
                      const std::vector<double>& spacing )
{
  const bool xbcPlus = patch.has_physical_bc_xplus();
  const bool ybcPlus = patch.has_physical_bc_yplus();
  const bool zbcPlus = patch.has_physical_bc_zplus();

  const std::vector<int>& dim = patch.dim();

  SpatialOps::OperatorDatabase& opDB = patch.operator_database();
  opDB.register_new_operator( new XDivT ( SpatialOps::build_two_point_coef_collection( -1.0/spacing[0], 1.0/spacing[1] ) ) );
  opDB.register_new_operator( new XGradT( SpatialOps::build_two_point_coef_collection( -1.0/spacing[0], 1.0/spacing[1] ) ) );
}

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

int main( int argc, char* argv[] )
{
  int nvar = 10;
  int nstep = 200000/nvar;
  std::vector<int> dim(3,1);
  dim[0] = 25;
  dim[1] = 10;
  dim[2] = 10;
  bool doDiffusion, coupledSrc, indepSrc;

#ifdef FIELD_EXPRESSION_THREADS
  int soThreadCount, exprThreadCount;
#endif

  // parse the command line options input describing the problem
  {
    po::options_description desc("Supported Options");
    desc.add_options()
      ( "help", "print help message" )
      ( "nvar", po::value<int>(&nvar)->default_value(10), "number of variables" )
      ( "nstep", po::value<int>(&nstep)->default_value(1000), "number of 'iterations'" )
      ( "diffusion",      po::bool_switch(&doDiffusion)->default_value(false), "activates diffusive flux terms" )
      ( "coupled-source", po::bool_switch(&coupledSrc )->default_value(false), "activates coupled source term" )
      ( "indep-source",   po::bool_switch(&indepSrc   )->default_value(false), "activates independent source term" )
#     ifdef FIELD_EXPRESSION_THREADS
      ( "tc", po::value<int>(&soThreadCount)->default_value(1), "Number of threads for Nebo")
      ( "etc", po::value<int>(&exprThreadCount)->default_value(NTHREADS), "Number of threads for ExprLib")
#     endif
      ( "nx", po::value<int>(&dim[0])->default_value(25), "Grid in x" )
      ( "ny", po::value<int>(&dim[1])->default_value(10), "Grid in y" )
      ( "nz", po::value<int>(&dim[2])->default_value(10), "Grid in z" );

    po::variables_map args;
    po::store( po::parse_command_line(argc,argv,desc), args );
    po::notify(args);

    if (args.count("help")) {
      cout << desc << endl;
      return 1;
    }
  }

# ifdef FIELD_EXPRESSION_THREADS
  cout << "FIELD_EXPRESSION_THREADS is ON" << endl;
  SpatialOps::set_hard_thread_count( NTHREADS );
  SpatialOps::set_soft_thread_count( soThreadCount );
  Expr::set_hard_thread_count( NTHREADS );
  Expr::set_soft_thread_count( exprThreadCount );
  sleep(1);
# endif

  if( coupledSrc && indepSrc ){
    cout << "cannot use coupled & independent source together" << endl;
    return -1;
  }
  cout << endl;
  if( coupledSrc  ) cout << " Using coupled source terms" << endl;
  if( indepSrc    ) cout << " Using independent source terms" << endl;
  if( doDiffusion ) cout << " Using diffusion" << endl;

  cout << " [nx,ny,nz] = [" << dim[0] << "," << dim[1] << "," << dim[2] << "]" << endl
       << " nsteps = " << nstep << endl
       << " nvar   = " << nvar << endl
#     ifdef FIELD_EXPRESSION_THREADS
       << " SpatialOps NTHREADS (can set at runtime) = " << SpatialOps::get_soft_thread_count()
       << " out of " << SpatialOps::get_hard_thread_count() << endl
       << " ExprLib    NTHREADS (can set at runtime) = "
       << SpatialOps::ThreadPool::get_pool_size() << " out of "
       << SpatialOps::ThreadPool::get_pool_capacity() << endl
#     endif
       << endl;

  std::vector<double> length(3,1);
  std::vector<double> spacing(3,1.0);
  for( int i=0; i<3; ++i ){
    if( dim[i]>1 ) spacing[i] = length[i]/dim[i];
  }

  PatchT patch( dim[0], dim[1], dim[2] );

  if( doDiffusion ) build_operators( patch, spacing );

  Expr::ExpressionFactory exprFactory;
  const Expr::TagList rhsTags = register_expressions( exprFactory, nvar, doDiffusion, coupledSrc, indepSrc );

  Expr::TimeStepper timeStepper( exprFactory );

  size_t ivar=0;
  BOOST_FOREACH( const Expr::Tag& tag, rhsTags ){
    timeStepper.add_equation<VolT>( "var"+boost::lexical_cast<std::string>(ivar++), tag, SpatialOps::GhostData(1) );
  }

  timeStepper.finalize( patch.field_manager_list(),
                        patch.operator_database(),
                        patch.field_info() );

  {
    std::ofstream fout("tree.dot");
    timeStepper.get_tree()->write_tree(fout);
  }

  try{
    const boost::posix_time::ptime start = boost::posix_time::microsec_clock::universal_time();

    for( int i=0; i<nstep; ++i ){
      timeStepper.step( 0 );  // timestep 0 avoids bizarre behavior that can come from the source terms, which are entirely artificial.
      cout << i+1 << " of " << nstep << endl;
    }

    const boost::posix_time::time_duration elapsed = boost::posix_time::microsec_clock::universal_time() - start;

    cout << endl << " time_per_step_per_var "<< elapsed.total_microseconds()*1e-6/nstep <<endl;
  }
  catch( std::runtime_error& err ){
    cout << err.what() << endl;
    return -1;
  }

  return 0;
}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <algorithm>
#include <stack>
#include <boost/graph/graphviz.hpp>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Spatial.h>
#include <rl/math/Unit.h>

#include "Exception.h"
#include "Dynamic.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		Dynamic::Dynamic() :
			Kinematic(),
			G(),
			invM(),
			invMx(),
			M(),
			V()
		{
		}

		Dynamic::~Dynamic()
		{
		}

		void
		Dynamic::calculateCentrifugalCoriolis()
		{
			this->calculateCentrifugalCoriolis(this->V);
		}

		void
		Dynamic::calculateCentrifugalCoriolis(::rl::math::Vector& V)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setAcceleration(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			this->inverseDynamics();
			V = this->getTorque();

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateGravity()
		{
			this->calculateGravity(this->G);
		}

		void
		Dynamic::calculateGravity(::rl::math::Vector& G)
		{
			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setAcceleration(tmp);

			this->inverseDynamics();
			G = this->getTorque();
		}

		void
		Dynamic::calculateMassMatrix()
		{
			this->calculateMassMatrix(this->M);
		}

		void
		Dynamic::calculateMassMatrix(::rl::math::Matrix& M)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setAcceleration(tmp);
				this->inverseDynamics();

				M.col(i) = this->getTorque();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateMassMatrixInverse()
		{
			this->calculateMassMatrixInverse(this->invM);
		}

		void
		Dynamic::calculateMassMatrixInverse(::rl::math::Matrix& invM)
		{
			::rl::math::Vector3 g = this->getWorldGravity();

			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setVelocity(tmp);
			this->setWorldGravity(::rl::math::Vector3::Zero());

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setTorque(tmp);
				this->forwardDynamics();

				invM.col(i) = this->getAcceleration();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse()
		{
			this->calculateOperationalMassMatrixInverse(this->J, this->invM, this->invMx);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse(const ::rl::math::Matrix& J, const ::rl::math::Matrix& invM, ::rl::math::Matrix& invMx) const
		{
			invMx = J * invM * J.transpose();
		}

		void
		Dynamic::forwardDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->forwardDynamics2();
			}

			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics3();
			}
		}

		const ::rl::math::Vector&
		Dynamic::getCentrifugalCoriolis() const
		{
			return this->V;
		}

		const ::rl::math::Vector&
		Dynamic::getGravity() const
		{
			return this->G;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrixInverse() const
		{
			return this->invM;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrix() const
		{
			return this->M;
		}

		const ::rl::math::Matrix&
		Dynamic::getOperationalMassMatrixInverse() const
		{
			return this->invMx;
		}

		void
		Dynamic::inverseDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->inverseDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseDynamics2();
			}
		}

		void
		Dynamic::inverseForce()
		{
			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseForce();
			}
		}

		void
		Dynamic::update()
		{
			Kinematic::update();

			this->M = ::rl::math::Matrix::Identity(this->getDof(), this->getDof());
			this->V = ::rl::math::Vector::Zero(this->getDof());
			this->G = ::rl::math::Vector::Zero(this->getDof());
			this->invM = ::rl::math::Matrix::Identity(this->getDof(), this->getDof());
			this->invMx = ::rl::math::Matrix::Identity(6 * this->getOperationalDof(), 6 * this->getOperationalDof());
		}
	}
}


// Copyright (c) 2016, Monea Research Labs
//
// Author: Shen Noether <shen.noether@gmx.com>
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other
//    materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors may be
//    used to endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
// THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <boost/lexical_cast.hpp>
#include "misc_log_ex.h"
#include "rctOps.h"
using namespace crypto;
using namespace std;

#undef MONEA_DEFAULT_LOG_CATEGORY
#define MONEA_DEFAULT_LOG_CATEGORY "ringct"

#define CHECK_AND_ASSERT_THROW_MES_L1(expr, message) {if(!(expr)) {MWARNING(message); throw std::runtime_error(message);}}

namespace rct {

    //Various key initialization functions

    //initializes a key matrix;
    //first parameter is rows,
    //second is columns
    keyM keyMInit(size_t rows, size_t cols) {
        keyM rv(cols);
        size_t i = 0;
        for (i = 0 ; i < cols ; i++) {
            rv[i] = keyV(rows);
        }
        return rv;
    }

    //Various key generation functions

    //generates a random scalar which can be used as a secret key or mask
    void skGen(key &sk) {
        sk = crypto::rand<key>();
        sc_reduce32(sk.bytes);
    }

    //generates a random scalar which can be used as a secret key or mask
    key skGen() {
        key sk = crypto::rand<key>();
        sc_reduce32(sk.bytes);
        return sk;
    }

    //Generates a vector of secret key
    //Mainly used in testing
    keyV skvGen(size_t rows ) {
        CHECK_AND_ASSERT_THROW_MES(rows > 0, "0 keys requested");
        keyV rv(rows);
        size_t i = 0;
        crypto::rand(rows * sizeof(key), (uint8_t*)&rv[0]);
        for (i = 0 ; i < rows ; i++) {
            sc_reduce32(rv[i].bytes);
        }
        return rv;
    }

    //generates a random curve point (for testing)
    key  pkGen() {
        key sk = skGen();
        key pk = scalarmultBase(sk);
        return pk;
    }

    //generates a random secret and corresponding public key
    void skpkGen(key &sk, key &pk) {
        skGen(sk);
        scalarmultBase(pk, sk);
    }

    //generates a random secret and corresponding public key
    tuple<key, key>  skpkGen() {
        key sk = skGen();
        key pk = scalarmultBase(sk);
        return make_tuple(sk, pk);
    }

    //generates C =aG + bH from b, a is given..
    void genC(key & C, const key & a, xmr_amount amount) {
        key bH = scalarmultH(d2h(amount));
        addKeys1(C, a, bH);
    }

    //generates a <secret , public> / Pedersen commitment to the amount
    tuple<ctkey, ctkey> ctskpkGen(xmr_amount amount) {
        ctkey sk, pk;
        skpkGen(sk.dest, pk.dest);
        skpkGen(sk.mask, pk.mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(pk.mask, pk.mask, bH);
        return make_tuple(sk, pk);
    }

    //generates a <secret , public> / Pedersen commitment but takes bH as input
    tuple<ctkey, ctkey> ctskpkGen(const key &bH) {
        ctkey sk, pk;
        skpkGen(sk.dest, pk.dest);
        skpkGen(sk.mask, pk.mask);
        addKeys(pk.mask, pk.mask, bH);
        return make_tuple(sk, pk);
    }

    key zeroCommit(xmr_amount amount) {
        key mask = identity();
        mask = scalarmultBase(mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(mask, mask, bH);
        return mask;
    }

    key commit(xmr_amount amount, const key &mask) {
        key c = scalarmultBase(mask);
        key am = d2h(amount);
        key bH = scalarmultH(am);
        addKeys(c, c, bH);
        return c;
    }

    //generates a random uint long long (for testing)
    xmr_amount randXmrAmount(xmr_amount upperlimit) {
        return h2d(skGen()) % (upperlimit);
    }

    //Scalar multiplications of curve points

    //does a * G where a is a scalar and G is the curve basepoint
    void scalarmultBase(key &aG,const key &a) {
        ge_p3 point;
        sc_reduce32copy(aG.bytes, a.bytes); //do this beforehand!
        ge_scalarmult_base(&point, aG.bytes);
        ge_p3_tobytes(aG.bytes, &point);
    }

    //does a * G where a is a scalar and G is the curve basepoint
    key scalarmultBase(const key & a) {
        ge_p3 point;
        key aG;
        sc_reduce32copy(aG.bytes, a.bytes); //do this beforehand
        ge_scalarmult_base(&point, aG.bytes);
        ge_p3_tobytes(aG.bytes, &point);
        return aG;
    }

    //does a * P where a is a scalar and P is an arbitrary point
    void scalarmultKey(key & aP, const key &P, const key &a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, P.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        ge_tobytes(aP.bytes, &R);
    }

    //does a * P where a is a scalar and P is an arbitrary point
    key scalarmultKey(const key & P, const key & a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, P.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        key aP;
        ge_tobytes(aP.bytes, &R);
        return aP;
    }

    //Computes aH where H= toPoint(cn_fast_hash(G)), G the basepoint
    key scalarmultH(const key & a) {
        ge_p3 A;
        ge_p2 R;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A, H.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_scalarmult(&R, a.bytes, &A);
        key aP;
        ge_tobytes(aP.bytes, &R);
        return aP;
    }

    //Curve addition / subtractions

    //for curve points: AB = A + B
    void addKeys(key &AB, const key &A, const key &B) {
        ge_p3 B2, A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_cached tmp2;
        ge_p3_to_cached(&tmp2, &B2);
        ge_p1p1 tmp3;
        ge_add(&tmp3, &A2, &tmp2);
        ge_p1p1_to_p3(&A2, &tmp3);
        ge_p3_tobytes(AB.bytes, &A2);
    }

    rct::key addKeys(const key &A, const key &B) {
      key k;
      addKeys(k, A, B);
      return k;
    }

    //addKeys1
    //aGB = aG + B where a is a scalar, G is the basepoint, and B is a point
    void addKeys1(key &aGB, const key &a, const key & B) {
        key aG = scalarmultBase(a);
        addKeys(aGB, aG, B);
    }

    //addKeys2
    //aGbB = aG + bB where a, b are scalars, G is the basepoint and B is a point
    void addKeys2(key &aGbB, const key &a, const key &b, const key & B) {
        ge_p2 rv;
        ge_p3 B2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_double_scalarmult_base_vartime(&rv, b.bytes, &B2, a.bytes);
        ge_tobytes(aGbB.bytes, &rv);
    }

    //Does some precomputation to make addKeys3 more efficient
    // input B a curve point and output a ge_dsmp which has precomputation applied
    void precomp(ge_dsmp rv, const key & B) {
        ge_p3 B2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_dsm_precomp(rv, &B2);
    }

    //addKeys3
    //aAbB = a*A + b*B where a, b are scalars, A, B are curve points
    //B must be input after applying "precomp"
    void addKeys3(key &aAbB, const key &a, const key &A, const key &b, const ge_dsmp B) {
        ge_p2 rv;
        ge_p3 A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_double_scalarmult_precomp_vartime(&rv, a.bytes, &A2, b.bytes, B);
        ge_tobytes(aAbB.bytes, &rv);
    }

    //addKeys3
    //aAbB = a*A + b*B where a, b are scalars, A, B are curve points
    //A and B must be input after applying "precomp"
    void addKeys3(key &aAbB, const key &a, const ge_dsmp A, const key &b, const ge_dsmp B) {
        ge_p2 rv;
        ge_double_scalarmult_precomp_vartime2(&rv, a.bytes, A, b.bytes, B);
        ge_tobytes(aAbB.bytes, &rv);
    }

    //subtract Keys (subtracts curve points)
    //AB = A - B where A, B are curve points
    void subKeys(key & AB, const key &A, const key &B) {
        ge_p3 B2, A2;
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&B2, B.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&A2, A.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_cached tmp2;
        ge_p3_to_cached(&tmp2, &B2);
        ge_p1p1 tmp3;
        ge_sub(&tmp3, &A2, &tmp2);
        ge_p1p1_to_p3(&A2, &tmp3);
        ge_p3_tobytes(AB.bytes, &A2);
    }

    //checks if A, B are equal in terms of bytes (may say no if one is a non-reduced scalar)
    //without doing curve operations
    bool equalKeys(const key & a, const key & b) {
        bool rv = true;
        for (int i = 0; i < 32; ++i) {
          if (a.bytes[i] != b.bytes[i]) {
            rv = false;
          }
        }
        return rv;
    }

    //Hashing - cn_fast_hash
    //be careful these are also in crypto namespace
    //cn_fast_hash for arbitrary multiples of 32 bytes
    void cn_fast_hash(key &hash, const void * data, const std::size_t l) {
        keccak((const uint8_t *)data, l, hash.bytes, 32);
    }

    void hash_to_scalar(key &hash, const void * data, const std::size_t l) {
        cn_fast_hash(hash, data, l);
        sc_reduce32(hash.bytes);
    }

    //cn_fast_hash for a 32 byte key
    void cn_fast_hash(key & hash, const key & in) {
        keccak((const uint8_t *)in.bytes, 32, hash.bytes, 32);
    }

    void hash_to_scalar(key & hash, const key & in) {
        cn_fast_hash(hash, in);
        sc_reduce32(hash.bytes);
    }

    //cn_fast_hash for a 32 byte key
    key cn_fast_hash(const key & in) {
        key hash;
        keccak((const uint8_t *)in.bytes, 32, hash.bytes, 32);
        return hash;
    }

     key hash_to_scalar(const key & in) {
        key hash = cn_fast_hash(in);
        sc_reduce32(hash.bytes);
        return hash;
     }

    //cn_fast_hash for a 128 byte unsigned char
    key cn_fast_hash128(const void * in) {
        key hash;
        keccak((const uint8_t *)in, 128, hash.bytes, 32);
        return hash;
    }

    key hash_to_scalar128(const void * in) {
        key hash = cn_fast_hash128(in);
        sc_reduce32(hash.bytes);
        return hash;
    }

    //cn_fast_hash for multisig purpose
    //This takes the outputs and commitments
    //and hashes them into a 32 byte sized key
    key cn_fast_hash(const ctkeyV &PC) {
        if (PC.empty()) return rct::hash2rct(crypto::cn_fast_hash("", 0));
        key rv;
        cn_fast_hash(rv, &PC[0], 64*PC.size());
        return rv;
    }

    key hash_to_scalar(const ctkeyV &PC) {
        key rv = cn_fast_hash(PC);
        sc_reduce32(rv.bytes);
        return rv;
    }

   //cn_fast_hash for a key-vector of arbitrary length
   //this is useful since you take a number of keys
   //put them in the key vector and it concatenates them
   //and then hashes them
   key cn_fast_hash(const keyV &keys) {
       if (keys.empty()) return rct::hash2rct(crypto::cn_fast_hash("", 0));
       key rv;
       cn_fast_hash(rv, &keys[0], keys.size() * sizeof(keys[0]));
       //dp(rv);
       return rv;
   }

   key hash_to_scalar(const keyV &keys) {
       key rv = cn_fast_hash(keys);
       sc_reduce32(rv.bytes);
       return rv;
   }

   key cn_fast_hash(const key64 keys) {
      key rv;
      cn_fast_hash(rv, &keys[0], 64 * sizeof(keys[0]));
      //dp(rv);
      return rv;
   }

   key hash_to_scalar(const key64 keys) {
       key rv = cn_fast_hash(keys);
       sc_reduce32(rv.bytes);
       return rv;
   }

    key hashToPointSimple(const key & hh) {
        key pointk;
        ge_p1p1 point2;
        ge_p2 point;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        CHECK_AND_ASSERT_THROW_MES_L1(ge_frombytes_vartime(&res, h.bytes) == 0, "ge_frombytes_vartime failed at "+boost::lexical_cast<std::string>(__LINE__));
        ge_p3_to_p2(&point, &res);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
        return pointk;
    }

    key hashToPoint(const key & hh) {
        key pointk;
        ge_p2 point;
        ge_p1p1 point2;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        ge_fromfe_frombytes_vartime(&point, h.bytes);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
        return pointk;
    }

    void hashToPoint(key & pointk, const key & hh) {
        ge_p2 point;
        ge_p1p1 point2;
        ge_p3 res;
        key h = cn_fast_hash(hh);
        ge_fromfe_frombytes_vartime(&point, h.bytes);
        ge_mul8(&point2, &point);
        ge_p1p1_to_p3(&res, &point2);
        ge_p3_tobytes(pointk.bytes, &res);
    }

    //sums a vector of curve points (for scalars use sc_add)
    void sumKeys(key & Csum, const keyV &  Cis) {
        identity(Csum);
        size_t i = 0;
        for (i = 0; i < Cis.size(); i++) {
            addKeys(Csum, Csum, Cis[i]);
        }
    }

    //Elliptic Curve Diffie Helman: encodes and decodes the amount b and mask a
    // where C= aG + bH
    void ecdhEncode(ecdhTuple & unmasked, const key & sharedSec) {
        key sharedSec1 = hash_to_scalar(sharedSec);
        key sharedSec2 = hash_to_scalar(sharedSec1);
        //encode
        sc_add(unmasked.mask.bytes, unmasked.mask.bytes, sharedSec1.bytes);
        sc_add(unmasked.amount.bytes, unmasked.amount.bytes, sharedSec2.bytes);
    }
    void ecdhDecode(ecdhTuple & masked, const key & sharedSec) {
        key sharedSec1 = hash_to_scalar(sharedSec);
        key sharedSec2 = hash_to_scalar(sharedSec1);
        //decode
        sc_sub(masked.mask.bytes, masked.mask.bytes, sharedSec1.bytes);
        sc_sub(masked.amount.bytes, masked.amount.bytes, sharedSec2.bytes);
    }
}


#include <Functions/FunctionFactory.h>
#include <Functions/PolygonUtils.h>
#include <Functions/FunctionHelpers.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnFixedString.h>
#include <Columns/ColumnString.h>
#include <Columns/ColumnTuple.h>
#include <Common/ObjectPool.h>
#include <Common/ProfileEvents.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeString.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypesNumber.h>
#include <IO/WriteHelpers.h>
#include <Interpreters/ExpressionActions.h>

#include <string>
#include <memory>

namespace ProfileEvents
{
    extern const Event PolygonsAddedToPool;
    extern const Event PolygonsInPoolAllocatedBytes;
}

namespace DB
{

namespace ErrorCodes
{
    extern const int TOO_FEW_ARGUMENTS_FOR_FUNCTION;
    extern const int BAD_ARGUMENTS;
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int ILLEGAL_COLUMN;
}

namespace
{

template <typename Polygon, typename PointInPolygonImpl>
UInt8 callPointInPolygonImplWithPool(Float64 x, Float64 y, Polygon & polygon)
{
    using Pool = ObjectPoolMap<PointInPolygonImpl, std::string>;
    /// C++11 has thread-safe function-local statics on most modern compilers.
    static Pool known_polygons;

    auto factory = [& polygon]()
    {
        auto ptr = std::make_unique<PointInPolygonImpl>(polygon);

        ProfileEvents::increment(ProfileEvents::PolygonsAddedToPool);
        ProfileEvents::increment(ProfileEvents::PolygonsInPoolAllocatedBytes, ptr->getAllocatedBytes());

        return ptr.release();
    };

    std::string serialized_polygon = serialize(polygon);
    auto impl = known_polygons.get(serialized_polygon, factory);

    return impl->contains(x, y);
}

template <typename Polygon, typename PointInPolygonImpl>
UInt8 callPointInPolygonImpl(Float64 x, Float64 y, Polygon & polygon)
{
    PointInPolygonImpl impl(polygon);
    return impl.contains(x, y);
}

}

template <typename PointInConstPolygonImpl, typename PointInNonConstPolygonImpl>
class FunctionPointInPolygon : public IFunction
{
public:
    using CoordinateType = Float64;

    using Point = boost::geometry::model::d2::point_xy<CoordinateType>;
    using Polygon = boost::geometry::model::polygon<Point, false>;
    using Box = boost::geometry::model::box<Point>;

    static inline const char * name = "pointInPolygon";

    explicit FunctionPointInPolygon(bool validate_) : validate(validate_) {}

    static FunctionPtr create(const Context & context)
    {
        return std::make_shared<FunctionPointInPolygon<PointInConstPolygonImpl, PointInNonConstPolygonImpl>>(
            context.getSettingsRef().validate_polygons);
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return true;
    }

    size_t getNumberOfArguments() const override
    {
        return 0;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes & arguments) const override
    {
        if (arguments.size() < 2)
        {
            throw Exception("Too few arguments", ErrorCodes::TOO_FEW_ARGUMENTS_FOR_FUNCTION);
        }

        auto validate_tuple = [this](size_t i, const DataTypeTuple * tuple)
        {
            if (tuple == nullptr)
                throw Exception(getMessagePrefix(i) + " must contain a tuple", ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

            const DataTypes & elements = tuple->getElements();

            if (elements.size() != 2)
                throw Exception(getMessagePrefix(i) + " must have exactly two elements", ErrorCodes::BAD_ARGUMENTS);

            for (auto j : ext::range(0, elements.size()))
            {
                if (!isNativeNumber(elements[j]))
                {
                    throw Exception(getMessagePrefix(i) + " must contain numeric tuple at position " + toString(j + 1),
                                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);
                }
            }
        };

        validate_tuple(0, checkAndGetDataType<DataTypeTuple>(arguments[0].get()));

        if (arguments.size() == 2)
        {
            const auto * array = checkAndGetDataType<DataTypeArray>(arguments[1].get());
            if (array == nullptr)
                throw Exception(getMessagePrefix(1) + " must contain an array of tuples or an array of arrays of tuples.",
                                ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

            const auto * nested_array = checkAndGetDataType<DataTypeArray>(array->getNestedType().get());
            if (nested_array != nullptr)
            {
                array = nested_array;
            }

            validate_tuple(1, checkAndGetDataType<DataTypeTuple>(array->getNestedType().get()));
        }
        else
        {
            for (size_t i = 1; i < arguments.size(); i++)
            {
                const auto * array = checkAndGetDataType<DataTypeArray>(arguments[i].get());
                if (array == nullptr)
                    throw Exception(getMessagePrefix(i) + " must contain an array of tuples",
                                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

                validate_tuple(i, checkAndGetDataType<DataTypeTuple>(array->getNestedType().get()));
            }
        }

        return std::make_shared<DataTypeUInt8>();
    }

    void executeImpl(Block & block, const ColumnNumbers & arguments, size_t result, size_t input_rows_count) override
    {
        const IColumn * point_col = block.getByPosition(arguments[0]).column.get();
        const auto * const_tuple_col = checkAndGetColumn<ColumnConst>(point_col);
        if (const_tuple_col)
            point_col = &const_tuple_col->getDataColumn();

        const auto * tuple_col = checkAndGetColumn<ColumnTuple>(point_col);
        if (!tuple_col)
            throw Exception("First argument for function " + getName() + " must be constant array of tuples.",
                            ErrorCodes::ILLEGAL_COLUMN);

        const auto & tuple_columns = tuple_col->getColumns();

        const IColumn * poly_col = block.getByPosition(arguments[1]).column.get();
        const auto * const_poly_col = checkAndGetColumn<ColumnConst>(poly_col);

        bool point_is_const = const_tuple_col != nullptr;
        bool poly_is_const = const_poly_col != nullptr;

        auto call_impl = poly_is_const
            ? callPointInPolygonImplWithPool<Polygon, PointInConstPolygonImpl>
            : callPointInPolygonImpl<Polygon, PointInNonConstPolygonImpl>;

        size_t size = point_is_const && poly_is_const ? 1 : input_rows_count;
        auto execution_result = ColumnVector<UInt8>::create(size);
        auto & data = execution_result->getData();

        Polygon polygon;
        for (auto i : ext::range(0, size))
        {
            if (!poly_is_const || i == 0)
            {
                polygon = parsePolygon(block, arguments, i);
            }

            size_t point_index = point_is_const ? 0 : i;
            data[i] = call_impl(tuple_columns[0]->getFloat64(point_index), tuple_columns[1]->getFloat64(point_index), polygon);
        }

        auto & result_column = block.safeGetByPosition(result).column;
        result_column = std::move(execution_result);
        if (point_is_const && poly_is_const)
            result_column = ColumnConst::create(result_column, const_tuple_col->size());
    }

private:
    bool validate;

    std::string getMessagePrefix(size_t i) const
    {
        return "Argument " + toString(i + 1) + " for function " + getName();
    }

    Polygon parsePolygonFromSingleColumn(Block & block, const ColumnNumbers & arguments, size_t i) const
    {
        const auto & poly = block.getByPosition(arguments[1]).column.get();
        const auto * column_const = checkAndGetColumn<ColumnConst>(poly);
        const auto * array_col =
            column_const ? checkAndGetColumn<ColumnArray>(column_const->getDataColumn()) : checkAndGetColumn<ColumnArray>(poly);

        if (!array_col)
            throw Exception(getMessagePrefix(1) + " must contain an array of tuples or an array of arrays of tuples",
                            ErrorCodes::ILLEGAL_COLUMN);

        const auto * nested_array_col = checkAndGetColumn<ColumnArray>(array_col->getData());
        const auto & tuple_data = nested_array_col ? nested_array_col->getData() : array_col->getData();
        const auto & tuple_col = checkAndGetColumn<ColumnTuple>(tuple_data);
        if (!tuple_col)
            throw Exception(getMessagePrefix(1) + " must contain an array of tuples or an array of arrays of tuples",
                            ErrorCodes::ILLEGAL_COLUMN);

        const auto & tuple_columns = tuple_col->getColumns();
        const auto & x_column = tuple_columns[0];
        const auto & y_column = tuple_columns[1];

        auto parse_polygon_part = [&x_column, &y_column](auto & container, size_t l, size_t r)
        {
            for (auto j : ext::range(l, r))
            {
                CoordinateType x_coord = x_column->getFloat64(j);
                CoordinateType y_coord = y_column->getFloat64(j);

                container.push_back(Point(x_coord, y_coord));
            }
        };

        Polygon polygon;
        if (nested_array_col)
        {
            for (auto j : ext::range(array_col->getOffsets()[i - 1], array_col->getOffsets()[i]))
            {
                size_t l = nested_array_col->getOffsets()[j - 1];
                size_t r = nested_array_col->getOffsets()[j];
                if (polygon.outer().empty())
                {
                    parse_polygon_part(polygon.outer(), l, r);
                }
                else
                {
                    polygon.inners().emplace_back();
                    parse_polygon_part(polygon.inners().back(), l, r);
                }
            }
        }
        else
        {
            size_t l = array_col->getOffsets()[i - 1];
            size_t r = array_col->getOffsets()[i];

            parse_polygon_part(polygon.outer(), l, r);
        }

        return polygon;
    }

    Polygon parsePolygonFromMultipleColumns(Block & block, const ColumnNumbers & arguments, size_t) const
    {
        Polygon polygon;

        for (size_t i = 1; i < arguments.size(); ++i)
        {
            const auto * const_col = checkAndGetColumn<ColumnConst>(block.getByPosition(arguments[i]).column.get());
            if (!const_col)
                throw Exception("Multi-argument version of function " + getName() + " works only with const polygon",
                            ErrorCodes::BAD_ARGUMENTS);

            const auto * array_col = checkAndGetColumn<ColumnArray>(&const_col->getDataColumn());
            const auto * tuple_col = array_col ? checkAndGetColumn<ColumnTuple>(&array_col->getData()) : nullptr;

            if (!tuple_col)
                throw Exception(getMessagePrefix(i) + " must be constant array of tuples", ErrorCodes::ILLEGAL_COLUMN);

            const auto & tuple_columns = tuple_col->getColumns();
            const auto & column_x = tuple_columns[0];
            const auto & column_y = tuple_columns[1];

            if (!polygon.outer().empty())
                polygon.inners().emplace_back();

            auto & container = polygon.outer().empty() ? polygon.outer() : polygon.inners().back();

            auto size = column_x->size();

            if (size == 0)
                throw Exception(getMessagePrefix(i) + " shouldn't be empty.", ErrorCodes::ILLEGAL_COLUMN);

            for (auto j : ext::range(0, size))
            {
                CoordinateType x_coord = column_x->getFloat64(j);
                CoordinateType y_coord = column_y->getFloat64(j);
                container.push_back(Point(x_coord, y_coord));
            }
        }

        return polygon;
    }

    Polygon parsePolygon(Block & block, const ColumnNumbers & arguments, size_t i) const
    {
        Polygon polygon;
        if (arguments.size() == 2)
        {
            polygon = parsePolygonFromSingleColumn(block, arguments, i);
        }
        else
        {
            polygon = parsePolygonFromMultipleColumns(block, arguments, i);
        }

        boost::geometry::correct(polygon);

#if !defined(__clang_analyzer__) /// It does not like boost.
        if (validate)
        {
            std::string failure_message;
            auto is_valid = boost::geometry::is_valid(polygon, failure_message);
            if (!is_valid)
                throw Exception("Polygon is not valid: " + failure_message, ErrorCodes::BAD_ARGUMENTS);
        }
#endif
        return polygon;
    }
};

void registerFunctionPointInPolygon(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPointInPolygon<PointInPolygonWithGrid<Float64>, PointInPolygonTrivial<Float64>>>();
}

}


#include "EQ_Tests.h"

#include "Common.h"
#include "Network/BtEndpoint.h"
#include "Common/Defines.h"
#include "OPPRF/OPPRFReceiver.h"
#include "OPPRF/OPPRFSender.h"
#include "OPPRF/binSet.h"
#include "Common/Log.h"

#include "NChooseOne/KkrtNcoOtReceiver.h"
#include "NChooseOne/KkrtNcoOtSender.h"

#include "NChooseOne/Oos/OosNcoOtReceiver.h"
#include "NChooseOne/Oos/OosNcoOtSender.h"

#include "Hashing/CuckooHasher1.h"
#include "Hashing/BitPosition.h"
#include "Common/Log.h"
#include "Common/Log1.h"
#include <array>

#include "NTL/GF2EX.h"
#include "NTL/GF2XFactoring.h"
#include <NTL/GF2E.h>
#include "NTL/GF2EX.h"
#include <NTL/ZZ_pE.h>
#include <NTL/vec_ZZ_pE.h>
#include "NTL/GF2EX.h"
#include "NTL/ZZ_p.h"
#include "NTL/GF2EX.h"
#include "NTL/GF2XFactoring.h"
#include "Common/Log.h"

using namespace osuCrypto;
#define PRINT
//#define BIN_PRINT

void EQ_EmptrySet_Test_Impl()
{
	Log::setThreadName("Sender");
	Log::setThreadName("CP_Test_Thread");
	std::string name("psi");
	u64 repeatCount = 1;
	u64 setPower = 8;
	u64 bitSize = 128;
	u64 setSize = pow(2, setPower);
	//u64 setSize = 10;
	u64 psiSecParam = 40, numThreads(1);

	BtIOService ios(0);
	BtEndpoint ep0(ios, "localhost", 1212, true, name);
	BtEndpoint ep1(ios, "localhost", 1212, false, name);

	std::vector<Channel*> recvChls(numThreads), sendChls(numThreads);
	for (u64 i = 0; i < numThreads; ++i)
		recvChls[i] = &ep1.addChannel(name + std::to_string(i), name + std::to_string(i));
	for (u64 i = 0; i < numThreads; ++i)
		sendChls[i] = &ep0.addChannel(name + std::to_string(i), name + std::to_string(i));

	PRNG prng(_mm_set_epi32(4253465, 34354565, 234435, 23987045));

	std::vector<u32> sendSet(setSize), recvSet(setSize);
	for (u64 i = 0; i < setSize; ++i)
	{
		sendSet[i] = prng.get<u32>();
		recvSet[i] = sendSet[i];
	}

	OPPRFSender send;
	OPPRFReceiver recv;
	binSet binSend;
	binSet binRecv;

	auto sendThrd = std::thread([&]() {
		binSend.init(0, 2, setSize, psiSecParam, 0);

	});

	auto recvThrd = std::thread([&]() {
		binRecv.init(1, 2, setSize, psiSecParam, 0);

	});

	sendThrd.join();
	recvThrd.join();

	for (u64 i = 0; i < numThreads; ++i)
	{
		sendChls[i]->close();
		recvChls[i]->close();
	}

	ep0.stop();
	ep1.stop();
	ios.stop();
}