// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief performance.cpp

\details
Test runtime performance.

Output:
@verbatim

multiplying ublas::matrix<double>(1000, 1000) : 25.03 seconds
multiplying ublas::matrix<quantity>(1000, 1000) : 24.49 seconds
tiled_matrix_multiply<double>(1000, 1000) : 1.12 seconds
tiled_matrix_multiply<quantity>(1000, 1000) : 1.16 seconds
solving y' = 1 - x + 4 * y with double: 1.97 seconds
solving y' = 1 - x + 4 * y with quantity: 1.84 seconds

@endverbatim
**/

#define _SCL_SECURE_NO_WARNINGS

#include <cstdlib>
#include <ctime>
#include <algorithm>
#include <iostream>
#include <iomanip>

#include <boost/config.hpp>
#include <boost/timer.hpp>
#include <boost/utility/result_of.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4267; disable:4127; disable:4244; disable:4100)
#endif

#include <boost/numeric/ublas/matrix.hpp>

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>

enum {
    tile_block_size = 24
};

template<class T0, class T1, class Out>
void tiled_multiply_carray_inner(T0* first,
                                 T1* second,
                                 Out* out,
                                 int totalwidth,
                                 int width2,
                                 int height1,
                                 int common) {
    for(int j = 0; j < height1; ++j) {
        for(int i = 0; i < width2; ++i) {
            Out value = out[j * totalwidth + i];
            for(int k = 0; k < common; ++k) {
                value += first[k + totalwidth * j] * second[k * totalwidth + i];
            }
            out[j * totalwidth + i] = value;
        }
    }
}

template<class T0, class T1, class Out>
void tiled_multiply_carray_outer(T0* first,
                                 T1* second,
                                 Out* out,
                                 int width2,
                                 int height1,
                                 int common) {
    std::fill_n(out, width2 * height1, Out());
    int j = 0;
    for(; j < height1 - tile_block_size; j += tile_block_size) {
        int i = 0;
        for(; i < width2 - tile_block_size; i += tile_block_size) {
            int k = 0;
            for(; k < common - tile_block_size; k += tile_block_size) {
                tiled_multiply_carray_inner(
                    &first[k + width2 * j],
                    &second[k * width2 + i],
                    &out[j * width2 + i],
                    width2,
                    tile_block_size,
                    tile_block_size,
                    tile_block_size);
            }
            tiled_multiply_carray_inner(
                &first[k + width2 * j],
                &second[k * width2 + i],
                &out[j * width2 + i],
                width2,
                tile_block_size,
                tile_block_size,
                common - k);
        }
        int k = 0;
        for(; k < common - tile_block_size; k += tile_block_size) {
            tiled_multiply_carray_inner(
                &first[k + width2 * j],
                &second[k * width2 + i],
                &out[j * width2 + i],
                width2, width2 - i,
                tile_block_size,
                tile_block_size);
        }
        tiled_multiply_carray_inner(
            &first[k + width2 * j],
            &second[k * width2 + i],
            &out[j * width2 + i],
            width2, width2 - i,
            tile_block_size,
            common - k);
    }
    int i = 0;
    for(; i < width2 - tile_block_size; i += tile_block_size) {
        int k = 0;
        for(; k < common - tile_block_size; k += tile_block_size) {
            tiled_multiply_carray_inner(
                &first[k + width2 * j],
                &second[k * width2 + i],
                &out[j * width2 + i],
                width2,
                tile_block_size,
                height1 - j,
                tile_block_size);
        }
        tiled_multiply_carray_inner(
            &first[k + width2 * j],
            &second[k * width2 + i],
            &out[j * width2 + i],
            width2,
            tile_block_size,
            height1 - j,
            common - k);
    }
    int k = 0;
    for(; k < common - tile_block_size; k += tile_block_size) {
        tiled_multiply_carray_inner(
            &first[k + width2 * j],
            &second[k * width2 + i],
            &out[j * width2 + i],
            width2,
            width2 - i,
            height1 - j,
            tile_block_size);
    }
    tiled_multiply_carray_inner(
        &first[k + width2 * j],
        &second[k * width2 + i],
        &out[j * width2 + i],
        width2,
        width2 - i,
        height1 - j,
        common - k);
}

enum { max_value = 1000};

template<class F, class T, class N, class R>
BOOST_CXX14_CONSTEXPR
R solve_differential_equation(F f, T lower, T upper, N steps, R start) {
    typedef typename F::template result<T, R>::type f_result;
    T h = (upper - lower) / (1.0*steps);
    for(N i = N(); i < steps; ++i) {
        R y = start;
        T x = lower + h * (1.0*i);
        f_result k1 = f(x, y);
        f_result k2 = f(x + h / 2.0, y + h * k1 / 2.0);
        f_result k3 = f(x + h / 2.0, y + h * k2 / 2.0);
        f_result k4 = f(x + h, y + h * k3);
        start = y + h * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
    }
    return(start);
}

using namespace boost::units;

//y' = 1 - x + 4 * y
struct f {
    template<class Arg1, class Arg2> struct result;

    BOOST_CONSTEXPR double operator()(const double& x, const double& y) const {
        return(1.0 - x + 4.0 * y);
    }

    boost::units::quantity<boost::units::si::velocity>
    BOOST_CONSTEXPR operator()(const quantity<si::time>& x,
                               const quantity<si::length>& y) const {
        using namespace boost::units;
        using namespace si;
        return(1.0 * meters / second -
                x * meters / pow<2>(seconds) +
                4.0 * y / seconds );
    }
};

template<>
struct f::result<double,double> {
    typedef double type;
};

template<>
struct f::result<quantity<si::time>, quantity<si::length> > {
    typedef quantity<si::velocity> type;
};

//y' = 1 - x + 4 * y
//y' - 4 * y = 1 - x
//e^(-4 * x) * (dy - 4 * y * dx) = e^(-4 * x) * (1 - x) * dx
//d/dx(y * e ^ (-4 * x)) = e ^ (-4 * x) (1 - x) * dx

//d/dx(y * e ^ (-4 * x)) = e ^ (-4 * x) * dx - x * e ^ (-4 * x) * dx
//d/dx(y * e ^ (-4 * x)) = d/dx((-3/16 + 1/4 * x) * e ^ (-4 * x))
//y * e ^ (-4 * x) = (-3/16 + 1/4 * x) * e ^ (-4 * x) + C
//y = (-3/16 + 1/4 * x) + C/e ^ (-4 * x)
//y = 1/4 * x - 3/16 + C * e ^ (4 * x)

//y(0) = 1
//1 = - 3/16 + C
//C = 19/16
//y(x) = 1/4 * x - 3/16 + 19/16 * e ^ (4 * x)

int main() {
    boost::numeric::ublas::matrix<double> ublas_result;
    {
        boost::numeric::ublas::matrix<double> m1(max_value, max_value);
        boost::numeric::ublas::matrix<double> m2(max_value, max_value);
        std::srand(1492);
        for(int i = 0; i < max_value; ++i) {
            for(int j = 0; j < max_value; ++j) {
                m1(i,j) = std::rand();
                m2(i,j) = std::rand();
            }
        }
        std::cout << "multiplying ublas::matrix<double>("
                  << max_value << ", " << max_value << ") : ";
        boost::timer timer;
        ublas_result = (prod(m1, m2));
        std::cout << timer.elapsed() << " seconds" << std::endl;
    }
    typedef boost::numeric::ublas::matrix<
        boost::units::quantity<boost::units::si::dimensionless>
    > matrix_type;
    matrix_type ublas_resultq;
    {
        matrix_type m1(max_value, max_value);
        matrix_type m2(max_value, max_value);
        std::srand(1492);
        for(int i = 0; i < max_value; ++i) {
            for(int j = 0; j < max_value; ++j) {
                m1(i,j) = std::rand();
                m2(i,j) = std::rand();
            }
        }
        std::cout << "multiplying ublas::matrix<quantity>("
                  << max_value << ", " << max_value << ") : ";
        boost::timer timer;
        ublas_resultq = (prod(m1, m2));
        std::cout << timer.elapsed() << " seconds" << std::endl;
    }
    std::vector<double> cresult(max_value * max_value);
    {
        std::vector<double> m1(max_value * max_value);
        std::vector<double> m2(max_value * max_value);
        std::srand(1492);
        for(int i = 0; i < max_value * max_value; ++i) {
            m1[i] = std::rand();
            m2[i] = std::rand();
        }
        std::cout << "tiled_matrix_multiply<double>("
                  << max_value << ", " << max_value << ") : ";
        boost::timer timer;
        tiled_multiply_carray_outer(
            &m1[0],
            &m2[0],
            &cresult[0],
            max_value,
            max_value,
            max_value);
        std::cout << timer.elapsed() << " seconds" << std::endl;
    }
    std::vector<
        boost::units::quantity<boost::units::si::energy>
    >  cresultq(max_value * max_value);
    {
        std::vector<
            boost::units::quantity<boost::units::si::force>
        > m1(max_value * max_value);
        std::vector<
            boost::units::quantity<boost::units::si::length>
        > m2(max_value * max_value);
        std::srand(1492);
        for(int i = 0; i < max_value * max_value; ++i) {
            m1[i] = std::rand() * boost::units::si::newtons;
            m2[i] = std::rand() * boost::units::si::meters;
        }
        std::cout << "tiled_matrix_multiply<quantity>("
                  << max_value << ", " << max_value << ") : ";
        boost::timer timer;
        tiled_multiply_carray_outer(
            &m1[0],
            &m2[0],
            &cresultq[0],
            max_value,
            max_value,
            max_value);
        std::cout << timer.elapsed() << " seconds" << std::endl;
    }
    for(int i = 0; i < max_value; ++i) {
        for(int j = 0; j < max_value; ++j) {
            const double diff =
                std::abs(ublas_result(i,j) - cresult[i * max_value + j]);
            if(diff > ublas_result(i,j) /1e14) {
                std::cout << std::setprecision(15) << "Uh Oh. ublas_result("
                          << i << "," << j << ") = " << ublas_result(i,j)
                          << std::endl
                          << "cresult[" << i << " * " << max_value << " + "
                          << j << "] = " << cresult[i * max_value + j]
                          << std::endl;
                return(EXIT_FAILURE);
            }
        }
    }
    {
        std::vector<double> values(1000);
        std::cout << "solving y' = 1 - x + 4 * y with double: ";
        boost::timer timer;
        for(int i = 0; i < 1000; ++i) {
            const double x = .1 * i;
            values[i] = solve_differential_equation(f(), 0.0, x, i * 100, 1.0);
        }
        std::cout << timer.elapsed() << " seconds" << std::endl;
        for(int i = 0; i < 1000; ++i) {
            const double x = .1 * i;
            const double value = 1.0/4.0 * x - 3.0/16.0 + 19.0/16.0 * std::exp(4 * x);
            if(std::abs(values[i] - value) > value / 1e9) {
                std::cout << std::setprecision(15) << "i = : " << i
                          << ", value = " << value << " approx = "  << values[i]
                          << std::endl;
                return(EXIT_FAILURE);
            }
        }
    }
    {
        using namespace boost::units;
        using namespace si;
        std::vector<quantity<length> > values(1000);
        std::cout << "solving y' = 1 - x + 4 * y with quantity: ";
        boost::timer timer;
        for(int i = 0; i < 1000; ++i) {
            const quantity<si::time> x = .1 * i * seconds;
            values[i] = solve_differential_equation(
                f(),
                0.0 * seconds,
                x,
                i * 100,
                1.0 * meters);
        }
        std::cout << timer.elapsed() << " seconds" << std::endl;
        for(int i = 0; i < 1000; ++i) {
            const double x = .1 * i;
            const quantity<si::length> value =
                (1.0/4.0 * x - 3.0/16.0 + 19.0/16.0 * std::exp(4 * x)) * meters;
            if(abs(values[i] - value) > value / 1e9) {
                std::cout << std::setprecision(15) << "i = : " << i
                          << ", value = " << value << " approx = "
                          << values[i] << std::endl;
                return(EXIT_FAILURE);
            }
        }
    }
}


//==============================================================================
//         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
//==============================================================================
#define NT2_UNIT_MODULE "nt2::as_index function"

#include <nt2/core/utility/as_index.hpp>
#include <nt2/include/functions/enumerate.hpp>
#include <boost/simd/sdk/simd/meta/vector_of.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE( as_index_0D )
{
  using nt2::as_index;
  using boost::fusion::make_vector;

  NT2_TEST_EQUAL( as_index( make_vector(), make_vector() ), 0u);
}

NT2_TEST_CASE( as_index_1D )
{
  using nt2::as_index;
  using boost::fusion::make_vector;

  NT2_TEST_EQUAL( as_index( make_vector(5), make_vector(0) ), 0u);
  NT2_TEST_EQUAL( as_index( make_vector(5), make_vector(1) ), 1u);
  NT2_TEST_EQUAL( as_index( make_vector(5), make_vector(2) ), 2u);
  NT2_TEST_EQUAL( as_index( make_vector(5), make_vector(3) ), 3u);
  NT2_TEST_EQUAL( as_index( make_vector(5), make_vector(4) ), 4u);
}

NT2_TEST_CASE_TPL ( as_index_1D_simd
                  , (nt2::int32_t)(nt2::uint32_t)(nt2::int64_t)(nt2::uint64_t)
                  )
{
  using nt2::as_index;
  using boost::simd::meta::vector_of;
  using boost::fusion::make_vector;
  using boost::mpl::int_;

  typedef typename vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type idx_t;
  typedef typename boost::dispatch::meta::as_unsigned<idx_t>::type sidx_t;

  NT2_TEST_EQUAL( as_index( make_vector(16)
                          , make_vector(nt2::enumerate<idx_t>(0))
                          )
                , nt2::enumerate<sidx_t>(0)
                );
}

NT2_TEST_CASE( as_index_2D )
{
  using nt2::as_index;
  using boost::fusion::make_vector;

  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(0,0) ), 0u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(1,0) ), 1u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(2,0) ), 2u);

  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(0,1) ), 3u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(1,1) ), 4u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(2,1) ), 5u);

  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(0,2) ), 6u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(1,2) ), 7u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(2,2) ), 8u);

  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(0,3) ), 9u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(1,3) ), 10u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(2,3) ), 11u);

  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(0,4) ), 12u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(1,4) ), 13u);
  NT2_TEST_EQUAL( as_index( make_vector(3,5), make_vector(2,4) ), 14u);
}

NT2_TEST_CASE( as_index_3D )
{
  using nt2::as_index;
  using boost::fusion::make_vector;

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(0,0,0) ), 0u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(1,0,0) ), 1u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(0,1,0) ), 2u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(1,1,0) ), 3u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(0,0,1) ), 4u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(1,0,1) ), 5u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(0,1,1) ), 6u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2), make_vector(1,1,1) ), 7u);
}

NT2_TEST_CASE( as_index_4D )
{
  using nt2::as_index;
  using boost::fusion::make_vector;

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,0,0,0) ), 0u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,0,0,0) ), 1u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,1,0,0) ), 2u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,1,0,0) ), 3u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,0,1,0) ), 4u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,0,1,0) ), 5u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,1,1,0) ), 6u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,1,1,0) ), 7u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,0,0,1) ), 8u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,0,0,1) ), 9u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,1,0,1) ), 10u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,1,0,1) ), 11u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,0,1,1) ), 12u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,0,1,1) ), 13u);

  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(0,1,1,1) ), 14u);
  NT2_TEST_EQUAL( as_index( make_vector(2,2,2,2), make_vector(1,1,1,1) ), 15u);
}


//
//  matrix_gemm.cpp
//  jpcnn
//
//  Created by Peter Warden on 1/9/14.
//  Copyright (c) 2014 Jetpac, Inc. All rights reserved.
//

#include "matrix_ops.h"

#include <stdio.h>
#include <assert.h>
#include <stdint.h>
#include <stdlib.h>

#ifdef USE_ACCELERATE_GEMM
#include <Accelerate/Accelerate.h>
#endif

#ifdef USE_MKL_GEMM
#include <mkl_cblas.h>
#endif // USE_MKL_GEMM

#ifdef USE_ATLAS_GEMM
#include <cblas.h>
#endif // USE_ATLAS_GEMM

#ifdef USE_EIGEN_GEMM
#include <Eigen/Dense>
#endif // USE_EIGEN_GEMM

#ifdef USE_OPENGL
#include "glgemm.h"
#endif // USE_OPENGL

#ifdef USE_NEON
#include <arm_neon.h>
#include <omp.h>
#endif // USE_NEON

#if !defined(USE_ACCELERATE_GEMM) && !defined(USE_MKL_GEMM) && !defined(USE_OPENGL) && !defined(USE_ATLAS_GEMM) && !defined(USE_EIGEN_GEMM) && !defined(USE_QPU_GEMM)
#define USE_NAIVE_GEMM
#endif

#define DO_LOG_OPERATIONS

void matrix_gemm(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  jpfloat_t *a,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

#ifdef DO_LOG_OPERATIONS
  fprintf(stderr, "matrix_gemm(\n  m=%d,\n  n=%d,\n  k=%d,\n  alpha=%f,\n  a=%p,\n  lda=%d,\n  b=%p,\n  ldb=%d,\n  beta=%f,\n  c=%p,\n  ldc=%d)\n",
    m,
    n,
    k,
    alpha,
    a,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc);
#endif // DO_LOG_OPERATIONS

#if defined(USE_NAIVE_GEMM)
  naive_cblas_sgemm(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_OPENGL)
  gl_gemm(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_ACCELERATE_GEMM) || defined(USE_MKL_GEMM) || defined(USE_ATLAS_GEMM)
  cblas_sgemm(
    (CBLAS_ORDER)(order),
    (CBLAS_TRANSPOSE)(transposeA),
    (CBLAS_TRANSPOSE)(transposeB),
    m,
    n,
    k,
    alpha,
    a,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_EIGEN_GEMM)
  eigen_cblas_sgemm(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_QPU_GEMM)
  assert(false); // You need to call the GEMM function directly so it has access to the GPU memory
#else
#error "No GEMM implementation defined"
#endif

}

void matrix_gemm_fixed(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  void *a,
  jpfloat_t aMin,
  jpfloat_t aMax,
  int aBitsPerElement,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

#if defined(USE_OPENGL)
  gl_gemm_fixed(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    aMin,
    aMax,
    aBitsPerElement,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_ACCELERATE_GEMM) || defined(USE_MKL_GEMM) || defined(USE_ATLAS_GEMM) || defined(USE_EIGEN_GEMM)
  cblas_sgemm_fixed(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    aMin,
    aMax,
    aBitsPerElement,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#elif defined(USE_QPU_GEMM)
  assert(false); // You need to call the GEMM function directly so it has access to the GPU memory
#else
  naive_cblas_sgemm_fixed(
    order,
    transposeA,
    transposeB,
    m,
    n,
    k,
    alpha,
    a,
    aMin,
    aMax,
    aBitsPerElement,
    lda,
    b,
    ldb,
    beta,
    c,
    ldc
  );
#endif
}

void naive_cblas_sgemm(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  jpfloat_t *a,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

  assert((transposeA == JPCblasNoTrans) || (transposeA == JPCblasTrans));
  assert(transposeB == JPCblasNoTrans);
  assert(order == JPCblasColMajor);

  int i, j, l;
  for (j = 0; j < n; j++) {
    for (i = 0; i < m; i++) {
      jpfloat_t total = 0.0f;
      for (l = 0; l < k; l++) {
        int aIndex;
        if (transposeA == JPCblasNoTrans) {
          aIndex = ((lda * l) + i);
        } else {
          aIndex = ((lda * i) + l);
        }
        const jpfloat_t aValue = a[aIndex];
        const int bIndex = ((ldb * j) + l);
        const jpfloat_t bValue = b[bIndex];
        total += (aValue * bValue);
      }
      const int cIndex = ((ldc * j) + i);
      const jpfloat_t oldCValue = c[cIndex];
      c[cIndex] = ((alpha * total) + (beta * oldCValue));
    }
  }
}

void naive_cblas_sgemm_fixed(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  void *a,
  jpfloat_t aMin,
  jpfloat_t aMax,
  int aBitsPerElement,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

  assert((transposeA == JPCblasNoTrans) || (transposeA == JPCblasTrans));
  assert(transposeB == JPCblasNoTrans);
  assert(order == JPCblasColMajor);

  const jpfloat_t aRange = ((aMax - aMin) / (1 << aBitsPerElement));

  if (aBitsPerElement == 16) {
    uint16_t* aData = (uint16_t*)(a);
    int i, j, l;
    for (j = 0; j < n; j++) {
      for (i = 0; i < m; i++) {
        jpfloat_t total = 0.0f;
        for (l = 0; l < k; l++) {
          int aIndex;
          if (transposeA == JPCblasNoTrans) {
            aIndex = ((lda * l) + i);
          } else {
            aIndex = ((lda * i) + l);
          }
          uint16_t aIntValue = aData[aIndex];
          const jpfloat_t aValue = aMin + (aIntValue * aRange);
          const int bIndex = ((ldb * j) + l);
          const jpfloat_t bValue = b[bIndex];
          total += (aValue * bValue);
        }
        const int cIndex = ((ldc * j) + i);
        const jpfloat_t oldCValue = c[cIndex];
        c[cIndex] = ((alpha * total) + (beta * oldCValue));
      }
    }
  } else if (aBitsPerElement == 8) {
    uint8_t* aData = (uint8_t*)(a);
    int i, j, l;
    for (j = 0; j < n; j++) {
      for (i = 0; i < m; i++) {
        jpfloat_t total = 0.0f;
        for (l = 0; l < k; l++) {
          int aIndex;
          if (transposeA == JPCblasNoTrans) {
            aIndex = ((lda * l) + i);
          } else {
            aIndex = ((lda * i) + l);
          }
          uint8_t aIntValue = aData[aIndex];
          const jpfloat_t aValue = aMin + (aIntValue * aRange);
          const int bIndex = ((ldb * j) + l);
          const jpfloat_t bValue = b[bIndex];
          total += (aValue * bValue);
        }
        const int cIndex = ((ldc * j) + i);
        const jpfloat_t oldCValue = c[cIndex];
        c[cIndex] = ((alpha * total) + (beta * oldCValue));
      }
    }
  } else {
    assert(false); // Should never get here, only 8 or 16 bit supported
  }
}

#if defined(USE_ACCELERATE_GEMM) || defined(USE_MKL_GEMM) || defined(USE_ATLAS_GEMM) || defined(USE_EIGEN_GEMM)
void cblas_sgemm_fixed(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  void *a,
  jpfloat_t aMin,
  jpfloat_t aMax,
  int aBitsPerElement,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

  assert(transposeA == JPCblasTrans);
  assert(transposeB == JPCblasNoTrans);
  assert(order == JPCblasColMajor);

  jpfloat_t aRange = ((aMax - aMin) / (1 << aBitsPerElement));

  const int rowsPerOperation = 64;

  const size_t bytesPerRow = (k * sizeof(jpfloat_t));
  const size_t bytesPerSubMatrix = (bytesPerRow * rowsPerOperation);
#if defined(TARGET_PI)
  jpfloat_t* aSubMatrix;
  posix_memalign((void**)(&aSubMatrix), 16, bytesPerSubMatrix);
  jpfloat_t* cSubMatrix;
  posix_memalign((void**)(&cSubMatrix), 16, bytesPerSubMatrix);
#else // TARGET_PI
  jpfloat_t* aSubMatrix = (jpfloat_t*)(malloc(bytesPerSubMatrix));
  jpfloat_t* cSubMatrix = (jpfloat_t*)(malloc(bytesPerSubMatrix));
#endif // TARGET_PI

  for (int iBase = 0; iBase < m; iBase += rowsPerOperation) {
    const int rowsThisTime = MIN(rowsPerOperation, (m - iBase));
    const int elementsPerSubMatrix = (rowsThisTime * k);
    if (aBitsPerElement == 16) {
      uint16_t* aData = (uint16_t*)(a);
#ifdef USE_ACCELERATE_GEMM
      uint16_t* aSubDataStart = (aData + (lda * iBase));
      vDSP_vfltu16(
        aSubDataStart,
        1,
        aSubMatrix,
        1,
        elementsPerSubMatrix);
      vDSP_vsmsa(
        aSubMatrix,
        1,
        &aRange,
        &aMin,
        aSubMatrix,
        1,
        elementsPerSubMatrix
      );
#elif defined(USE_NEON)

      // Only works on data that's multiples of 8 in size
      assert((k % 8) == 0);

      const float32x4_t vAMin0 = vdupq_n_f32(aMin);
      const float32x4_t vAMin1 = vdupq_n_f32(aMin);
      const float32x4_t vARange0 = vdupq_n_f32(aRange);
      const float32x4_t vARange1 = vdupq_n_f32(aRange);

      for (int iOffset = 0; iOffset < rowsThisTime; iOffset += 1) {
        const int i = (iBase + iOffset);
        uint16_t* currentA = (aData + (lda * i));
        uint16_t* endA = (currentA + k);
        jpfloat_t* currentSubMatrix = (aSubMatrix + (lda * iOffset));
        while (currentA < endA) {
          uint16x8_t vAInput16Bit = vld1q_u16(currentA);
          uint16x4_t vAInput16BitHigh = vget_high_u16(vAInput16Bit);
          uint16x4_t vAInput16BitLow = vget_low_u16(vAInput16Bit);
          uint32x4_t vAInput32BitHigh = vmovl_u16(vAInput16BitHigh);
          uint32x4_t vAInput32BitLow = vmovl_u16(vAInput16BitLow);
          float32x4_t vAInputFloatHigh = vcvtq_f32_u32(vAInput32BitHigh);
          float32x4_t vAInputFloatLow = vcvtq_f32_u32(vAInput32BitLow);
          float32x4_t vOutputHigh = vmlaq_f32(vAMin0, vARange0, vAInputFloatHigh);
          float32x4_t vOutputLow = vmlaq_f32(vAMin1, vARange1, vAInputFloatLow);

          vst1q_f32(currentSubMatrix, vOutputLow);
          currentSubMatrix += 4;
          vst1q_f32(currentSubMatrix, vOutputHigh);
          currentSubMatrix += 4;

          currentA += 8;

          const uint16_t dummyValue = *currentA;
        }
      }
#else // USE_ACCELERATE_GEMM
      for (int iOffset = 0; iOffset < rowsThisTime; iOffset += 1) {
        const int i = (iBase + iOffset);
        uint16_t* currentA = (aData + (lda * i));
        uint16_t* endA = (currentA + k);
        jpfloat_t* currentSubMatrix = (aSubMatrix + (lda * iOffset));
        while (currentA < endA) {
          *currentSubMatrix = (aMin + ((*currentA) * aRange));
          currentA += 1;
          currentSubMatrix += 1;
        }
      }
#endif // USE_ACCELERATE_GEMM
    } else if (aBitsPerElement == 8) {
      uint8_t* aData = (uint8_t*)(a);
#ifdef USE_ACCELERATE_GEMM
      uint8_t* aSubDataStart = (aData + (lda * iBase));
      vDSP_vfltu8(
        aSubDataStart,
        1,
        aSubMatrix,
        1,
        elementsPerSubMatrix);
      vDSP_vsmsa(
        aSubMatrix,
        1,
        &aRange,
        &aMin,
        aSubMatrix,
        1,
        elementsPerSubMatrix
      );
#elif defined(USE_NEON)

      // Only works on data that's multiples of 8 in size
      assert((k % 8) == 0);

      const float32x4_t vAMin0 = vdupq_n_f32(aMin);
      const float32x4_t vAMin1 = vdupq_n_f32(aMin);
      const float32x4_t vARange0 = vdupq_n_f32(aRange);
      const float32x4_t vARange1 = vdupq_n_f32(aRange);

      for (int iOffset = 0; iOffset < rowsThisTime; iOffset += 1) {
        const int i = (iBase + iOffset);
        uint8_t* currentA = (aData + (lda * i));
        uint8_t* endA = (currentA + k);
        jpfloat_t* currentSubMatrix = (aSubMatrix + (lda * iOffset));
        while (currentA < endA) {
          uint8x8_t vAInput8Bit = vld1_u8(currentA);
          uint16x8_t vAInput16Bit = vmovl_u8(vAInput8Bit);
          uint16x4_t vAInput16BitHigh = vget_high_u16(vAInput16Bit);
          uint16x4_t vAInput16BitLow = vget_low_u16(vAInput16Bit);
          uint32x4_t vAInput32BitHigh = vmovl_u16(vAInput16BitHigh);
          uint32x4_t vAInput32BitLow = vmovl_u16(vAInput16BitLow);
          float32x4_t vAInputFloatHigh = vcvtq_f32_u32(vAInput32BitHigh);
          float32x4_t vAInputFloatLow = vcvtq_f32_u32(vAInput32BitLow);
          float32x4_t vOutputHigh = vmlaq_f32(vAMin0, vARange0, vAInputFloatHigh);
          float32x4_t vOutputLow = vmlaq_f32(vAMin1, vARange1, vAInputFloatLow);

          vst1q_f32(currentSubMatrix, vOutputLow);
          currentSubMatrix += 4;
          vst1q_f32(currentSubMatrix, vOutputHigh);
          currentSubMatrix += 4;

          currentA += 8;

          const uint8_t dummyValue = *currentA;
        }
      }
#else // USE_ACCELERATE_GEMM
      for (int iOffset = 0; iOffset < rowsThisTime; iOffset += 1) {
        const int i = (iBase + iOffset);
        uint8_t* currentA = (aData + (lda * i));
        uint8_t* endA = (currentA + k);
        jpfloat_t* currentSubMatrix = (aSubMatrix + (lda * iOffset));
        while (currentA < endA) {
          *currentSubMatrix = (aMin + ((*currentA) * aRange));
          currentA += 1;
          currentSubMatrix += 1;
        }
      }
#endif // USE_ACCELERATE_GEMM
    } else {
      assert(false); // Should never get here, only 8 or 16 bit supported
    }
#if defined(USE_EIGEN_GEMM)
    eigen_cblas_sgemm(
      order,
      transposeA,
      transposeB,
      rowsThisTime,
      n,
      k,
      alpha,
      aSubMatrix,
      lda,
      b,
      ldb,
      beta,
      (c + iBase),
      ldc
    );
#else // USE_EIGEN_GEMM
    cblas_sgemm(
      (CBLAS_ORDER)(order),
      (CBLAS_TRANSPOSE)(transposeA),
      (CBLAS_TRANSPOSE)(transposeB),
      rowsThisTime,
      n,
      k,
      alpha,
      aSubMatrix,
      lda,
      b,
      ldb,
      beta,
      (c + iBase),
      ldc
    );
#endif // USE_EIGEN_GEMM
  }

  free(aSubMatrix);
  free(cSubMatrix);
}
#endif

#if defined(USE_EIGEN_GEMM)

typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> DynamicStride;
typedef Eigen::Matrix<jpfloat_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> EigenMatrix;
typedef Eigen::Map<EigenMatrix, Eigen::Aligned, DynamicStride> eigen_matrix_t;
typedef Eigen::Map<const EigenMatrix, Eigen::Aligned> eigen_matrix_const_t;

void eigen_cblas_sgemm(
  int order,
  int transposeA,
  int transposeB,
  int m,
  int n,
  int k,
  jpfloat_t alpha,
  jpfloat_t *a,
  int lda,
  jpfloat_t *b,
  int ldb,
  jpfloat_t beta,
  jpfloat_t* c,
  int ldc) {

  assert((transposeA == JPCblasNoTrans) || (transposeA == JPCblasTrans));
  assert(transposeB == JPCblasNoTrans);
  assert(order == JPCblasColMajor);
  assert(alpha == 1.0f);

  eigen_matrix_t cMatrix(c, m, n, DynamicStride(ldc, 1));
  if (beta > 0.0f) {
    cMatrix *= beta;
  }
  eigen_matrix_const_t bMatrix(b, k, n);
  if (transposeA == JPCblasNoTrans) {
    eigen_matrix_const_t aMatrix(a, m, k);
    if (beta > 0.0f) {
      cMatrix.noalias() += (aMatrix * bMatrix);
    } else {
      cMatrix.noalias() = (aMatrix * bMatrix);
    }
  } else {
    eigen_matrix_const_t aMatrix(a, k, m);
    if (beta > 0.0f) {
      cMatrix.noalias() += (aMatrix.transpose() * bMatrix);
    } else {
      cMatrix.noalias() = (aMatrix.transpose() * bMatrix);
    }
  }
}
#endif // USE_EIGEN_GEMM


// Copyright 2019 Yan Yan
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <tensorview/tensorview.h>
#include <boost/geometry.hpp>
#include <torch/script.h>
#include <vector>

namespace spconv {
namespace functor{
template <typename Device, typename T, typename Index>
struct NonMaxSupressionFunctor {
  Index operator()(const Device &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxes, T threshold, T eps);
};

template <typename Device, typename T, typename Index>
struct rotateNonMaxSupressionFunctor {
  Index operator()(const Device &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxCorners,
                   tv::TensorView<const T> standupIoU, T threshold);
};

}
}

namespace spconv {

namespace functor {
template <typename T, typename Index>
struct NonMaxSupressionFunctor<tv::CPU, T, Index> {
  Index operator()(const tv::CPU &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxes, T threshold, T eps) {
    auto ndets = boxes.dim(0);
    auto suppressed = std::vector<Index>(ndets);
    auto area = std::vector<T>(ndets);
    for (int i = 0; i < ndets; ++i) {
      area[i] =
          (boxes(i, 2) - boxes(i, 0) + eps) * (boxes(i, 3) - boxes(i, 1) + eps);
    }
    int i, j;
    T xx1, xx2, w, h, inter, ovr;
    int keepNum = 0;
    for (int _i = 0; _i < ndets; ++_i) {
      i = _i;
      if (suppressed[i] == 1)
        continue;
      keep[keepNum] = i;
      keepNum += 1;
      for (int _j = _i + 1; _j < ndets; ++_j) {
        j = _j;
        if (suppressed[j] == 1)
          continue;
        xx2 = std::min(boxes(i, 2), boxes(j, 2));
        xx1 = std::max(boxes(i, 0), boxes(j, 0));
        w = xx2 - xx1 + eps;
        if (w > 0) {
          xx2 = std::min(boxes(i, 3), boxes(j, 3));
          xx1 = std::max(boxes(i, 1), boxes(j, 1));
          h = xx2 - xx1 + eps;
          if (h > 0) {
            inter = w * h;
            ovr = inter / (area[i] + area[j] - inter);
            if (ovr >= threshold)
              suppressed[j] = 1;
          }
        }
      }
    }
    return keepNum;
  }
};

template <typename T, typename Index>
struct rotateNonMaxSupressionFunctor<tv::CPU, T, Index> {
  Index operator()(const tv::CPU &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxCorners,
                   tv::TensorView<const T> standupIoU, T threshold) {
    auto ndets = boxCorners.dim(0);
    auto suppressed = std::vector<Index>(ndets);
    int i, j;
    namespace bg = boost::geometry;
    typedef bg::model::point<T, 2, bg::cs::cartesian> point_t;
    typedef bg::model::polygon<point_t> polygon_t;
    polygon_t poly, qpoly;
    std::vector<polygon_t> poly_inter, poly_union;
    T inter_area, union_area, overlap;
    int keepNum = 0;
    for (int _i = 0; _i < ndets; ++_i) {
      i = _i;
      if (suppressed[i] == 1)
        continue;
      keep[keepNum] = i;
      keepNum += 1;
      for (int _j = _i + 1; _j < ndets; ++_j) {
        j = _j;
        if (suppressed[j] == 1)
          continue;
        if (standupIoU(i, j) <= 0.0)
          continue;
        bg::append(poly, point_t(boxCorners(i, 0, 0), boxCorners(i, 0, 1)));
        bg::append(poly, point_t(boxCorners(i, 1, 0), boxCorners(i, 1, 1)));
        bg::append(poly, point_t(boxCorners(i, 2, 0), boxCorners(i, 2, 1)));
        bg::append(poly, point_t(boxCorners(i, 3, 0), boxCorners(i, 3, 1)));
        bg::append(poly, point_t(boxCorners(i, 0, 0), boxCorners(i, 0, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 0, 0), boxCorners(j, 0, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 1, 0), boxCorners(j, 1, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 2, 0), boxCorners(j, 2, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 3, 0), boxCorners(j, 3, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 0, 0), boxCorners(j, 0, 1)));
        bg::intersection(poly, qpoly, poly_inter);

        if (!poly_inter.empty()) {
          inter_area = bg::area(poly_inter.front());
          bg::union_(poly, qpoly, poly_union);
          if (!poly_union.empty()) { // ignore invalid box
            union_area = bg::area(poly_union.front());
            overlap = inter_area / union_area;
            if (overlap >= threshold)
              suppressed[j] = 1;
            poly_union.clear();
          }
        }
        poly.clear();
        qpoly.clear();
        poly_inter.clear();
      }
    }
    return keepNum;
  }
};

} // namespace functor

#define DECLARE_CPU_T_INDEX(T, Index)                                          \
  template struct functor::NonMaxSupressionFunctor<tv::CPU, T, Index>;         \
  template struct functor::rotateNonMaxSupressionFunctor<tv::CPU, T, Index>;

#define DECLARE_CPU_INDEX(Index)                                               \
  DECLARE_CPU_T_INDEX(float, Index);                                           \
  DECLARE_CPU_T_INDEX(double, Index);

DECLARE_CPU_INDEX(int);
DECLARE_CPU_INDEX(long);

#undef DECLARE_CPU_INDEX
#undef DECLARE_CPU_T_INDEX

} // namespace spconv


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  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)

#ifndef BOOST_MATH_HYPERGEOMETRIC_PFQ_HPP
#define BOOST_MATH_HYPERGEOMETRIC_PFQ_HPP

#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>
#include <chrono>
#include <initializer_list>

namespace boost {
   namespace math {

      namespace detail {

         struct pFq_termination_exception : public std::runtime_error
         {
            pFq_termination_exception(const char* p) : std::runtime_error(p) {}
         };

         struct timed_iteration_terminator
         {
            timed_iteration_terminator(std::uintmax_t i, double t) : max_iter(i), max_time(t), start_time(std::chrono::system_clock::now()) {}

            bool operator()(std::uintmax_t iter)const
            {
               if (iter > max_iter)
                  boost::throw_exception(boost::math::detail::pFq_termination_exception("pFq exceeded maximum permitted iterations."));
               if (std::chrono::duration<double>(std::chrono::system_clock::now() - start_time).count() > max_time)
                  boost::throw_exception(boost::math::detail::pFq_termination_exception("pFq exceeded maximum permitted evaluation time."));
               return false;
            }

            std::uintmax_t max_iter;
            double max_time;
            std::chrono::system_clock::time_point start_time;
         };

      }

      template <class Seq, class Real, class Policy>
      inline typename tools::promote_args<Real, typename Seq::value_type>::type hypergeometric_pFq(const Seq& aj, const Seq& bj, const Real& z, Real* p_abs_error, const Policy& pol)
      {
         typedef typename tools::promote_args<Real, typename Seq::value_type>::type result_type;
         typedef typename policies::evaluation<result_type, Policy>::type value_type;
         typedef typename policies::normalise<
            Policy,
            policies::promote_float<false>,
            policies::promote_double<false>,
            policies::discrete_quantile<>,
            policies::assert_undefined<> >::type forwarding_policy;

         BOOST_MATH_STD_USING

         long long scale = 0;
         std::pair<value_type, value_type> r = boost::math::detail::hypergeometric_pFq_checked_series_impl(aj, bj, value_type(z), pol, boost::math::detail::iteration_terminator(boost::math::policies::get_max_series_iterations<forwarding_policy>()), scale);
         r.first *= exp(Real(scale));
         r.second *= exp(Real(scale));
         if (p_abs_error)
            *p_abs_error = static_cast<Real>(r.second) * boost::math::tools::epsilon<Real>();
         return policies::checked_narrowing_cast<result_type, Policy>(r.first, "boost::math::hypergeometric_pFq<%1%>(%1%,%1%,%1%)");
      }

      template <class Seq, class Real>
      inline typename tools::promote_args<Real, typename Seq::value_type>::type hypergeometric_pFq(const Seq& aj, const Seq& bj, const Real& z, Real* p_abs_error = 0)
      {
         return hypergeometric_pFq(aj, bj, z, p_abs_error, boost::math::policies::policy<>());
      }

      template <class R, class Real, class Policy>
      inline typename tools::promote_args<Real, R>::type hypergeometric_pFq(const std::initializer_list<R>& aj, const std::initializer_list<R>& bj, const Real& z, Real* p_abs_error, const Policy& pol)
      {
         return hypergeometric_pFq<std::initializer_list<R>, Real, Policy>(aj, bj, z, p_abs_error, pol);
      }

      template <class R, class Real>
      inline typename tools::promote_args<Real, R>::type  hypergeometric_pFq(const std::initializer_list<R>& aj, const std::initializer_list<R>& bj, const Real& z, Real* p_abs_error = 0)
      {
         return hypergeometric_pFq<std::initializer_list<R>, Real>(aj, bj, z, p_abs_error);
      }

      template <class T>
      struct scoped_precision
      {
         scoped_precision(unsigned p)
         {
            old_p = T::default_precision();
            T::default_precision(p);
         }
         ~scoped_precision()
         {
            T::default_precision(old_p);
         }
         unsigned old_p;
      };

      template <class Seq, class Real, class Policy>
      Real hypergeometric_pFq_precision(const Seq& aj, const Seq& bj, Real z, unsigned digits10, double timeout, const Policy& pol)
      {
         unsigned current_precision = digits10 + 5;

         for (auto ai = aj.begin(); ai != aj.end(); ++ai)
         {
            current_precision = (std::max)(current_precision, ai->precision());
         }
         for (auto bi = bj.begin(); bi != bj.end(); ++bi)
         {
            current_precision = (std::max)(current_precision, bi->precision());
         }
         current_precision = (std::max)(current_precision, z.precision());

         Real r, norm;
         std::vector<Real> aa(aj), bb(bj);
         do
         {
            scoped_precision<Real> p(current_precision);
            for (auto ai = aa.begin(); ai != aa.end(); ++ai)
               ai->precision(current_precision);
            for (auto bi = bb.begin(); bi != bb.end(); ++bi)
               bi->precision(current_precision);
            z.precision(current_precision);
            try
            {
               long long scale = 0;
               std::pair<Real, Real> rp = boost::math::detail::hypergeometric_pFq_checked_series_impl(aa, bb, z, pol, boost::math::detail::timed_iteration_terminator(boost::math::policies::get_max_series_iterations<Policy>(), timeout), scale);
               rp.first *= exp(Real(scale));
               rp.second *= exp(Real(scale));

               r = rp.first;
               norm = rp.second;

               unsigned cancellation;
               try {
                  cancellation = itrunc(log10(abs(norm / r)));
               }
               catch (const boost::math::rounding_error&)
               {
                  // Happens when r is near enough zero:
                  cancellation = UINT_MAX;
               }
               if (cancellation >= current_precision - 1)
               {
                  current_precision *= 2;
                  continue;
               }
               unsigned precision_obtained = current_precision - 1 - cancellation;
               if (precision_obtained < digits10)
               {
                  current_precision += digits10 - precision_obtained + 5;
               }
               else
                  break;
            }
            catch (const boost::math::evaluation_error&)
            {
               current_precision *= 2;
            }
            catch (const detail::pFq_termination_exception& e)
            {
               //
               // Either we have exhausted the number of series iterations, or the timeout.
               // Either way we quit now.
               throw boost::math::evaluation_error(e.what());
            }
         } while (true);

         return r;
      }
      template <class Seq, class Real>
      Real hypergeometric_pFq_precision(const Seq& aj, const Seq& bj, const Real& z, unsigned digits10, double timeout = 0.5)
      {
         return hypergeometric_pFq_precision(aj, bj, z, digits10, timeout, boost::math::policies::policy<>());
      }

      template <class Real, class Policy>
      Real hypergeometric_pFq_precision(const std::initializer_list<Real>& aj, const std::initializer_list<Real>& bj, const Real& z, unsigned digits10, double timeout, const Policy& pol)
      {
         return hypergeometric_pFq_precision< std::initializer_list<Real>, Real>(aj, bj, z, digits10, timeout, pol);
      }
      template <class Real>
      Real hypergeometric_pFq_precision(const std::initializer_list<Real>& aj, const std::initializer_list<Real>& bj, const Real& z, unsigned digits10, double timeout = 0.5)
      {
         return hypergeometric_pFq_precision< std::initializer_list<Real>, Real>(aj, bj, z, digits10, timeout, boost::math::policies::policy<>());
      }

   }
} // namespaces

#endif // BOOST_MATH_BESSEL_ITERATORS_HPP


#include "demo.h"
#include "Pool.h"
#include "light/DirLight.h"
#include "light/PointLight.h"
#include "model/CubeMesh.h"
#include "model/CylinderMesh.h"
#include "render.h"
#include "util/Seeder.h"
#include "util/debug.h"

#include <Eigen/Dense>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <array>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

using namespace std;

namespace demo {
std::ofstream paths;

template <typename T> int sgn(T val) { return (T(0) < val) - (val < T(0)); }

static glm::vec2 opposite_goal(glm::vec2 pos, unsigned) { return -pos; }
static auto make_translate_goal(glm::vec2 t) {
    return [t](glm::vec2 pos, unsigned) { return pos + t; };
}
static glm::vec2 mirror_x_goal(glm::vec2 pos, unsigned) {
    return glm::vec2(-pos.x, pos.y);
}
static glm::vec2 axial_goal(glm::vec2 pos, unsigned) {
    int axis = sgn<float>(abs(pos.x) - abs(pos.y));
    axis = (axis == 0 ? 1 : axis);
    float faxis = static_cast<float>(axis);
    return glm::vec2(-faxis * pos.x, faxis * pos.y);
}

static BoundVolume* null_shape(glm::vec2, unsigned) { return nullptr; }
static auto make_square_shape(float sz) {
    return [sz](glm::vec2 pos, unsigned) { return new Rect(pos, sz, sz); };
}

static auto make_radial_bots(unsigned num, float radius) {
    return [num, radius](unsigned i) {
        float radian = static_cast<float>(i) / static_cast<float>(num) * 2.f
            * glm::pi<float>();
        return radius * glm::vec2(cos(radian), sin(radian));
    };
}

struct Regiment {
    glm::vec2 facing;
    glm::vec2 head;
    glm::vec2 spacing;
    unsigned cols;
    unsigned n;
};
static glm::vec2 right_face(Regiment r) {
    return glm::cross(glm::vec3(r.facing, 0), glm::vec3(0, 0, 1));
}
static Regiment center(Regiment r) {
    float half = (r.spacing.x * static_cast<float>(r.cols - 1)) / 2.f;
    r.head -= right_face(r) * half;
    return r;
}

template <size_t N>
static auto make_regimented_bots(std::array<Regiment, N> regs) {
    for (Regiment r : regs) {
        r.facing /= glm::length(r.facing);
    }

    return [regs](unsigned i) {
        unsigned current = 0;
        unsigned last = regs[current].n;
        unsigned j = i;
        while (j >= last) {
            j -= last;
            last = regs[++current].n;
        }

        unsigned col = j % regs[current].cols;
        unsigned row = j / regs[current].cols;
        float u = regs[current].spacing.x * static_cast<float>(col);
        float v = regs[current].spacing.y * static_cast<float>(row);
        glm::vec2 x = right_face(regs[current]) * u;
        glm::vec2 y = -regs[current].facing * v;
        return x + y + regs[current].head;
    };
}
static auto make_intersecting_bots(unsigned n_per, unsigned cols, float d) {
    std::array<Regiment, 4> regiments{
        center({{1, 0}, {-d, 0}, {1.f, 1.f}, cols, n_per}),
        center({{-1, 0}, {d, 0}, {1.f, 1.f}, cols, n_per}),
        center({{0, 1}, {0, -d}, {1.f, 1.f}, cols, n_per}),
        center({{0, -1}, {0, d}, {1.f, 1.f}, cols, n_per}),
    };
    return make_regimented_bots(regiments);
}
static glm::vec2 ignore_wall(unsigned) { return glm::vec2(1000.0f); }
static auto make_hole_wall(float gap, float scale) {
    return [gap, scale](unsigned i) {
        float x = 0;
        float side = ((i % 2) == 0 ? 1.f : -1.f);
        float height = static_cast<float>(i / 2);
        float y = side * (scale * (height + .5f) + gap / 2.f);
        return glm::vec2(x, y);
    };
}
static auto make_hall(float gap, float scale) {
    return [gap, scale](unsigned i) {
        float side = ((i % 2) == 0 ? 1.f : -1.f);
        float width = static_cast<float>((i + 2 % 4) / 4);
        float xside = (i % 4 < 2 ? 1.f : -1.f);
        float x = xside * scale * width;
        float y = side * .5f * (gap + scale);
        return glm::vec2(x, y);
    };
}

// TODO: compose/union/intersect

struct Scene {
    unsigned num_robos;
    unsigned num_walls = 0u;
    float wall_scale = 0.f;
    float max_duration;
    float cam_dist = 1.f;
    std::function<glm::vec2(unsigned)> pos_of;
    std::function<glm::vec2(glm::vec2, unsigned)> goal_of;
    std::function<BoundVolume*(glm::vec2, unsigned)> wall_shape_of = null_shape;
    std::function<glm::vec2(unsigned)> wall_pos_of = ignore_wall;
};

// TODO: replace with reading a file with data
static Scene make_scene(unsigned scn) {
    Scene s;

    switch (scn) {
    case 0: // circle radius 10; 30 agents; 45s
        s.num_robos = 60u;
        s.max_duration = 45.f;
        s.cam_dist = .85f;
        s.pos_of = make_radial_bots(s.num_robos, 10.f);
        s.goal_of = opposite_goal;
        break;

    case 1: // circle radius 20; 90 agents 70s
        s.num_robos = 90u;
        s.max_duration = 70.f;
        s.cam_dist = 1.1f;
        s.pos_of = make_radial_bots(s.num_robos, 13.f);
        s.goal_of = opposite_goal;
        break;

    case 2: // circle radius 10; 30 agent; 40 .3x.3m posts in radius 8; 40 s
        s.num_robos = 60u;
        s.num_walls = 60u;
        s.wall_scale = 0.3f;
        s.cam_dist = .85f;
        s.max_duration = 40.f;
        s.pos_of = make_radial_bots(s.num_robos, 10.f);
        s.goal_of = opposite_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = [](unsigned) {
            Seeder seed;
            uniform_real_distribution<float> map(-7.f, 7.f);
            return glm::vec2(map(seed.gen()), map(seed.gen()));
        };
        break;

    case 3: // circle radius 10; 30 agents 1 4x4m pillar at center; 60s
        s.num_robos = 60u;
        s.num_walls = 1u;
        s.wall_scale = 4.0f;
        s.cam_dist = .85f;
        s.max_duration = 40.f;
        s.pos_of = make_radial_bots(s.num_robos, 10.f);
        s.goal_of = opposite_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = [](unsigned) { return glm::vec2(0.f); };
        break;

    case 4: // interesection 14 wide; 112 agents; 40s
        s.num_robos = 56u;
        s.max_duration = 40.f;
        s.cam_dist = .6f;
        s.pos_of = make_intersecting_bots(s.num_robos / 4, 7, 6.f);
        s.goal_of = axial_goal;
        break;

    case 5: // interesection 14 wide; 112 agents; swapped goals; 70s
        s.num_robos = 56u;
        s.max_duration = 70.f;
        s.cam_dist = .6f;
        s.pos_of = make_intersecting_bots(s.num_robos / 4, 7, 6.f);
        s.goal_of = opposite_goal;
        break;

    case 6: // intersection 14 wide; 112 agents; curbed; 70s
        s.num_robos = 56u;
        s.num_walls = 12u;
        s.wall_scale = 2.0f;
        s.max_duration = 40.f;
        s.cam_dist = .6f;
        s.pos_of = make_intersecting_bots(s.num_robos / 4, 7, 6.f);
        s.goal_of = axial_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = [](unsigned i) {
            switch (i) {
            case 0: return glm::vec2(-3.f, -3.f); // wall 1
            case 10: return glm::vec2(-4.f, -4.f);
            case 11: return glm::vec2(-6.f, -5.f);
            case 1: return glm::vec2(-3.f, 3.f); // wall 2
            case 8: return glm::vec2(-4.f, 4.f);
            case 9: return glm::vec2(-6.f, 5.f);
            case 2: return glm::vec2(3.f, -3.f); // wall 3
            case 5: return glm::vec2(4.f, -4.f);
            case 6: return glm::vec2(6.f, -5.f);
            case 3: return glm::vec2(3.f, 3.f); // wall 4
            case 4: return glm::vec2(4.f, 4.f);
            case 7: return glm::vec2(6.f, 5.f);
            default: assert(false);
            }
            exit(1);
        };
        break;

    case 7: // clogged doorway with 1 each way
        s.num_robos = 2u;
        s.num_walls = 30u;
        s.wall_scale = 2.0f;
        s.cam_dist = .3f;
        s.max_duration = 70.f;
        s.pos_of = [](unsigned i) {
            return (i == 0 ? glm::vec2(-3.f, 0) : glm::vec2(3.f, 0));
        };
        s.goal_of = mirror_x_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = make_hole_wall(1.f, s.wall_scale);
        break;

    case 8: { // clogged doorway qith 2 running in and 2 out
        s.num_robos = 4u;
        s.num_walls = 30u;
        s.wall_scale = 2.0f;
        s.cam_dist = .3f;
        s.max_duration = 30.f;
        std::array<Regiment, 2> r{
            center({{1, 0}, {-3, 0}, {1.f, 1.f}, 2, 2}),
            center({{-1, 0}, {3, 0}, {1.f, 1.f}, 2, 2}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = mirror_x_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = make_hole_wall(1.f, s.wall_scale);
        break;
    }

    case 9: { // clogged doorway with 20 running in, and 20 escaping.
        s.num_robos = 40u;
        s.num_walls = 30u;
        s.wall_scale = 2.0f;
        s.cam_dist = .6f;
        s.max_duration = 100.f;
        std::array<Regiment, 2> r{
            center({{1, 0}, {-3, 0}, {1.f, 1.f}, 5, 20}),
            center({{-1, 0}, {3, 0}, {1.f, 1.f}, 5, 20}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = mirror_x_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = make_hole_wall(1.f, s.wall_scale);
        break;
    }

    case 10: { // clogged doorway with 5 running in, and 35 escaping
        s.num_robos = 40u;
        s.num_walls = 30u;
        s.wall_scale = 2.0f;
        s.cam_dist = .8f;
        s.max_duration = 100.f;
        std::array<Regiment, 2> r{
            center({{1, 0}, {-3, 0}, {1.f, 1.f}, 5, 5}),
            center({{-1, 0}, {3, 0}, {1.f, 1.f}, 5, 35}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = mirror_x_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = make_hole_wall(1.f, s.wall_scale);
        break;
    }

    // escaping room
    case 11: {
        s.num_robos = 70u;
        s.num_walls = 32u;
        s.wall_scale = 2.0f;
        s.max_duration = 100.0f;
        std::array<Regiment, 2> r{
            center({{0, 1}, {1, 6}, {2.f, 1.5f}, 3, 6}),
            center({{-1, 0}, {-4, -2}, {1.f, 1.f}, 8, 64}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = make_translate_goal({-20.f, 0.f});
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = [](unsigned i) -> glm::vec2 {
            switch (i) {
            case 0: return {0.f, 8.f}; // bottom
            case 1: return {2.f, 8.f};
            case 2: return {4.f, 8.f};
            case 3: return {8.f, 8.f};
            case 4: return {-6.f, 8.f};
            case 5: return {-4.f, 8.f};
            case 6: return {-2.f, 8.f};

            case 7: return {-8.f, 0.f}; // left
            case 8: return {-8.f, 2.f};
            case 9: return {-8.f, 4.f};
            case 10: return {-8.f, 6.f};
            case 11: return {-8.f, 8.f};
            case 12: return {-8.f, -6.f};
            case 13: return {-8.f, -4.f};
            case 14: return {-8.f, -2.f};

            case 15: return {-7.4f, -8.f}; // top
            case 16: return {1.4f, -8.f};
            case 17: return {3.4f, -8.f};
            case 18: return {5.4f, -8.f};
            case 19: return {-5.4f, -8.f};
            case 20: return {-3.4f, -8.f};
            case 21: return {-1.4f, -8.f};

            case 22: return {8.f, 0.f}; // right
            case 23: return {8.f, 2.f};
            case 24: return {8.f, 4.f};
            case 25: return {8.f, 6.f};
            case 26: return {7.4f, -8.f};
            case 27: return {8.f, -6.f};
            case 28: return {8.f, -4.f};
            case 29: return {8.f, -2.f};

            case 30: return {6.f, 3.f};
            case 31: return {4.f, 3.f};

            default: return {0.f, 0.f};
            }
        };
        break;
    }

    // dense crowd & groups
    case 12: {
        s.num_robos = 200u;
        s.num_walls = 0u;
        s.max_duration = 30.f;
        Seeder sd;
        typedef uniform_real_distribution<float> UFD;
        s.pos_of = [&sd](unsigned) {
            UFD inside(-10.f, 10.f);
            return glm::vec2(inside(sd.gen()), inside(sd.gen()));
        };
        s.goal_of = [&sd](glm::vec2, unsigned) {
            UFD inside(-10.f, 10.f);
            return glm::vec2(inside(sd.gen()), inside(sd.gen()));
        };
        break;
    }

    case 13: { // two tight formations of 20 going down a hall
        s.num_robos = 40u;
        s.num_walls = 18u;
        s.wall_scale = 2.0f;
        s.cam_dist = .6f;
        s.max_duration = 90.0f;
        std::array<Regiment, 2> r{
            center({{1, 0}, {-3, 0}, {.5f, .7f}, 5, 20}),
            center({{-1, 0}, {3, 0}, {.5f, .7f}, 5, 20}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = mirror_x_goal;
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = make_hall(3.f, s.wall_scale);
        break;
    }

    case 14: // staggered 1 on 1
        s.num_robos = 2u;
        s.cam_dist = .6f;
        s.max_duration = 30.f;
        s.pos_of = [](unsigned i) {
            return (i == 0 ? glm::vec2(-5.f, .1f) : glm::vec2(3.f, 0));
        };
        s.goal_of = mirror_x_goal;
        break;

    case 15: // staggered 1 on 2
        s.num_robos = 3u;
        s.cam_dist = .6f;
        s.max_duration = 40.f;
        s.pos_of = [](unsigned i) {
            switch (i) {
            case 0: return glm::vec2(-5.f, .1f);
            case 1: return glm::vec2(3.f, 0);
            case 2: return glm::vec2(7.f, 0);
            default: assert(false);
            }
            exit(1);
        };
        s.goal_of = mirror_x_goal;
        break;

    case 16: // staggered 2 on 2
        s.num_robos = 4u;
        s.cam_dist = .6f;
        s.max_duration = 40.f;
        s.pos_of = [](unsigned i) {
            switch (i) {
            case 0: return glm::vec2(-5.f, .1f);
            case 1: return glm::vec2(3.f, 0);
            case 2: return glm::vec2(7.f, 0);
            case 3: return glm::vec2(-2.f, .1f);
            default: assert(false);
            }
            exit(1);
        };
        s.goal_of = mirror_x_goal;
        break;

    case 17: { // staggered equal 2 on equal 2
        s.num_robos = 4u;
        s.cam_dist = .6f;
        s.max_duration = 45.f;
        std::array<Regiment, 2> r{
            center({{1, 0}, {-5, .1f}, {1.f, 1.f}, 2, 2}),
            center({{-1, 0}, {3, 0}, {1.f, 1.f}, 2, 2}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = mirror_x_goal;
        break;
    }

    case 18: { // two-door
        s.num_robos = 65u;
        s.num_walls = 20u;
        s.wall_scale = 2.0f;
        s.cam_dist = .9f;
        s.max_duration = 60.f;
        std::array<Regiment, 2> r{
            center({{-1, 0}, {3, 3}, {1.f, 1.f}, 5, 5}),
            center({{-1, 0}, {3, -6}, {1.f, 1.f}, 10, 60}),
        };
        s.pos_of = make_regimented_bots(r);
        s.goal_of = make_translate_goal({-15.f, 0.f});
        s.wall_shape_of = make_square_shape(s.wall_scale);
        s.wall_pos_of = [](unsigned i) {
            float y = 10.f;
            switch (i) {
            case 0: y = +1.0f; break;
            case 1: y = -1.0f; break;
            case 2: y = +3.0f; break;
            case 3: y = -3.0f; break;
            case 4: y = +6.0f; break;
            case 5: y = -5.0f; break;
            case 6: y = +8.0f; break;
            case 7: y = -8.0f; break;
            case 8: y = +10.0f; break;
            case 9: y = -10.0f; break;
            case 10: y = +12.0f; break;
            case 11: y = -12.0f; break;
            case 12: y = +14.0f; break;
            case 13: y = -14.0f; break;
            case 14: y = +16.0f; break;
            case 15: y = -16.0f; break;
            case 16: y = +18.0f; break;
            case 17: y = -18.0f; break;
            case 18: y = +20.0f; break;
            case 19: y = -20.0f; break;
            }
            return glm::vec2(0.f, y);
        };
        break;
    }

    default: { // variable num_robos version of s9
        if (scn > 100) {
            s.num_robos = 40u + (scn - 900);
            s.num_walls = 30u;
            s.wall_scale = 2.0f;
            s.cam_dist = .6f;
            s.max_duration = 100.f;
            unsigned even_size = s.num_robos / 2;
            unsigned odd_size = even_size + s.num_robos % 2;
            std::array<Regiment, 2> r{
                center({{1, 0}, {-3, 0}, {1.f, 1.f}, 5, odd_size}),
                center({{-1, 0}, {3, 0}, {1.f, 1.f}, 5, even_size}),
            };
            s.pos_of = make_regimented_bots(r);
            s.goal_of = mirror_x_goal;
            s.wall_shape_of = make_square_shape(s.wall_scale);
            s.wall_pos_of = make_hole_wall(1.f, s.wall_scale);
        }
    }
    }
    return s;
}

Scene scn;

static void create_wall(Entity& e, vector<Texture> texs, glm::vec2 pos) {
#ifdef NO_RENDER
    UNUSED(texs);
#endif
    uint16_t tid = POOL.create<Transform>(Transform(nullptr));
#ifndef NO_RENDER
    uint16_t mid = POOL.create<Mesh>(CubeMesh(texs));
#endif
    uint16_t bvid = POOL.create<BoundVolume*>(scn.wall_shape_of(pos, 0));

    auto& t = *POOL.get<Transform>(tid);
    glm::mat4 scale(scn.wall_scale);
    scale[1][1] = 1.f;
    scale[3][3] = 1.f;
    t.set_mat(scale);
    t.set_pos(glm::vec3(pos.x, 0, pos.y));

    POOL.attach<Transform>(e, tid);
#ifndef NO_RENDER
    POOL.attach<Mesh>(e, mid);
#endif
    POOL.attach<BoundVolume*>(e, bvid);
}

static void create_robo(Entity& e, vector<Texture> texs, glm::vec2 pos) {
    float s = .3f;
    float outline_percent = .25f;
#ifdef NO_RENDER
    UNUSED(texs);
#endif
    uint16_t tid = POOL.create<Transform>(Transform(nullptr));
#ifndef NO_RENDER
    uint16_t mid = POOL.create<Mesh>(CylinderMesh(texs, 18, .5f));
    // uint16_t mid = POOL.create<Mesh>(CubeMesh(texs));
#endif
    uint16_t did = POOL.create<Dynamics>(Dynamics());
    // uint16_t bvid = POOL.create<BoundVolume*>(new Rect(pos, s, s));
    uint16_t bvid = POOL.create<BoundVolume*>(new Circ(pos, s * .5f));
    uint16_t aid = POOL.create<Agent>(Agent());
#ifndef NO_COMM
    uint16_t cid = POOL.create<CommComp>(CommComp());
#endif

    auto& t = *POOL.get<Transform>(tid);
    glm::mat4 scale((1.f - outline_percent) * s);
    scale[1][1] = 1.f;
    scale[3][3] = 1.f;
    t.set_mat(scale);

    auto& d = *POOL.get<Dynamics>(did);
    d.force = glm::vec3(0);
    d.acc = glm::vec3(0);
    d.vel = glm::vec3(0);
    d.pos = glm::vec3(pos.x, 0, pos.y);
    d.mass = 1.f;

    auto& a = *POOL.get<Agent>(aid);
    a.final_goal = scn.goal_of(pos, 0);
    a.start = pos;

#ifndef NO_COMM
    auto& c = *POOL.get<CommComp>(cid);
    c.c = comm::vecc::Zero();
    c.c_buf = comm::vecc::Zero();
    c.f_buf = glm::vec2(0);
    c.facing = a.final_goal - a.start;
#endif

    POOL.attach<Transform>(e, tid);
#ifndef NO_RENDER
    POOL.attach<Mesh>(e, mid);
#endif
    POOL.attach<Dynamics>(e, did);
    POOL.attach<BoundVolume*>(e, bvid);
    POOL.attach<Agent>(e, aid);
#ifndef NO_COMM
    POOL.attach<CommComp>(e, cid);
#endif

#ifndef NO_RENDER
    Entity& outline = POOL.spawn_entity();
    tid = POOL.create<Transform>(Transform(&t));
    auto& ot = *POOL.get<Transform>(tid);
    glm::mat4 line_scale(1.f / (1.f - outline_percent));
    line_scale[1][1] = .9f;
    line_scale[3][3] = 1.f;
    ot.set_mat(line_scale);
    vector<Texture> blank{};
    mid = POOL.create<Mesh>(CylinderMesh(blank, 18, .5f));
    auto& m = *POOL.get<Mesh>(mid);
    m._ambient = glm::vec3(0.f);
    POOL.attach<Transform>(outline, tid);
    POOL.attach<Mesh>(outline, mid);
#endif
}

void init(unsigned scn_i, std::string data) {
    string pwd(PROJECT_SRC_DIR);
    // TODO: open buffer to {this_scene}/{mode}.paths
    // id, groupID, x, y, vx, vy, radius, timeSTAMP

    paths = std::ofstream(data + "/agent_paths.csv", std::ios::binary);
    scn = make_scene(scn_i);

    // so, normally I'd only want one mesh shared amongst many entities, but the
    // renderer does a for_<Mesh> so I can't do that.
    vector<Texture> robo_tex = {};
    vector<Texture> wall_tex = {
#ifndef NO_RENDER
        {render::create_tex(pwd + "/res/outline.png"), Texmap::diffuse},
#endif
    };

    for (unsigned i = 0; i < scn.num_robos; ++i) {
        create_robo(POOL.spawn_entity(), robo_tex, scn.pos_of(i));
    }

#pragma GCC diagnostic ignored "-Wtype-limits"
    for (unsigned i = 0; i < scn.num_walls; ++i) {
        create_wall(POOL.spawn_entity(), wall_tex, scn.wall_pos_of(i));
    }

#ifndef NO_RENDER
    {
        render::dir_lights.push_back(make_unique<DirLight>());
        render::dir_lights.back()->dir(glm::vec3(0, -1, 0));
        render::dir_lights.back()->ambient(glm::vec3(1.f));
        render::dir_lights.back()->diffuse(glm::vec3(0.f));
        render::dir_lights.back()->specular(glm::vec3(0.f));
    }

    render::cam_dist = scn.cam_dist;
#endif
}

int total_num_done = 0;
float run_avg_time = 0;
float M2 = 0;
float run_std_time = 0;

static void update_runs(float overhead) {
    float ftotal = static_cast<float>(++total_num_done);
    float delta = overhead - run_avg_time;
    run_avg_time += delta / ftotal;
    float delta2 = overhead - run_avg_time;
    M2 += delta * delta2;
    run_std_time = sqrt(M2 / ftotal);
    printf("t:%f u:%f s:%f\n", overhead, run_avg_time, run_std_time);
}

bool run(double dt, double time, unsigned frame_count) {
    bool all_done = true;
    static double last_s = 0;
    float fake_time = static_cast<float>(dt) * static_cast<float>(frame_count);
    POOL.for_<Agent>([&](Agent& ai, Entity& e) {
        if (!ai.done()) {
            all_done = false;
        } else if (!ai.overhead_counted) {
            update_runs(fake_time - ai.min_time);
            ai.overhead_counted = true;
        }
        Dynamics& d = *POOL.get<Dynamics>(e);
        Transform& t = *POOL.get<Transform>(e);
        paths //
            << e.id << ",0," //
            << d.pos.x << "," << d.pos.z << "," //
            << d.vel.x << "," << d.vel.z << "," //
            << t.mat()[0][0] * .5f << "," // .5f for circles
            << static_cast<float>(dt) * static_cast<float>(frame_count - 1) //
            << "\n";
    });
    if (time - last_s >= 1) {
        std::clog << "NUM DONE: " << total_num_done << "\n";
        last_s = time;
    }

    if (fake_time > scn.max_duration) {
        POOL.for_<Agent>([&](Agent& ai, Entity&) {
            if (!ai.done()) {
                float finish_time = 2 * glm::length(ai.final_goal - ai.start)
                    + fake_time - ai.min_time;
                update_runs(finish_time);
            }
        });
        all_done = true;
    }
    return all_done;
}

void terminate() {
    POOL.for_<BoundVolume*>([&](BoundVolume*& bv, Entity&) {
        if (bv)
            delete bv;
    });
    paths.close();
}
} // namespace demo


#include <Functions/FunctionFactory.h>
#include <Functions/GeoUtils.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 LOGICAL_ERROR;
    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 FunctionPointInPolygonDetail
{

template <typename Polygon, typename PointInPolygonImpl>
ColumnPtr callPointInPolygonImplWithPool(const IColumn & x, const IColumn & 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]()
    {
        GeoUtils::normalizePolygon(polygon);
        auto ptr = std::make_unique<PointInPolygonImpl>(polygon);

        /// To allocate memory.
        ptr->init();

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

        return ptr.release();
    };

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

    return GeoUtils::pointInPolygon(x, y, *impl);
}

template <typename Polygon, typename PointInPolygonImpl>
ColumnPtr callPointInPolygonImpl(const IColumn & x, const IColumn & y, Polygon & polygon)
{
    PointInPolygonImpl impl(polygon);
    return GeoUtils::pointInPolygon(x, y, impl);
}

}

template <template <typename> typename PointInPolygonImpl, bool use_object_pool = false>
class FunctionPointInPolygon : public IFunction
{
public:

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

    static const char * name;

    static FunctionPtr create(const Context &)
    {
        return std::make_shared<FunctionPointInPolygon<PointInPolygonImpl, use_object_pool>>();
    }

    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 get_message_prefix = [this](size_t i) { return "Argument " + toString(i + 1) + " for function " + getName(); };

        for (size_t i = 1; i < arguments.size(); ++i)
        {
            auto * array = checkAndGetDataType<DataTypeArray>(arguments[i].get());
            if (array == nullptr && i != 1)
                throw Exception(get_message_prefix(i) + " must be array of tuples.", ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

            auto * tuple = checkAndGetDataType<DataTypeTuple>(array ? array->getNestedType().get() : arguments[i].get());
            if (tuple == nullptr)
                throw Exception(get_message_prefix(i) + " must contains tuple.", ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

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

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

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

        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();
        auto const_tuple_col = checkAndGetColumn<ColumnConst>(point_col);
        if (const_tuple_col)
            point_col = &const_tuple_col->getDataColumn();
        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 DataTypes & tuple_types = typeid_cast<const DataTypeTuple &>(*block.getByPosition(arguments[0]).type).getElements();

        bool use_float64 = WhichDataType(tuple_types[0]).isFloat64() || WhichDataType(tuple_types[1]).isFloat64();

        auto & result_column = block.safeGetByPosition(result).column;

        if (use_float64)
            result_column = executeForType<Float64>(*tuple_columns[0], *tuple_columns[1], block, arguments);
        else
            result_column = executeForType<Float32>(*tuple_columns[0], *tuple_columns[1], block, arguments);

        if (const_tuple_col)
            result_column = ColumnConst::create(result_column, const_tuple_col->size());
    }

private:

    Float64 getCoordinateFromField(const Field & field)
    {
        switch (field.getType())
        {
            case Field::Types::Float64:
                return field.get<Float64>();
            case Field::Types::Int64:
                return field.get<Int64>();
            case Field::Types::UInt64:
                return field.get<UInt64>();
            default:
            {
                std::string msg = "Expected numeric field, but got ";
                throw Exception(msg + Field::Types::toString(field.getType()), ErrorCodes::LOGICAL_ERROR);
            }
        }
    }

    template <typename Type>
    ColumnPtr executeForType(const IColumn & x, const IColumn & y, Block & block, const ColumnNumbers & arguments)
    {
        Polygon<Type> polygon;

        auto get_message_prefix = [this](size_t i) { return "Argument " + toString(i + 1) + " for function " + getName(); };

        for (size_t i = 1; i < arguments.size(); ++i)
        {
            auto const_col = checkAndGetColumn<ColumnConst>(block.getByPosition(arguments[i]).column.get());
            auto array_col = const_col ? checkAndGetColumn<ColumnArray>(&const_col->getDataColumn()) : nullptr;
            auto tuple_col = array_col ? checkAndGetColumn<ColumnTuple>(&array_col->getData()) : nullptr;

            if (!tuple_col)
                throw Exception(get_message_prefix(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(get_message_prefix(i) + " shouldn't be empty.", ErrorCodes::ILLEGAL_COLUMN);

            for (auto j : ext::range(0, size))
            {
                Type x_coord = getCoordinateFromField((*column_x)[j]);
                Type y_coord = getCoordinateFromField((*column_y)[j]);
                container.push_back(Point<Type>(x_coord, y_coord));
            }

            /// Polygon assumed to be closed. Allow user to escape repeating of first point.
            if (!boost::geometry::equals(container.front(), container.back()))
                container.push_back(container.front());
        }

        auto call_impl = use_object_pool
            ? FunctionPointInPolygonDetail::callPointInPolygonImplWithPool<Polygon<Type>, PointInPolygonImpl<Type>>
            : FunctionPointInPolygonDetail::callPointInPolygonImpl<Polygon<Type>, PointInPolygonImpl<Type>>;

        return call_impl(x, y, polygon);
    }
};

template <typename Type>
using Point = boost::geometry::model::d2::point_xy<Type>;

template <typename Type>
using PointInPolygonWithGrid = GeoUtils::PointInPolygonWithGrid<Type>;

template <>
const char * FunctionPointInPolygon<PointInPolygonWithGrid, true>::name = "pointInPolygon";

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

}


#include <Eigen/Dense>
#include <ostream>
#include <string>

const int FEAT_DIM = 512;
const int SALU_LEFT_DIM = 2;
const int SALU_RIGHT_DIM = 2;

class Instance {
public:
  int classID;
  float score;
  Eigen::Vector4f bbox;
  Eigen::VectorXf feat;

  Instance(int featDim = FEAT_DIM);
  Instance(const Eigen::VectorXf &pred, const Eigen::VectorXf &roiFeat);
  Instance(const Eigen::VectorXf &pred, const Eigen::VectorXf &roiFeat,
           const Eigen::VectorXf &saluRoot, const Eigen::VectorXf &saluLeft,
           const Eigen::VectorXf &saluRight);

  void update_salutation(const Eigen::VectorXf &saluRoot,
                         const Eigen::VectorXf &saluLeft,
                         const Eigen::VectorXf &saluRight);
  std::string salutation_cls_tree() const;
  std::string get_salutation(double l1, double l2) const;
  float get_area_size() const;

private:
  Eigen::Vector2f saluRoot;
  Eigen::VectorXf saluLeft;
  Eigen::VectorXf saluRight;

  double get_conf_gap(Eigen::VectorXf v) const;
};

Instance::Instance(int featDim) {
  classID = -1;
  score = 0.0;
  bbox.setZero();
  feat.resize(featDim);
  feat.setZero();

  saluLeft.resize(SALU_LEFT_DIM);
  saluRight.resize(SALU_RIGHT_DIM);
  saluRoot.setZero();
  saluLeft.setZero();
  saluRight.setZero();
}

Instance::Instance(const Eigen::VectorXf &pred,
                   const Eigen::VectorXf &roiFeat) {
  classID = static_cast<int>(pred(0));
  score = pred(1);
  for (int i = 0; i < 4; i++)
    bbox(i) = pred(i + 2);
  feat = roiFeat;

  saluLeft.resize(SALU_LEFT_DIM);
  saluRight.resize(SALU_RIGHT_DIM);
  saluRoot.setZero();
  saluLeft.setZero();
  saluRight.setZero();
}

Instance::Instance(const Eigen::VectorXf &pred, const Eigen::VectorXf &roiFeat,
                   const Eigen::VectorXf &root, const Eigen::VectorXf &left,
                   const Eigen::VectorXf &right) {
  classID = static_cast<int>(pred(0));
  score = pred(1);
  for (int i = 0; i < 4; i++)
    bbox(i) = pred(i + 2);
  feat = roiFeat;
  update_salutation(root, left, right);
}

void Instance::update_salutation(const Eigen::VectorXf &root,
                                 const Eigen::VectorXf &left,
                                 const Eigen::VectorXf &right) {
  saluRoot(0) = root(0);
  saluRoot(1) = root(1);
  saluLeft.resize(SALU_LEFT_DIM);
  saluRight.resize(SALU_RIGHT_DIM);
  for (int i = 0; i < SALU_LEFT_DIM; i++)
    saluLeft(i) = left(i);
  for (int i = 0; i < SALU_RIGHT_DIM; i++)
    saluRight(i) = right(i);
}

std::string Instance::salutation_cls_tree() const {
  std::string tree;

  tree += "[" + std::to_string(saluRoot(0)) + ", ( ";
  for (int i = 0; i < SALU_LEFT_DIM; i++)
    tree += std::to_string(saluLeft(i)) + " ";
  tree += ")], ";

  tree += "[" + std::to_string(saluRoot(1)) + ", ( ";
  for (int i = 0; i < SALU_RIGHT_DIM; i++)
    tree += std::to_string(saluRight(i)) + " ";
  tree += ")]";

  return tree;
}

std::string Instance::get_salutation(double l1, double l2) const {
  std::string salu = "";
  if (saluRoot(0) - saluRoot(1) > l1 && get_conf_gap(saluLeft) > l2) {
    if (saluLeft(0) > saluLeft(1))
      salu = "小哥哥";
    else
      salu = "叔叔";
  } else if (saluRoot(1) - saluRoot(0) > l1 && get_conf_gap(saluRight) > l2) {
    if (saluRight(0) > saluRight(1))
      salu = "小姐姐";
    else
      salu = "阿姨";
  }

  return salu;
}

float Instance::get_area_size() const {
  float xmin, ymin, xmax, ymax;
  xmin = bbox(0);
  ymin = bbox(1);
  xmax = bbox(2);
  ymax = bbox(3);

  return (ymax - ymin) * (xmax - xmin);
}

double Instance::get_conf_gap(Eigen::VectorXf v) const {
  double gap = 0.0;
  float maxV = 0.0, maxV2 = -1.0;
  for (int i = 0; i < v.size(); i++) {
    if (v(i) > maxV) {
      maxV2 = maxV;
      maxV = v(i);
    } else if (v(i) > maxV2) {
      maxV2 = v(i);
    }
  }

  if (maxV2 > 0)
    gap = maxV - maxV2;
  return gap;
}

std::ostream &operator<<(std::ostream &os, const Instance &inst) {
  os << "{cid: " << inst.classID << ",\n bbox: " << inst.bbox
     << ", \n salutation: " << inst.salutation_cls_tree()
     << ",\n feat: " << inst.feat.segment(0, 20) << "...}";
  return os;
}


/*************************  FWMAV Simulation  *************************
* Version 0.3
* Fan Fei		Jan 2018
* FWMAV simulation with dual motor driven robotic flapper
* PID controller using split cycle mechanism
***********************************************************************
*/

#include <vector>
#include <iostream>
#include <Eigen/Dense>

namespace FWMAV{
class Actuator {
public:
	Actuator(double variable_resistance, double* stroke_velocity, double* stroke_acceleration);
	virtual ~Actuator();

	void doNothing();
	void updateDriverVoltage(double voltage);
	void UpdateTorque();

	double GetTorque();
	double GetMotorTorque();
	double GetMagTorque();
	double GetInerTorque();
	double GetDampTorque();
	double GetFricTorque();
	double GetBEMF();
	double GetCurrent();

protected:
	double* stroke_velocity_;
	double* stroke_acceleration_;

	const double k_resistance_;
	const double k_torque_constant_;
	const double k_gear_ratio_;
	const double k_mechanical_efficiency_;
	const double k_friction_coefficient_;
	const double k_damping_coefficient_;
	const double k_inertia_;

	double psi_dot_;
	double psi_ddot_;
	double motor_vel_;
	double motor_accel_;
	double sign_;
	double voltage_;
	double current_;
	double back_EMF_;
	double variable_resistance_;
	double resistance_;

	double inertia_torque_;
	double damping_torque_;
	double friction_torque_;
	double magnetic_torque_;
	double motor_torque_;
	double output_torque_;
};
}