/**
 *
 * Copyright (C) 2021 Mohammad Javad Dousti, Qing Xie, Mahdi Nazemi,
 * and Massoud Pedram. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
 */

// These three header files override their Boost counter part. They should be
// imported first.

#if USE_GPU == 0

#include "headers/implicit_euler.hpp"
#include "headers/rosenbrock4.hpp"
#include "headers/rosenbrock4_controller.hpp"

#endif

#include "headers/solver.hpp"
#include "headers/utils.hpp"
#include <omp.h>

#if USE_GPU == 1

#include <Eigen/IterativeLinearSolvers>
#include <helper_cuda.h>

void solver::gpuSolveDenseSteady(const matrix_type &matrix,
                                 const state_type &p_vector,
                                 state_type t_vector, bool use_cholesky) {
  cusolverDnHandle_t cusolverH = NULL;
  cudaStream_t stream = NULL;

  const int n = p_vector.size();
  int info = 0; /* host copy of error info */

  VALUE *d_matrix = NULL;   /* device copy of A */
  VALUE *d_p_vector = NULL; /* device copy of B */
  int *d_Ipiv = NULL;       /* pivoting sequence */
  int *d_info = NULL;       /* error info */
  int lwork = 0;            /* size of workspace */
  VALUE *d_work = NULL;     /* device workspace for getrf */

  // step 1: create cusolver handle, bind a stream
  checkCudaErrors(cusolverDnCreate(&cusolverH));
  checkCudaErrors(cudaStreamCreateWithFlags(&stream, cudaStreamNonBlocking));
  checkCudaErrors(cusolverDnSetStream(cusolverH, stream));

  // step 2: copy matrices to device
  checkCudaErrors(cudaMalloc((void **)&d_matrix, sizeof(VALUE) * n * n));
  checkCudaErrors(cudaMalloc((void **)&d_p_vector, sizeof(VALUE) * n));
  checkCudaErrors(cudaMalloc((void **)&d_Ipiv, sizeof(int) * n));
  checkCudaErrors(cudaMalloc((void **)&d_info, sizeof(int)));
  {
    Eigen::MatrixXf dense_representation = Eigen::MatrixXf(matrix);
    checkCudaErrors(cudaMemcpy(d_matrix, dense_representation.data(),
                               sizeof(VALUE) * n * n, cudaMemcpyHostToDevice));
  }
  checkCudaErrors(cudaMemcpy(d_p_vector, p_vector.data(), sizeof(VALUE) * n,
                             cudaMemcpyHostToDevice));

  // step 3: query working space of getrf
  if (use_cholesky) {
    checkCudaErrors(cusolverDnSpotrf_bufferSize(
        cusolverH, CUBLAS_FILL_MODE_LOWER, n, d_matrix, n, &lwork));
  } else {
    checkCudaErrors(
        cusolverDnSgetrf_bufferSize(cusolverH, n, n, d_matrix, n, &lwork));
  }
  checkCudaErrors(cudaMalloc((void **)&d_work, sizeof(VALUE) * lwork));

  // step 4: LUP factorization
  if (use_cholesky) {
    checkCudaErrors(cusolverDnSpotrf(cusolverH, CUBLAS_FILL_MODE_LOWER, n,
                                     d_matrix, n, d_work, lwork, d_info));
  } else {
    checkCudaErrors(
        cusolverDnSgetrf(cusolverH, n, n, d_matrix, n, d_work, d_Ipiv, d_info));
  }

  checkCudaErrors(
      cudaMemcpy(&info, d_info, sizeof(int), cudaMemcpyDeviceToHost));
  ASSERT(info >= 0, -info << "-th parameter is wrong.");

  // step 5: solve A*X = B
  if (use_cholesky) {
    checkCudaErrors(cusolverDnSpotrs(cusolverH, CUBLAS_FILL_MODE_LOWER, n,
                                     1, /* nrhs */
                                     d_matrix, n, d_p_vector, n, d_info));
  } else {
    checkCudaErrors(cusolverDnSgetrs(cusolverH, CUBLAS_OP_N, n, 1, /* nrhs */
                                     d_matrix, n, d_Ipiv, d_p_vector, n,
                                     d_info));
  }

  checkCudaErrors(cudaDeviceSynchronize());
  checkCudaErrors(cudaMemcpy(t_vector.data(), d_p_vector, sizeof(VALUE) * n,
                             cudaMemcpyDeviceToHost));

  /* free resources */
  checkCudaErrors(cudaFree(d_matrix));
  checkCudaErrors(cudaFree(d_p_vector));
  checkCudaErrors(cudaFree(d_Ipiv));
  checkCudaErrors(cudaFree(d_info));
  checkCudaErrors(cudaFree(d_work));
  checkCudaErrors(cusolverDnDestroy(cusolverH));
  checkCudaErrors(cudaStreamDestroy(stream));

  checkCudaErrors(cudaDeviceReset());
}

/**
 * Taken from
 * https://github.com/phrb/intro-cuda/blob/master/src/cuda-samples/7_CUDALibraries/cuSolverSp_LinearSolver/cuSolverSp_LinearSolver.cpp
 */
void solver::gpuSolveSparseSteady(const matrix_type &matrix,
                                  const state_type &p_vector,
                                  state_type &t_vector) {
  cusolverSpHandle_t handle = NULL;
  cusparseHandle_t cusparseHandle = NULL; // used in residual evaluation
  cudaStream_t stream = NULL;
  cusparseMatDescr_t descrA = NULL;

  const int n = p_vector.size();
  const int nnzA = matrix.nonZeros();          // number of nonzeros of A
  const int baseA = matrix.outerIndexPtr()[0]; // base index in CSR format

  int *d_csrRowPtrA = NULL;
  int *d_csrColIndA = NULL;
  VALUE *d_csrValA = NULL;
  VALUE *d_t_vector = NULL; // x = A \ b
  VALUE *d_p_vector = NULL; // a copy of h_b

  const VALUE tol = 1.e-12;
  const int reorder = 0;     // no reordering

  int singularity = 0; // -1 if A is invertible under tol.

  VALUE start, stop;
  VALUE time_solve_cpu;
  VALUE time_solve_gpu;

  checkCudaErrors(cusolverSpCreate(&handle));
  checkCudaErrors(cusparseCreate(&cusparseHandle));
  checkCudaErrors(cudaStreamCreate(&stream));
  checkCudaErrors(cusolverSpSetStream(handle, stream));
  checkCudaErrors(cusparseSetStream(cusparseHandle, stream));
  checkCudaErrors(cusparseCreateMatDescr(&descrA));
  checkCudaErrors(cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL));

  if (baseA) {
    checkCudaErrors(cusparseSetMatIndexBase(descrA, CUSPARSE_INDEX_BASE_ONE));
  } else {
    checkCudaErrors(cusparseSetMatIndexBase(descrA, CUSPARSE_INDEX_BASE_ZERO));
  }

  checkCudaErrors(cudaMalloc((void **)&d_csrRowPtrA, sizeof(int) * (n + 1)));
  checkCudaErrors(cudaMalloc((void **)&d_csrColIndA, sizeof(int) * nnzA));
  checkCudaErrors(cudaMalloc((void **)&d_csrValA, sizeof(VALUE) * nnzA));
  checkCudaErrors(cudaMalloc((void **)&d_t_vector, sizeof(VALUE) * n));
  checkCudaErrors(cudaMalloc((void **)&d_p_vector, sizeof(VALUE) * n));

  checkCudaErrors(cudaMemcpy(d_csrRowPtrA, matrix.outerIndexPtr(),
                             sizeof(int) * (n + 1), cudaMemcpyHostToDevice));

  checkCudaErrors(cudaMemcpy(d_csrColIndA, matrix.innerIndexPtr(),
                             sizeof(int) * nnzA, cudaMemcpyHostToDevice));

  checkCudaErrors(cudaMemcpy(d_csrValA, matrix.valuePtr(), sizeof(VALUE) * nnzA,
                             cudaMemcpyHostToDevice));

  checkCudaErrors(cudaMemcpy(d_p_vector, p_vector.data(), sizeof(VALUE) * n,
                             cudaMemcpyHostToDevice));

  // solve A*x = b on GPU
  checkCudaErrors(cusolverSpScsrlsvchol(
      handle, n, nnzA, descrA, d_csrValA, d_csrRowPtrA, d_csrColIndA,
      d_p_vector, tol, reorder, d_t_vector, &singularity));
  checkCudaErrors(cudaDeviceSynchronize());

  if (singularity >= 0) {
    printf("WARNING: the matrix is singular at row %d under tol (%E)\n",
           singularity, tol);
  }

  // Copying results back to local memory
  checkCudaErrors(cudaMemcpy(t_vector.data(), d_t_vector, sizeof(VALUE) * n,
                             cudaMemcpyDeviceToHost));

  checkCudaErrors(cusolverSpDestroy(handle));
  checkCudaErrors(cusparseDestroy(cusparseHandle));
  checkCudaErrors(cudaStreamDestroy(stream));
  checkCudaErrors(cusparseDestroyMatDescr(descrA));

  checkCudaErrors(cudaFree(d_csrValA));
  checkCudaErrors(cudaFree(d_csrRowPtrA));
  checkCudaErrors(cudaFree(d_csrColIndA));
  checkCudaErrors(cudaFree(d_t_vector));

  checkCudaErrors(cudaDeviceReset());
}

void solver::gpuSolveTransient() {
  cerr << "GPU implementation doesn't support transient_ mode.";
  exit(-1);
}

#else // use CPU

#include <Eigen/PardisoSupport>
#include <unsupported/Eigen/SparseExtra>

/**
 * This solver uses Eigen library to solve the linear set of equations
 */
void solver::cpuSolveSteady(const Eigen::SparseMatrix<VALUE> &matrix,
                            const state_type &p_vector, state_type &t_vector) {
  Eigen::PardisoLLT<Eigen::SparseMatrix<VALUE>> solver;
  solver.compute(matrix);
  t_vector = solver.solve(p_vector).eval();
}

struct OdeSystem {
  OdeSystem(const state_type &p_prime_vector,
            const Eigen::SparseMatrix<VALUE> &a_matrix)
      : p_prime_vector_(p_prime_vector), a_matrix_(a_matrix) {}

  void operator()(const state_type &t_vector, state_type &dTdt, VALUE _t = 0) {
    dTdt = p_prime_vector_ - a_matrix_ * t_vector;
  }

protected:
  const state_type &p_prime_vector_;
  const Eigen::SparseMatrix<VALUE> &a_matrix_;
};

struct OdeSystemJacobi : OdeSystem {
  using OdeSystem::OdeSystem;

  void operator()(const state_type &t_vector, Eigen::SparseMatrix<VALUE> &J,
                  const VALUE &_t, state_type &dfdt) {
    J = -a_matrix_;
    dfdt.setZero(dfdt.size());
  }
};

struct OdeSystemEulerJacobi : OdeSystem {
  using OdeSystem::OdeSystem;

  void operator()(const state_type &t_vector, Eigen::SparseMatrix<VALUE> &J,
                  const VALUE &_t) {
    J = -a_matrix_;
  }
};

void solver::writeTemp(const state_type &t_vector_, const VALUE t) {
  cout << t << ":\t" << t_vector_(0) - 273.15 << " C"
       << "\n";
}

#define DT (VALUE)1e-5
#define ABS_ERROR (VALUE)1E-1
#define REL_ERROR (VALUE)1E-1

using boost::numeric::odeint::integrate_adaptive;
using boost::numeric::odeint::integrate_const;
void euler_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                  const state_type &p_prime_vector, state_type &t_vector,
                  VALUE time_start, VALUE time_end, bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit =
        integrate_adaptive(boost::numeric::odeint::euler<state_type, VALUE>(),
                           OdeSystem(p_prime_vector, a_matrix), t_vector,
                           time_start, time_end, DT);
  } else {
    steps_explicit =
        integrate_const(boost::numeric::odeint::euler<state_type, VALUE>(),
                        OdeSystem(p_prime_vector, a_matrix), t_vector,
                        time_start, time_end, DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void modified_midpoint_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                              const state_type &p_prime_vector,
                              state_type &t_vector, VALUE time_start,
                              VALUE time_end, bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        boost::numeric::odeint::modified_midpoint<state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        boost::numeric::odeint::modified_midpoint<state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void runge_kutta4_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                         const state_type &p_prime_vector, state_type &t_vector,
                         VALUE time_start, VALUE time_end,
                         bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        boost::numeric::odeint::runge_kutta4<state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        boost::numeric::odeint::runge_kutta4<state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void runge_kutta_cash_karp54_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                                    const state_type &p_prime_vector,
                                    state_type &t_vector, VALUE time_start,
                                    VALUE time_end, bool adaptive = true) {
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<state_type, VALUE>
      error_stepper_type;
  size_t steps_explicit;

  if (adaptive) {
    steps_explicit = integrate_adaptive(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void runge_kutta_dopri5_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                               const state_type &p_prime_vector,
                               state_type &t_vector, VALUE time_start,
                               VALUE time_end, bool adaptive = true) {
  typedef boost::numeric::odeint::runge_kutta_dopri5<state_type, VALUE>
      error_stepper_type;
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void runge_kutta_fehlberg78_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                                   const state_type &p_prime_vector,
                                   state_type &t_vector, VALUE time_start,
                                   VALUE time_end, bool adaptive = true) {
  typedef boost::numeric::odeint::runge_kutta_fehlberg78<state_type, VALUE>
      error_stepper_type;
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        make_controlled(ABS_ERROR, REL_ERROR, error_stepper_type()),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void adams_bashforth_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                            const state_type &p_prime_vector,
                            state_type &t_vector, VALUE time_start,
                            VALUE time_end, bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        boost::numeric::odeint::adams_bashforth<4, state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        boost::numeric::odeint::adams_bashforth<4, state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void adams_bashforth_moulton_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                                    const state_type &p_prime_vector,
                                    state_type &t_vector, VALUE time_start,
                                    VALUE time_end, bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = integrate_adaptive(
        boost::numeric::odeint::adams_bashforth_moulton<4, state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  } else {
    steps_explicit = integrate_const(
        boost::numeric::odeint::adams_bashforth_moulton<4, state_type, VALUE>(),
        OdeSystem(p_prime_vector, a_matrix), t_vector, time_start, time_end,
        DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void bulirsch_stoer_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                           const state_type &p_prime_vector,
                           state_type &t_vector, VALUE time_start,
                           VALUE time_end, bool adaptive = true) {
  boost::numeric::odeint::bulirsch_stoer_dense_out<state_type, VALUE> stepper(
      5E-1, 1E-1);
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit =
        integrate_adaptive(stepper, OdeSystem(p_prime_vector, a_matrix),
                           t_vector, time_start, time_end, DT);
  } else {
    steps_explicit =
        integrate_const(stepper, OdeSystem(p_prime_vector, a_matrix), t_vector,
                        time_start, time_end, DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void implicit_euler_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                           const state_type &p_prime_vector,
                           state_type &t_vector, VALUE time_start,
                           VALUE time_end, bool adaptive = true) {
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = boost::numeric::odeint::integrate_adaptive(
        boost::numeric::odeint::implicit_euler<VALUE>(ABS_ERROR),
        std::make_pair(OdeSystem(p_prime_vector, a_matrix),
                       OdeSystemEulerJacobi(p_prime_vector, a_matrix)),
        t_vector, time_start, time_end, DT);
  } else {
    steps_explicit = boost::numeric::odeint::integrate_const(
        boost::numeric::odeint::implicit_euler<VALUE>(ABS_ERROR),
        std::make_pair(OdeSystem(p_prime_vector, a_matrix),
                       OdeSystemEulerJacobi(p_prime_vector, a_matrix)),
        t_vector, time_start, time_end, DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void rosenbrock_method(const Eigen::SparseMatrix<VALUE> &a_matrix,
                       const state_type &p_prime_vector, state_type &t_vector,
                       VALUE time_start, VALUE time_end, bool adaptive = true) {
  typedef boost::numeric::odeint::rosenbrock4_controller<
      boost::numeric::odeint::rosenbrock4<VALUE>>
      controlled_stepper_type;
  size_t steps_explicit;
  if (adaptive) {
    steps_explicit = boost::numeric::odeint::integrate_adaptive(
        controlled_stepper_type(ABS_ERROR, REL_ERROR),
        std::make_pair(OdeSystem(p_prime_vector, a_matrix),
                       OdeSystemJacobi(p_prime_vector, a_matrix)),
        t_vector, time_start, time_end, DT);
  } else {
    steps_explicit = boost::numeric::odeint::integrate_const(
        controlled_stepper_type(ABS_ERROR, REL_ERROR),
        std::make_pair(OdeSystem(p_prime_vector, a_matrix),
                       OdeSystemJacobi(p_prime_vector, a_matrix)),
        t_vector, time_start, time_end, DT);
  }
  cout << "Number of steps: " << steps_explicit << "\n";
}

void solver::cpuSolveTransient(
    const Eigen::SparseMatrix<VALUE> &a_matrix,
    const Eigen::DiagonalMatrix<VALUE, Eigen::Dynamic, Eigen::Dynamic> &invC,
    const state_type &p_vector, state_type &t_vector, VALUE time_start,
    VALUE time_end) {

  // utils::dumpVector(invC.diagonal(), "inv_c.mtx");
  /* Calculating P' = C^-1 * P  */
  state_type p_prime_vector = invC * p_vector;

  const int k = 21;

  switch (k) {
  case 0:
    euler_method(a_matrix, p_prime_vector, t_vector, time_start, time_end,
                 false);
    break;
  case 1:
    euler_method(a_matrix, p_prime_vector, t_vector, time_start, time_end,
                 true);
    break;
  case 2:
    modified_midpoint_method(a_matrix, p_prime_vector, t_vector, time_start,
                             time_end, false);
    break;
  case 3:
    modified_midpoint_method(a_matrix, p_prime_vector, t_vector, time_start,
                             time_end, true);
    break;
  case 4:
    runge_kutta4_method(a_matrix, p_prime_vector, t_vector, time_start,
                        time_end, false);
    break;
  case 5:
    runge_kutta4_method(a_matrix, p_prime_vector, t_vector, time_start,
                        time_end, true);
    break;
  case 6:
    runge_kutta_cash_karp54_method(a_matrix, p_prime_vector, t_vector,
                                   time_start, time_end, false);
    break;
  case 7:
    runge_kutta_cash_karp54_method(a_matrix, p_prime_vector, t_vector,
                                   time_start, time_end, true);
    break;
  case 8:
    runge_kutta_dopri5_method(a_matrix, p_prime_vector, t_vector, time_start,
                              time_end, false);
    break;
  case 9:
    runge_kutta_dopri5_method(a_matrix, p_prime_vector, t_vector, time_start,
                              time_end, true);
    break;
  case 10:
    runge_kutta_fehlberg78_method(a_matrix, p_prime_vector, t_vector,
                                  time_start, time_end, false);
    break;
  case 11:
    runge_kutta_fehlberg78_method(a_matrix, p_prime_vector, t_vector,
                                  time_start, time_end, true);
    break;
  case 12:
    adams_bashforth_method(a_matrix, p_prime_vector, t_vector, time_start,
                           time_end, false);
    break;
  case 13:
    adams_bashforth_method(a_matrix, p_prime_vector, t_vector, time_start,
                           time_end, true);
    break;
  case 14:
    adams_bashforth_moulton_method(a_matrix, p_prime_vector, t_vector,
                                   time_start, time_end, false);
    break;
  case 15:
    adams_bashforth_moulton_method(a_matrix, p_prime_vector, t_vector,
                                   time_start, time_end, true);
    break;
  case 16:
    bulirsch_stoer_method(a_matrix, p_prime_vector, t_vector, time_start,
                          time_end, false);
    break;
  case 17:
    bulirsch_stoer_method(a_matrix, p_prime_vector, t_vector, time_start,
                          time_end, true);
    break;
  case 18:
    implicit_euler_method(a_matrix, p_prime_vector, t_vector, time_start,
                          time_end, false);
    break;
  case 19:
    implicit_euler_method(a_matrix, p_prime_vector, t_vector, time_start,
                          time_end, true);
    break;
  case 20:
    rosenbrock_method(a_matrix, p_prime_vector, t_vector, time_start, time_end,
                      false);
    break;
  case 21:
    rosenbrock_method(a_matrix, p_prime_vector, t_vector, time_start, time_end,
                      true);
    break;
  default:
    exit(-1);
  }
}

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_SPLIT_INTERVAL_SET_HPP_JOFA_990223
#define BOOST_ICL_SPLIT_INTERVAL_SET_HPP_JOFA_990223

#include <boost/icl/type_traits/is_interval_splitter.hpp>
#include <boost/icl/interval_base_set.hpp>
#include <boost/icl/interval_set.hpp>

namespace boost{namespace icl
{

/** \brief implements a set as a set of intervals - on insertion
    overlapping intervals are split */
template
<
    typename                  DomainT,
    ICL_COMPARE               Compare  = ICL_COMPARE_INSTANCE(ICL_COMPARE_DEFAULT, DomainT),
    ICL_INTERVAL(ICL_COMPARE) Interval = ICL_INTERVAL_INSTANCE(ICL_INTERVAL_DEFAULT, DomainT, Compare),
    ICL_ALLOC                 Alloc    = std::allocator
>
class split_interval_set:
    public interval_base_set<split_interval_set<DomainT,Compare,Interval,Alloc>,
                             DomainT,Compare,Interval,Alloc>
{
public:
    typedef split_interval_set<DomainT,Compare,Interval,Alloc> type;
    typedef interval_base_set<type,DomainT,Compare,Interval,Alloc> base_type;

    typedef interval_set<DomainT,Compare,Interval,Alloc> joint_type;
    typedef type overloadable_type;
    typedef type key_object_type;

    /// The domain type of the set
    typedef DomainT   domain_type;
    /// The codomaintype is the same as domain_type
    typedef DomainT   codomain_type;

    /// The element type of the set
    typedef DomainT   element_type;
    /// The interval type of the set
    typedef ICL_INTERVAL_TYPE(Interval,DomainT,Compare) interval_type;
    /// The segment type of the set
    typedef interval_type   segment_type;

    /// Comparison functor for domain values
    typedef ICL_COMPARE_DOMAIN(Compare,DomainT) domain_compare;
    /// Comparison functor for intervals
    typedef exclusive_less_than<interval_type> interval_compare;

    /// Comparison functor for keys
    typedef exclusive_less_than<interval_type> key_compare;

    /// The allocator type of the set
    typedef Alloc<interval_type> allocator_type;

    /// allocator type of the corresponding element set
    typedef Alloc<DomainT> domain_allocator_type;

    /// The corresponding atomized type representing this interval container of elements
    typedef typename base_type::atomized_type atomized_type;

    /// Container type for the implementation
    typedef typename base_type::ImplSetT ImplSetT;

    /// key type of the implementing container
    typedef typename ImplSetT::key_type   key_type;
    /// data type of the implementing container
    typedef typename ImplSetT::value_type data_type;
    /// value type of the implementing container
    typedef typename ImplSetT::value_type value_type;

    /// iterator for iteration over intervals
    typedef typename ImplSetT::iterator iterator;
    /// const_iterator for iteration over intervals
    typedef typename ImplSetT::const_iterator const_iterator;

    enum { fineness = 3 };

public:
    //==========================================================================
    //= Construct, copy, destruct
    //==========================================================================
    /// Default constructor for the empty object
    split_interval_set(): base_type() {}

    /// Copy constructor
    split_interval_set(const split_interval_set& src): base_type(src) {}

    /// Copy constructor for base_type
    template<class SubType>
    split_interval_set
        (const interval_base_set<SubType,DomainT,Compare,Interval,Alloc>& src)
    { this->assign(src); }

    /// Constructor for a single element
    explicit split_interval_set(const interval_type& elem): base_type() { this->add(elem); }
    /// Constructor for a single interval
    explicit split_interval_set(const domain_type& itv): base_type() { this->add(itv); }

    /// Assignment operator
    split_interval_set& operator = (const split_interval_set& src)
    {
        base_type::operator=(src);
        return *this;
    }

    /// Assignment operator for base type
    template<class SubType>
    split_interval_set& operator =
        (const interval_base_set<SubType,DomainT,Compare,Interval,Alloc>& src)
    { this->assign(src); return *this; }

    /// Assignment from a base interval_set.
    template<class SubType>
    void assign(const interval_base_set<SubType,DomainT,Compare,Interval,Alloc>& src)
    {
        this->clear();
        this->_set.insert(src.begin(), src.end());
    }

#   ifndef BOOST_ICL_NO_CXX11_RVALUE_REFERENCES
    //==========================================================================
    //= Move semantics
    //==========================================================================

    /// Move constructor
    split_interval_set(split_interval_set&& src)
        : base_type(boost::move(src))
    {}

    /// Move assignment operator
    split_interval_set& operator = (split_interval_set&& src)
    {
        base_type::operator=(boost::move(src));
        return *this;
    }
    //==========================================================================
#   endif // BOOST_ICL_NO_CXX11_RVALUE_REFERENCES

private:
    // Private functions that shall be accessible by the baseclass:
    friend class
        interval_base_set<split_interval_set<DomainT,Compare,Interval,Alloc>,
                                             DomainT,Compare,Interval,Alloc>;

    iterator handle_inserted(iterator inserted_)
    {
        return inserted_;
    }

    iterator add_over(const interval_type& addend, iterator last_)
    {
        iterator first_ = this->_set.lower_bound(addend);
        //BOOST_ASSERT(next(last_) == this->_set.upper_bound(inter_val));

        iterator it_ = first_;
        interval_type rest_interval = addend;

        this->add_front(rest_interval, it_);
        this->add_main (rest_interval, it_, last_);
        this->add_rear (rest_interval, it_);
        return it_;
    }

    iterator add_over(const interval_type& addend)
    {
        std::pair<iterator,iterator> overlap = this->equal_range(addend);
        iterator first_ = overlap.first,
                 end_   = overlap.second,
                 last_  = end_; --last_;

        iterator it_ = first_;
        interval_type rest_interval = addend;

        this->add_front(rest_interval, it_);
        this->add_main (rest_interval, it_, last_);
        this->add_rear (rest_interval, it_);

        return it_;
    }

} ;

//-----------------------------------------------------------------------------
// type traits
//-----------------------------------------------------------------------------
template <class DomainT, ICL_COMPARE Compare, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_set<icl::split_interval_set<DomainT,Compare,Interval,Alloc> >
{
    typedef is_set<icl::split_interval_set<DomainT,Compare,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class DomainT, ICL_COMPARE Compare, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_interval_container<icl::split_interval_set<DomainT,Compare,Interval,Alloc> >
{
    typedef is_interval_container<icl::split_interval_set<DomainT,Compare,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class DomainT, ICL_COMPARE Compare, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct is_interval_splitter<icl::split_interval_set<DomainT,Compare,Interval,Alloc> >
{
    typedef is_interval_splitter<icl::split_interval_set<DomainT,Compare,Interval,Alloc> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

//-----------------------------------------------------------------------------
// type representation
//-----------------------------------------------------------------------------
template <class DomainT, ICL_COMPARE Compare, ICL_INTERVAL(ICL_COMPARE)  Interval, ICL_ALLOC Alloc>
struct type_to_string<icl::split_interval_set<DomainT,Compare,Interval,Alloc> >
{
    static std::string apply()
    { return "sp_itv_set<"+ type_to_string<DomainT>::apply() +">"; }
};

}} // namespace icl boost

#endif // BOOST_ICL_SPLIT_INTERVAL_SET_HPP_JOFA_990223


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-boolean
    This function object conditionally returns  @c x+y (respectively @c x)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/if_plus.hpp>

    @par Example:

      @snippet if_plus.cpp if_plus

    @par Possible output:

      @snippet if_plus.txt if_plus

  **/
  Value1 if_plus(Value0 const& c, Value1 const& x, Value1 const& y);
} }
#endif

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

#endif


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

#include <armadillo>

using namespace std;
using namespace arma;

#define TEST1_EXPORTS

#ifdef TEST1_EXPORTS
#define TEST1_API __declspec(dllexport)
#else
#define TEST1_API __declspec(dllimport)
#endif

extern "C"
{

	//
	//
	// note: check %% in config.hpp
	//
	//

	TEST1_API void ArmadilloLib_MatrixMul(int monoDim, double m1[], double m2[], double mRes[])
	{
		mat A(m1, monoDim, monoDim);
		mat B(m2, monoDim, monoDim);

		mat Res = A * B;

		int indexRoot = 0;
		for(int row = 0; row < monoDim; row++, indexRoot+=monoDim)
			for(int col = 0; col < monoDim; col++)
				mRes[indexRoot + col] = Res(col, row); // this is correct
	}

	//LAPACK is needed
	//TEST1_API double ArmadilloLib_CalcDeterminant(int monoDim, double m[])
	//{
	//	mat mArm(m, monoDim, monoDim);
	//	double res = det(mArm);
	//	return res;
	//}

	TEST1_API int ArmadilloLib_TestBase()
	{
		mat A(2,3);
		return A.n_rows;
	}
}


#ifndef INPUT_PREPROCESSOR_HH
#define INPUT_PREPROCESSOR_HH

#include "lwtnn/lightweight_network_config.hh"
#include "lwtnn/Exceptions.hh"

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

namespace lwt {

  template<typename T>
  using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;

  template<typename T>
  using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  // use a normal map externally, since these are more common in user
  // code.  TODO: is it worth changing to unordered_map?
  typedef std::map<std::string, double> ValueMap;
  typedef std::vector<std::pair<std::string, double> > ValueVector;
  typedef std::map<std::string, std::vector<double> > VectorMap;

  // ______________________________________________________________________
  // input preprocessor (handles normalization and packing into Eigen)

  template<typename T>
  class InputPreprocessorT
  {
  public:
    InputPreprocessorT(const std::vector<Input>& inputs);
    VectorX<T> operator()(const ValueMap&) const;
  private:
    // input transformations
    VectorX<T> m_offsets;
    VectorX<T> m_scales;
    std::vector<std::string> m_names;
  };

  using InputPreprocessor = InputPreprocessorT<double>;

  template<typename T>
  class InputVectorPreprocessorT
  {
  public:
    InputVectorPreprocessorT(const std::vector<Input>& inputs);
    MatrixX<T> operator()(const VectorMap&) const;
  private:
    // input transformations
    VectorX<T> m_offsets;
    VectorX<T> m_scales;
    std::vector<std::string> m_names;
  };

  using InputVectorPreprocessor = InputVectorPreprocessorT<double>;
}

#include "InputPreprocessor.txx"

#endif


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <json/json.h>
#include <NTL/RR.h>
#include <NTL/ZZ_p.h>
#include "HE2Encipher.h"
#include "Random.h"

NTL::ZZ HE2Encipher::ONE = NTL::ZZ(1);
NTL::vec_ZZ_p HE2Encipher::ONE_VECTOR;

HE2Encipher::HE2Encipher()
{
	pmod=nullptr;
	a=nullptr;
	gamma=nullptr;
	R=nullptr;
};

HE2Encipher::~HE2Encipher()
{
	delete pmod;
	delete a;
	delete gamma;
	delete R;
};

std::string HE2Encipher::writeParametersToJSON()
{
	Json::Value root;
	std::ostringstream modStr;
	modStr << modulus;
	root["modulus"] = modStr.str();
	Json::Value RArray(Json::arrayValue);
	for (int i=0; i < R->NumRows(); i++){
		Json::Value RRow(Json::arrayValue);
		for (int j=0; j < R->NumCols(); j++){
			std::ostringstream eltStr;
			eltStr << R->get(i,j);
			RRow[j]= eltStr.str();
		}
		RArray[i] = RRow;
	}
	root["R"] = RArray;
	Json::FastWriter writer;
	std::string json = writer.write(root);
	return json;
};

std::string HE2Encipher::encryptToString(NTL::ZZ& plaintext)
{
	Json::Value root(Json::arrayValue);
	NTL::vec_ZZ_p encrypted = encrypt(plaintext);
	std::ostringstream ss;
	ss << encrypted;
	return ss.str();
};

std::string HE2Encipher::encryptToString(int16_t plaintext)
{
	NTL::ZZ p = NTL::ZZ(plaintext);
	return encryptToString(p);
};

std::string HE2Encipher::encryptToString(int32_t plaintext)
{
	NTL::ZZ p = NTL::ZZ(plaintext);
	return encryptToString(p);
};

std::string HE2Encipher::encryptToString(int64_t plaintext)
{
	NTL::ZZ p = NTL::ZZ(plaintext);
	return encryptToString(p);
};

void HE2Encipher::generateParameters(int lambda, int eta){
	generateModulus(lambda,eta);

	/* Set local modulus to pq */
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p ptmp = NTL::to_ZZ_p(p);
	pmod = new NTL::ZZ_p(ptmp);

	/* Configure common HE2x vectors and matrices */
    /* Initialise ONE_VECTOR */
	HE2Encipher::ONE_VECTOR.SetLength(2);
	HE2Encipher::ONE_VECTOR[0] = NTL::to_ZZ_p(HE2Encipher::ONE);
	HE2Encipher::ONE_VECTOR[1] = NTL::to_ZZ_p(HE2Encipher::ONE);

	/* Set constants for calculations below */
    NTL::ZZ_p one(1);
    NTL::ZZ_p two(2);

    /* Generate a */
    Random rnd = Random();

	NTL::ZZ a1temp;
	do{
		a1temp = rnd.nextBigInteger(ONE, modulus);
	} while (a1temp % p == 0 || a1temp % q == 0);
	NTL::ZZ a2temp;
	do {
		a2temp = rnd.nextBigInteger(ONE, modulus);
	} while (a1temp == a2temp || a2temp % p == 0 || a2temp % q == 0 || SubMod(a1temp,a2temp,p)==0 || SubMod(a1temp,a2temp,q)==0);
	NTL::ZZ_p a1 = NTL::to_ZZ_p(a1temp);
	NTL::ZZ_p a2 = NTL::to_ZZ_p(a2temp);
	a = new NTL::vec_ZZ_p;
	a->SetLength(2);
	a->put(0,a1);
	a->put(1,a2);

	/* Generate gamma */
	NTL::ZZ_p adiff = a2-a1;
	NTL::ZZ_p adiffinv = NTL::inv(adiff);
    NTL::ZZ_p gamma1 = adiffinv*a2;
    NTL::ZZ_p gamma2 = adiffinv*(-a1);
    gamma = new NTL::vec_ZZ_p;
    gamma->SetLength(2);
    gamma->put(0,gamma1);
    gamma->put(1,gamma2);

    /* Generate R */
    NTL::ZZ_p r = NTL::to_ZZ_p(rnd.nextBigInteger(q));
    NTL::ZZ_p s = NTL::to_ZZ_p(rnd.nextBigInteger(modulus));
    NTL::ZZ_p beta = adiff*adiff*two;
    NTL::ZZ_p betainv = NTL::inv(beta);
    NTL::ZZ_p rp = r*NTL::to_ZZ_p(p);
    NTL::ZZ_p alpha1 = betainv*(s*a1+rp - a1*a1);
    NTL::ZZ_p alpha2 = betainv*(s*a2+rp - a2*a2);
    R = new NTL::mat_ZZ_p;
    R->SetDims(2, 3);
    NTL::ZZ_p R11 = one - two*alpha1;
    R->put(0,0,R11);
    NTL::ZZ_p R12 = alpha1;
    R->put(0,1,R12);
    NTL::ZZ_p R13 = alpha1;
    R->put(0,2,R13);
    NTL::ZZ_p R21 = -two*alpha2;
    R->put(1,0,R21);
    NTL::ZZ_p R22 = alpha2+one;
    R->put(1,1,R22);
    NTL::ZZ_p R23 = alpha2;
    R->put(1,2,R23);
}


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

#include <optimization/visibility_control.hpp>
#include <Eigen/Core>
#include <vector>
#include <cstddef>
#include <memory>
#include <tuple>

namespace autoware
{
namespace common
{
namespace optimization
{
// Mathematical expression base class (CRTP for performance)
template<typename Derived, typename DomainValueT, int NumJacobianRowsT, int NumVarsT>
class Expression
{
protected:
public:
  static constexpr auto NumJacobianRows = NumJacobianRowsT;
  static constexpr auto NumVars = NumVarsT;
  using DomainValue = DomainValueT;
  using Value = double;
  using Jacobian = Eigen::Matrix<Value, NumJacobianRows, NumVars>;
  using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;

  Value operator()(const DomainValue & x)
  {
    return impl()(x);
  }
  Jacobian & jacobian(const DomainValue & x)
  {
    return impl().jacobian(x);
  }

  Hessian & hessian(const DomainValue & x)
  {
    return impl().hessian(x);
  }

private:
  const Derived & impl() const
  {
    return *static_cast<const Derived *>(this);
  }
};

// This is a generalized representation of an optimization problem. (CRTP base class)
template<typename Derived, typename DomainValueT, int NumJacobianRowsT, int NumVarsT,
  typename ObjectiveT, typename EqualityConstraintsT, typename InequalityConstraintsT>
class OptimizationProblem;

// Definition of OptimizationProblem.
//1. Since Optimization problem is just a collection of expressions, it's an expression itself as well and hence inherits
//      and implements the `Expression` class. (Uses identical parameters to its objective function.)
//2. Template specialization is used for type deduction and forwarding constraint parameter packs to the tuples.
// This way "std::tuple" is enforced to be used on the constrained OptimizationProblem implementations.

template<typename Derived,
  typename ... EqualityConstraints,
  typename ... InequalityConstraints,
  typename ObjectiveT, typename DomainValueT, int NumJacobianRowsT, int NumVarsT>
class OptimizationProblem<Derived, DomainValueT, NumJacobianRowsT, NumVarsT, ObjectiveT,
    std::tuple<EqualityConstraints...>,
    std::tuple<InequalityConstraints...>>
  : public Expression<
    OptimizationProblem<Derived, DomainValueT, NumJacobianRowsT, NumVarsT, ObjectiveT,
    std::tuple<EqualityConstraints...>,
    std::tuple<InequalityConstraints...>>,
    DomainValueT, NumJacobianRowsT, NumVarsT>
{
  // Same types and dimensions used as the objective function.
  using DomainValue = typename ObjectiveT::DomainValue;
  using Value = typename ObjectiveT::Value;
  using Jacobian = typename ObjectiveT::Jacobian;
  using Hessian = typename ObjectiveT::Hessian;

  Value operator()(const DomainValue & x);
  Jacobian & jacobian(const DomainValue & x);
  Hessian & hessian(const DomainValue & x);

  // getters, setters.

private:
  ObjectiveT m_objective;
  std::tuple<EqualityConstraints...> m_equality_constraints;
  std::tuple<InequalityConstraints...> m_inequality_constraints;
};

// convenience alias for defining unconstrained optimization problems.
template<typename Derived, typename DomainValueT, int NumVarsT, typename ObjectiveT>
using UnconstrainedOptimizationProblem = OptimizationProblem<Derived, DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>,
    std::tuple<>>;

}          // namespace autoware
}      // namespace common
}  // namespace optimization


#pragma once

#include <Eigen/Core>

#include <vector>
#include <array>
#include <cassert>

namespace BVH
{
	class BVH
	{
	private:
		std::vector<std::array<Eigen::Vector3d, 2>> boxlist;
		std::vector<int> new2old;
		size_t n_corners = -1;

		void init_boxes_recursive(const std::vector<std::array<Eigen::Vector3d, 2>> &cornerlist,
								  int node_index,
								  int b, int e);

		// void triangle_search_recursive(const Eigen::Vector3d &triangle0, const Eigen::Vector3d &triangle1, const Eigen::Vector3d &triangle2,
		// 							   std::vector<unsigned int> &list,
		// 							   int n, int b, int e) const;

		// void point_search_recursive(
		// 	const Eigen::Vector3d &point,
		// 	std::vector<unsigned int> &list,
		// 	int n, int b, int e) const;
		// void segment_search_recursive(
		// 	const Eigen::Vector3d &seg0, const Eigen::Vector3d &seg1,
		// 	std::vector<unsigned int> &list,
		// 	int n, int b, int e) const;

		void box_search_recursive(
			const Eigen::Vector3d &bbd0, const Eigen::Vector3d &bbd1,
			std::vector<unsigned int> &list,
			int n, int b, int e) const;

		static int max_node_index(int node_index, int b, int e);

		// bool triangle_intersects_box(const Eigen::Vector3d &tri0, const Eigen::Vector3d &tri1, const Eigen::Vector3d &tri2, int index) const;
		// bool point_intersects_box(const Eigen::Vector3d &p, int index) const;
		// bool segment_intersects_box(const Eigen::Vector3d &seg0, const Eigen::Vector3d &seg1, int index) const;
		bool box_intersects_box(const Eigen::Vector3d &bbd0, const Eigen::Vector3d &bbd1, int index) const;

		bool is_initialized = false;

	public:
		void init(const std::vector<std::array<Eigen::Vector3d, 2>> &cornerlist);
		void init(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F, const double tol);

		// inline void triangle_find_bbox(
		// 	const Eigen::Vector3d &triangle0, const Eigen::Vector3d &triangle1, const Eigen::Vector3d &triangle2,
		// 	std::vector<unsigned int> &list) const
		// {
		// 	assert(n_corners >= 0);
		// 	assert(boxlist.size() > 0);
		// 	// int de = algorithms::is_triangle_degenerated(triangle0, triangle1, triangle2);
		// 	// if (de == DEGENERATED_SEGMENT)
		// 	// {
		// 	// 	Eigen::Vector3d tmin, tmax;
		// 	// 	algorithms::get_tri_corners(triangle0, triangle1, triangle2, tmin, tmax);
		// 	// 	segment_find_bbox(tmin, tmax, list);
		// 	// 	return;
		// 	// }
		// 	// else if (de == DEGENERATED_POINT)
		// 	// {
		// 	// 	point_find_bbox(triangle0, list);
		// 	// 	return;
		// 	// }

		// 	triangle_search_recursive(triangle0, triangle1, triangle2, list, 1, 0, n_corners);
		// }

		// inline void intersect_point(const Eigen::Vector3d &p, std::vector<unsigned int> &list) const
		// {
		// 	assert(boxlist.size() > 0);
		// 	point_search_recursive(p, list, 1, 0, n_corners);
		// }
		// inline void intersect_segment(const Eigen::Vector3d &seg0, const Eigen::Vector3d &seg1, std::vector<unsigned int> &list) const
		// {
		// 	assert(boxlist.size() > 0);
		// 	segment_search_recursive(
		// 		seg0, seg1, list, 1, 0, n_corners);
		// }
		inline void intersect_box(const Eigen::Vector3d &bbd0, const Eigen::Vector3d &bbd1, std::vector<unsigned int> &list) const
		{
			std::vector<unsigned int> tmp;
			assert(n_corners >= 0);
			box_search_recursive(bbd0, bbd1, tmp, 1, 0, n_corners);

			list.resize(tmp.size());
			for (int i = 0; i < tmp.size(); ++i)
				list[i] = new2old[tmp[i]];
		}
	};
} // namespace BVH


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbRosuMappableDoseAccessor.h"

#include <boost/make_shared.hpp>

#include "rttbNullPointerException.h"
#include "rttbMappingOutsideOfImageException.h"
#include "rttbLinearInterpolation.h"

namespace rttb
{
	namespace interpolation
	{
		RosuMappableDoseAccessor::RosuMappableDoseAccessor(const core::GeometricInfo& geoInfoTargetImage,
            core::DoseAccessorInterface::ConstPointer doseMovingImage,
		        const TransformationInterface::Pointer aTransformation,
		        bool acceptPadding,
		        DoseTypeGy defaultOutsideValue): MappableDoseAccessorInterface(geoInfoTargetImage, doseMovingImage,
			                aTransformation, acceptPadding, defaultOutsideValue)
		{
			//define linear interpolation
      auto interpolationLinear = ::boost::make_shared<LinearInterpolation>();
			_spInterpolation = interpolationLinear;
			_spInterpolation->setAccessorPointer(_spOriginalDoseDataMovingImage);
		}

		GenericValueType RosuMappableDoseAccessor::getValueAt(const VoxelGridID aID) const
		{
			VoxelGridIndex3D aVoxelGridIndex3D;

			if (_geoInfoTargetImage.convert(aID, aVoxelGridIndex3D))
			{
				return getValueAt(aVoxelGridIndex3D);
			}
			else
			{
				if (_acceptPadding)
				{
					return _defaultOutsideValue;
				}
				else
				{
					throw core::MappingOutsideOfImageException("Error in conversion from index to world coordinates");
				}
			}
		}

		GenericValueType RosuMappableDoseAccessor::getValueAt(const VoxelGridIndex3D& aIndex) const
		{
			//Transform requested voxel coordinates of original image into world coordinates with RTTB
			WorldCoordinate3D worldCoordinateTarget;

			if (_geoInfoTargetImage.indexToWorldCoordinate(aIndex, worldCoordinateTarget))
			{
				std::vector<WorldCoordinate3D> octants = getOctants(worldCoordinateTarget);

				if (octants.size() > 2)
				{
					DoseTypeGy interpolatedDoseValue = 0.0;

					//get trilinear interpolation value of every octant point
					for (std::vector<WorldCoordinate3D>::const_iterator octantIterator = octants.begin();
					     octantIterator != octants.end();
					     ++octantIterator)
					{
						//transform coordinates
						WorldCoordinate3D worldCoordinateMoving;
						WorldCoordinate3D worldCoordinateTargetOctant = *octantIterator;
						_spTransformation->transformInverse(worldCoordinateTargetOctant, worldCoordinateMoving);

						try
						{
							interpolatedDoseValue += _spInterpolation->getValue(worldCoordinateMoving);
						}
						//Mapped outside of image? Check if padding is allowed
						catch (core::MappingOutsideOfImageException& /*e*/)
						{
							if (_acceptPadding)
							{
								interpolatedDoseValue += _defaultOutsideValue;
							}
							else
							{
								throw core::MappingOutsideOfImageException("Mapping outside of image");
							}
						}
						catch (core::Exception& e)
						{
							std::cout << e.what() << std::endl;
							return -1;
						}
					}

					return interpolatedDoseValue / (DoseTypeGy)octants.size();
				}
				else
				{
					if (_acceptPadding)
					{
						return _defaultOutsideValue;
					}
					else
					{
						throw core::MappingOutsideOfImageException("Too many samples are mapped outside the image!");
					}
				}
			}
			else
			{
				if (_acceptPadding)
				{
					return _defaultOutsideValue;
				}
				else
				{
					throw core::MappingOutsideOfImageException("Error in conversion from index to world coordinates");
				}
			}
		}

		std::vector<WorldCoordinate3D> RosuMappableDoseAccessor::getOctants(
		    const WorldCoordinate3D& aCoordinate) const
		{
			std::vector<WorldCoordinate3D> octants;
			SpacingVectorType3D spacingTargetImage = _geoInfoTargetImage.getSpacing();

			core::GeometricInfo geometricInfoDoseData = _spOriginalDoseDataMovingImage->getGeometricInfo();

			//as the corner point is the coordinate of the voxel (grid), 0.25 and 0.75 are the center of the subvoxels
			for (double xOct = -0.25; xOct <= 0.25; xOct += 0.5)
			{
				for (double yOct = -0.25; yOct <= 0.25; yOct += 0.5)
				{
					for (double zOct = -0.25; zOct <= 0.25; zOct += 0.5)
					{
						WorldCoordinate3D aWorldCoordinate(aCoordinate.x() + (xOct * spacingTargetImage.x()),
						                                   aCoordinate.y() + (yOct * spacingTargetImage.y()),
						                                   aCoordinate.z() + (zOct * spacingTargetImage.z()));

						if (geometricInfoDoseData.isInside(aWorldCoordinate))
						{
							octants.push_back(aWorldCoordinate);
						}
					}
				}
			}

			return octants;
		}

	}//end namespace interpolation
}//end namespace rttb


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#include "VertexWelder.hpp"
#include "../UnorderedMap.hpp"
#include <boost/functional/hash.hpp>
#include <cmath>

namespace Thea {
namespace Graphics {

class VertexWelderImpl
{
  private:
    struct WeldableVertex
    {
      WeldableVertex(void * v, Vector3 const & p) : vertex(v), position(p) {}

      void * vertex;
      Vector3 position;
    };

    struct Long3
    {
      intx x, y, z;

      Long3(intx x_, intx y_, intx z_) : x(x_), y(y_), z(z_) {}

      bool operator==(Long3 const & other) const { return x == other.x && y == other.y && z == other.z; }
    };

    struct Long3Hasher
    {
      std::size_t operator()(Long3 const & a) const
      {
        std::size_t s = boost::hash_value(a.x);
        boost::hash_combine(s, a.y);
        boost::hash_combine(s, a.z);
        return s;
      }
    };

    typedef UnorderedMultiMap<Long3, WeldableVertex, Long3Hasher> VertexMap;

    Long3 toGrid(Vector3 const & pos) const
    {
      return Long3(static_cast<intx>(std::floor(pos.x() / (double)weld_radius)),
                   static_cast<intx>(std::floor(pos.y() / (double)weld_radius)),
                   static_cast<intx>(std::floor(pos.z() / (double)weld_radius)));
    }

    Real weld_radius;
    Real squared_weld_radius;
    VertexMap vertex_map;

  public:
    VertexWelderImpl(Real weld_radius_) : weld_radius(weld_radius_), squared_weld_radius(weld_radius_ * weld_radius_) {}

    void addVertex(void * vertex, Vector3 const & position)
    {
      WeldableVertex value(vertex, position);
      Long3 key = toGrid(position);
      vertex_map.insert(VertexMap::value_type(key, value));
    }

    void * getVertex(Vector3 const & position) const
    {
      typedef std::pair<VertexMap::const_iterator, VertexMap::const_iterator> IteratorRange;
      Long3 base_key = toGrid(position);

      // Every vertex within the welding distance of the given point must be inside a neighbor of the grid cell containing the
      // point
      for (int dx = -1; dx <= 1; ++dx)
        for (int dy = -1; dy <= 1; ++dy)
          for (int dz = -1; dz <= 1; ++dz)
          {
            Long3 key(base_key.x + dx, base_key.y + dy, base_key.z + dz);
            IteratorRange nbrs = vertex_map.equal_range(key);
            for (VertexMap::const_iterator ni = nbrs.first; ni != nbrs.second; ++ni)
              if ((ni->second.position - position).squaredNorm() <= squared_weld_radius)
                return ni->second.vertex;
          }

      return nullptr;
    }

}; // class VertexWelderImpl

VertexWelder::VertexWelder(Real weld_radius)
: impl(new VertexWelderImpl(weld_radius))
{
  alwaysAssertM(weld_radius >= 0, "VertexWelder: Weld radius cannot be negative");
}

VertexWelder::~VertexWelder()
{
  delete impl;
}

void
VertexWelder::addVertex(void * vertex, Vector3 const & position)
{
  impl->addVertex(vertex, position);
}

void *
VertexWelder::getVertex(Vector3 const & position) const
{
  return impl->getVertex(position);
}

} // namespace Graphics
} // namespace Thea


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
using namespace boost::numeric::ublas;

#include <google/dense_hash_map>
#include <boost/functional/hash.hpp>
using google::dense_hash_map;

//typedef matrix_range< matrix<double> > mr;
//typedef vector_range< vector <double> > vr;

#define PI 3.14159265359

//double gdb_get( matrix<double> & m, int i, int j) {
//  return m(i,j);
//}

#ifdef DEBUG
#define D(x) x
#else
#define D(x)
#endif

class StateSpaceModel {

public:
  virtual int apply_transition_matrix(const double * x,
					int k, double * result) = 0;
  virtual int apply_transition_matrix( const matrix<double> &X,
					 unsigned int x_row_offset,
					 int k,
					 matrix<double> &result,
					 unsigned int r_row_offset,
					 unsigned int n)  = 0;
  virtual void transition_bias(int k, double * result) = 0;
  virtual void transition_noise_diag(int k, double * result) = 0;
  virtual double apply_observation_matrix(const  double * x, int k) = 0;

  virtual void apply_observation_matrix(const matrix<double> &X, unsigned int row_offset, int k, double * result, double *result_tmp, unsigned int n) = 0;
  virtual double observation_bias(int k) = 0;
  virtual double observation_noise(int k) = 0;
  virtual int prior_mean(double * result) = 0;
  virtual int prior_vars(double * result) = 0;
  virtual bool stationary(int k) = 0;
  virtual unsigned int state_size_at_timestep(unsigned int k) {
    return this->max_dimension;
  };

  virtual ~StateSpaceModel() { return; };

  unsigned int max_dimension;
  bool is_cssm;
};

class FilterState {
public:

  unsigned int state_size;

  bool wasnan;
  bool at_fixed_point;
  double alpha;
  double pred_z;
  matrix<double> obs_U;
  vector<double> obs_d;
  vector<double> gain;
  matrix<double> pred_U;
  vector<double> pred_d;

  matrix<double> tmp_U1;
  matrix<double> tmp_U2;
  matrix<double> P;

  vector<double> f;
  vector<double> v;

  vector<double> xk;

  double eps_stationary;

  FilterState (int max_dimension, double eps_stationary);
  void init_priors(StateSpaceModel &ssm);
  void init_incremental_state(StateSpaceModel &ssm, int k);

};

class CompactSupportSSM : public StateSpaceModel {
public:
  unsigned int n_basis;
  unsigned int n_steps;
  const vector<double> & coef_means;
  const vector<double> & coef_vars;

  CompactSupportSSM(const vector<int> & start_idxs, const vector<int> & end_idxs,
		    const vector<int> & identities, const matrix<double> & basis_prototypes,
		    const vector<double> & coef_means,  const vector<double> & coef_vars,
		    double obs_noise, double bias);

  ~CompactSupportSSM();

  int apply_transition_matrix(const double *x, int k, double * result);
  int apply_transition_matrix( const matrix<double> &X,
				 unsigned int x_row_offset,
				 int k,
				 matrix<double> &result,
				 unsigned int r_row_offset,
				 unsigned int n) ;
  void transition_bias (int k, double * result) ;
  void transition_noise_diag(int k, double * result);
  double apply_observation_matrix(const double * x, int k);
  void apply_observation_matrix(const matrix<double> &X, unsigned int row_offset, int k, double * result, double *result_tmp, unsigned int n);
  double observation_bias(int k);
  double observation_noise(int k);
  int prior_mean(double * result);
  int prior_vars(double * result);
  bool stationary(int k);
  void extract_coefs(const vector<double> &x,
		     const matrix<double> &P,
		     unsigned int state_offset,
		     int k,
		     vector<double> & coef_means,
		     vector<double> & coef_vars);

private:
dense_hash_map< std::pair<int, int>, int, boost::hash< std::pair< int,int> >  > active_indices;
  matrix<int> active_basis;

  const vector<int> & start_idxs;
  const vector<int> & end_idxs;
  const vector<int> & identities;
  const matrix <double> & basis_prototypes;

  double obs_noise;
  double bias;

};

class ARSSM : public StateSpaceModel {
public:
  ARSSM(vector<double> & params, double error_var,
	  double obs_noise, double bias);
  ~ARSSM();

  int apply_transition_matrix(const double *x, int k, double * result);
  int apply_transition_matrix( const matrix<double> &X,
				 unsigned int x_row_offset,
				 int k,
				 matrix<double> &result,
				 unsigned int r_row_offset,
				 unsigned int n) ;
  void transition_bias (int k, double * result) ;
  void transition_noise_diag(int k, double * result);
  double apply_observation_matrix(const double * x, int k);
  void apply_observation_matrix(const matrix<double> &X, unsigned int row_offset, int k, double * result, double *result_tmp, unsigned int n);
  double observation_bias(int k);
  double observation_noise(int k);
  int prior_mean(double * result);
  int prior_vars(double * result);
  bool stationary(int k);

  vector<double> & params;
  double error_var;
  double obs_noise;
  double bias;

};

class TransientCombinedSSM : public StateSpaceModel {
public:
TransientCombinedSSM(std::vector<StateSpaceModel *> & ssms, const vector<int> & start_idxs,
			 const vector<int> & end_idxs, const std::vector<const double * > & scales,
			 double obs_noise);
~TransientCombinedSSM();

  int apply_transition_matrix(const double *x, int k, double * result);
  int apply_transition_matrix( const matrix<double> &X,
				 unsigned int x_row_offset,
				 int k,
				 matrix<double> &result,
				 unsigned int r_row_offset,
				 unsigned int n) ;

  void transition_bias (int k, double * result) ;
  void transition_noise_diag(int k, double * result);
  double apply_observation_matrix(const double * x, int k);
  void apply_observation_matrix(const matrix<double> &X, unsigned int row_offset, int k, double * result, double *result_tmp, unsigned int n);
  double observation_bias(int k);
  double observation_noise(int k);
  int prior_mean(double * result);
  int prior_vars(double * result);
  unsigned int state_size_at_timestep(unsigned int k);
  bool stationary(int k);

  void init_coef_priors(std::vector<vector<double> > & cmeans,
			std::vector<vector<double> > & cvars);

  void extract_all_coefs(FilterState &cache, int k,
			 std::vector<vector<double> > & cmeans,
			 std::vector<vector<double> > & cvars);
  void extract_component_means(double *xk, int k,
			       std::vector<vector<double> > & means);
  void extract_component_vars(matrix<double> &P, matrix<double> &P_tmp, int k,
			       std::vector<vector<double> > & vars);

  const unsigned int n_ssms;
private:

  std::vector<StateSpaceModel *> & ssms;
  const vector<int> & start_idxs;
  const vector<int> & end_idxs;
  const std::vector<const double * > & scales;

  const double obs_noise;
  unsigned int n_steps;

  int active_ssm_cache1_k;
  int active_ssm_cache1_v;
  int active_ssm_cache2_k;
  int active_ssm_cache2_v;

  vector<unsigned int> ssms_tmp;
  std::vector<int> changepoints;

  matrix<int> active_sets;

  int active_set_idx(int k);

};

const int ERR_INCR_NUMERIC = -1;
const int ERR_INCR_INIT = -2;

double kalman_observe_sqrt(StateSpaceModel &ssm, FilterState &cache, int k, double zk);
int kalman_predict_sqrt(StateSpaceModel &ssm, FilterState &cache, int k);
double filter_likelihood(StateSpaceModel &ssm, const vector<double> &z);
double filter_incremental(StateSpaceModel &ssm,
			  const vector<double> &z,
			  double * ells,
			  int filter_start_idx,
			  int incr_start_idx,
			  int incr_end_idx,
			  int update_ells,
			  double step_ell_tol,
			  int * steps_processed,
			  int * errcode);

void mean_obs(StateSpaceModel &ssm, vector<double> & result);
void obs_var(StateSpaceModel &ssm, vector<double> & result);
void prior_sample(StateSpaceModel &ssm, vector<double> & result, unsigned long seed);

double tssm_component_means(TransientCombinedSSM &tssm,
			  const vector<double> &z,
			  std::vector<vector<double> > & means);
double tssm_component_vars(TransientCombinedSSM &tssm,
			  const vector<double> &z,
			  std::vector<vector<double> > & vars);
double all_filtered_cssm_coef_marginals(TransientCombinedSSM &ssm,
				      const vector<double> &z,
				      vector<double> & step_ells,
				      std::vector<vector<double> > & cmeans,
				      std::vector<vector<double> > & cvars);
  void step_obs_likelihoods(StateSpaceModel &ssm, const vector<double> &z,
			    vector<double> & ells,
			    vector<double> & preds,
			    vector<double> & alphas);


#pragma once

#include <crab/domains/abstract_domain_operators.hpp>
#include <crab/support/debug.hpp>
#include <crab/support/os.hpp>

#include <boost/optional.hpp>

namespace crab {
namespace domains {
namespace term {

typedef enum {
  TERM_OP_ADD,
  TERM_OP_SUB,
  TERM_OP_MUL,
  TERM_OP_SDIV,
  TERM_OP_UDIV,
  TERM_OP_SREM,
  TERM_OP_UREM,
  TERM_OP_NOT,
  TERM_OP_BAND,
  TERM_OP_BOR,
  TERM_OP_BXOR,
  TERM_OP_AND,
  TERM_OP_OR,
  TERM_OP_XOR,
  TERM_OP_SHL,
  TERM_OP_LSHR,
  TERM_OP_ASHR,
  TERM_OP_FUNCTION
} term_operator_t;

inline crab::crab_os &operator<<(crab::crab_os &o, term_operator_t op) {
  switch (op) {
  case TERM_OP_ADD:
    o << "+";
    break;
  case TERM_OP_SUB:
    o << "-";
    break;
  case TERM_OP_MUL:
    o << "*";
    break;
  case TERM_OP_SDIV:
    o << "/";
    break;
  case TERM_OP_UDIV:
    o << "/_u";
    break;
  case TERM_OP_SREM:
    o << "%";
    break;
  case TERM_OP_UREM:
    o << "%_u";
    break;
  case TERM_OP_NOT:
    o << "not";
    break;
  case TERM_OP_BAND:
    o << "band";
    break;
  case TERM_OP_BOR:
    o << "bor";
    break;
  case TERM_OP_BXOR:
    o << "bxor";
    break;
  case TERM_OP_AND:
    o << "&";
    break;
  case TERM_OP_OR:
    o << "|";
    break;
  case TERM_OP_XOR:
    o << "^";
    break;
  case TERM_OP_SHL:
    o << "<<";
    break;
  case TERM_OP_LSHR:
    o << ">>_l";
    break;
  case TERM_OP_ASHR:
    o << ">>_r";
    break;
  case TERM_OP_FUNCTION:
    o << "uf";
    break;
  default:
    CRAB_ERROR("unexpected binary operation ", op);
  }
  return o;
}

/* Convert between Crab operators and term domain uninterpreted functors */
term_operator_t conv2termop(arith_operation_t op);
term_operator_t conv2termop(bitwise_operation_t op);
term_operator_t conv2termop(bool_operation_t op);
boost::optional<arith_operation_t> conv2arith(term_operator_t op);
boost::optional<bitwise_operation_t> conv2bitwise(term_operator_t op);
boost::optional<bool_operation_t> conv2bool(term_operator_t op);
} // end namespace term
} // end namespace domains
} // end namespace crab


#include <cmath>
#include <iostream>
#include <limits>
#include <map>
#include <sstream>
#include <stdexcept>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem/fstream.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include "object.hpp"

namespace hw4 {
    SphereObject::SphereObject(
        float radius,
        glm::vec3 center,
        const std::shared_ptr<Material>& material
    )
        : Object(
            BoundingBox(glm::vec3(-1), glm::vec3(1)),
            glm::scale(glm::translate(glm::mat4(1.0), center), glm::vec3(radius)),
            material
        ) {}

    boost::optional<Intersection> SphereObject::find_intersection(const Ray& r) const {
        float b = glm::dot(2.0f * r.direction(), r.origin());
        float c = glm::dot(r.origin(), r.origin()) - 1;

        float qterm = b * b - 4 * c;

        if (qterm < 0)
            return boost::none;

        float t = -(b + std::sqrt(qterm)) / 2;

        if (t <= 0) {
            t = -(b - std::sqrt(qterm)) / 2;

            if (t <= 0) return boost::none;
        }

        auto p = r.origin() + t * r.direction();

        return Intersection(
            p,
            p,
            glm::vec2(
                0.5 + std::atan2(p.z, p.x) / tau,
                0.5 + std::asin(p.y) * 2 / tau
            ),
            this->material().get(),
            t
        );
    }

    boost::optional<Intersection> TriMesh::find_intersection(const Ray& r, const Triangle& tri) const {
        const auto& a = this->m_vertices[tri.a];
        const auto& b = this->m_vertices[tri.b];
        const auto& c = this->m_vertices[tri.c];

        // Use the Möller-Trumbore algorithm for intersection
        auto ab = b.pos - a.pos;
        auto ac = c.pos - a.pos;

        auto pvec = glm::cross(r.direction(), ac);
        float det = glm::dot(ab, pvec);

        // If det is close to zero, then the ray is parallel to the triangle, so we can bail early.
        if (std::fabs(det) <= 1e-7f) return boost::none;

        float inv_det = 1.0 / det;

        auto tvec = r.origin() - a.pos;
        float u = glm::dot(tvec, pvec) * inv_det;

        if (u < 0 || u > 1) return boost::none;

        auto qvec = glm::cross(tvec, ab);
        float v = glm::dot(r.direction(), qvec) * inv_det;

        if (v < 0 || u + v > 1) return boost::none;

        float t = glm::dot(ac, qvec) * inv_det;

        if (t < 0) return boost::none;

        // Now that we know an intersection occurs, we need to use u and v to perform barycentric
        // interpolation to find the correct attribute values
        return Intersection(
            r.origin() + t * r.direction(),
            glm::normalize((1 - u - v) * a.normal + u * b.normal + v * c.normal),
            (1 - u - v) * a.texcoord + u * b.texcoord + v * c.texcoord,
            nullptr,
            t
        );
    }

    boost::optional<Intersection> TriMesh::find_intersection(const Ray& r) const {
        float depth = std::numeric_limits<float>::infinity();
        boost::optional<Intersection> intersection;

        this->m_bvh.search(r, [&](auto& t) {
            auto new_intersection = this->find_intersection(r, t);

            if (new_intersection && new_intersection->distance() < depth) {
                depth = new_intersection->distance();
                intersection = new_intersection;
            }
        });

        return intersection;
    }

    class TriMeshLoader {
        std::vector<Vertex> m_vertices;
        std::vector<Triangle> m_triangles;

        std::map<std::tuple<unsigned int, unsigned int, unsigned int>, unsigned int> m_vertex_indices;

        std::vector<glm::vec3> m_pos;
        std::vector<glm::vec2> m_tex;
        std::vector<glm::vec3> m_norm;

        unsigned int add_vertex(unsigned int pos, unsigned int tex, unsigned int norm);
        unsigned int add_vertex(std::string spec);
    public:
        TriMeshLoader() {}
        TriMeshLoader(const TriMeshLoader& other) = delete;

        TriMeshLoader& operator =(const TriMeshLoader& other) = delete;

        void handle_line(std::string line);
        std::shared_ptr<TriMesh> finish();
    };

    unsigned int TriMeshLoader::add_vertex(unsigned int pos, unsigned int tex, unsigned int norm) {
        auto t = std::make_tuple(pos, tex, norm);
        auto existing_entry = this->m_vertex_indices.find(t);

        if (existing_entry != this->m_vertex_indices.end()) {
            return existing_entry->second;
        }

        if (pos >= this->m_pos.size() || tex >= this->m_tex.size() || norm >= this->m_norm.size()) {
            throw std::runtime_error("Face vertex index out of range");
        }

        this->m_vertices.push_back(Vertex {
            .pos = this->m_pos[pos],
            .normal = this->m_norm[norm],
            .texcoord = this->m_tex[tex]
        });
        this->m_vertex_indices.emplace(t, this->m_vertices.size() - 1);

        return this->m_vertices.size() - 1;
    }

    unsigned int TriMeshLoader::add_vertex(std::string spec) {
        std::vector<std::string> parts;

        boost::split(parts, spec, boost::is_any_of("/"));

        if (parts.size() != 3) {
            throw std::runtime_error(([&]() {
                std::ostringstream ss;

                ss << "Invalid face vertex specification \"" << spec << "\"";

                return ss.str();
            })());
        }

        int pos = std::stoi(parts[0]);
        if (pos < 0) {
            pos += this->m_pos.size() + 1;

            if (pos <= 0) {
                throw std::out_of_range("Negative index out of range");
            }
        } else if (pos == 0) {
            throw std::out_of_range("0 is not a valid index");
        }

        int tex = std::stoi(parts[1]);
        if (tex < 0) {
            tex += this->m_tex.size() + 1;

            if (tex <= 0) {
                throw std::out_of_range("Negative index out of range");
            }
        } else if (tex == 0) {
            throw std::out_of_range("0 is not a valid index");
        }

        int norm = std::stoi(parts[2]);
        if (norm < 0) {
            norm += this->m_norm.size() + 1;

            if (norm <= 0) {
                throw std::out_of_range("Negative index out of range");
            }
        } else if (norm == 0) {
            throw std::out_of_range("0 is not a valid index");
        }

        return this->add_vertex(
            static_cast<unsigned int>(pos - 1),
            static_cast<unsigned int>(tex - 1),
            static_cast<unsigned int>(norm - 1)
        );
    }

    static glm::vec3 parse_vec3(const std::vector<std::string>& parts, int start) {
        return glm::vec3(
            std::stof(parts[start]),
            std::stof(parts[start + 1]),
            std::stof(parts[start + 2])
        );
    }

    static glm::vec2 parse_vec2(const std::vector<std::string>& parts, int start) {
        return glm::vec2(
            std::stof(parts[start]),
            std::stof(parts[start + 1])
        );
    }

    void TriMeshLoader::handle_line(std::string line) {
        size_t comment_pos = line.find("#");

        if (comment_pos != std::string::npos) {
            line = line.substr(0, comment_pos);
        }

        boost::trim(line);

        if (line.size() == 0) {
            return;
        }

        std::vector<std::string> parts;

        boost::split(parts, line, boost::is_any_of(" \t"), boost::token_compress_on);

        if (parts[0] == "v") {
            if (parts.size() != 4) {
                throw std::runtime_error("Wrong number of arguments for \"v\"");
            }

            this->m_pos.push_back(parse_vec3(parts, 1));
        } else if (parts[0] == "vt") {
            if (parts.size() != 3) {
                throw std::runtime_error("Wrong number of arguments for \"vt\"");
            }

            auto v = parse_vec2(parts, 1);

            this->m_tex.push_back(glm::vec2(v.x, 1 - v.y));
        } else if (parts[0] == "vn") {
            if (parts.size() != 4) {
                throw std::runtime_error("Wrong number of arguments for \"vn\"");
            }

            this->m_norm.push_back(parse_vec3(parts, 1));
        } else if (parts[0] == "f") {
            if (parts.size() != 4) {
                throw std::runtime_error("Wrong number of arguments for \"f\"");
            }

            this->m_triangles.push_back(Triangle {
                .a = this->add_vertex(parts[1]),
                .b = this->add_vertex(parts[2]),
                .c = this->add_vertex(parts[3])
            });
        }
    }

    std::shared_ptr<TriMesh> TriMeshLoader::finish() {
        auto mesh = std::make_shared<TriMesh>(
            std::move(this->m_vertices),
            std::move(this->m_triangles)
        );

        this->m_vertex_indices.clear();
        this->m_pos.clear();
        this->m_tex.clear();
        this->m_norm.clear();

        return mesh;
    }

    std::shared_ptr<TriMesh> TriMesh::load_mesh(boost::filesystem::path path) {
        boost::filesystem::ifstream f(path);

        if (!f) {
            throw std::runtime_error(([&]() {
                std::ostringstream ss;

                ss << "Failed to open object file \"" << path.string() << "\"";

                return ss.str();
            })());
        }

        TriMeshLoader loader;
        std::string line;

        while (std::getline(f, line)) {
            loader.handle_line(line);
        }

        if (f.bad()) {
            throw std::runtime_error(([&]() {
                std::ostringstream ss;

                ss << "Error reading object file \"" << path.string() << "\"";

                return ss.str();
            })());
        }

        return loader.finish();
    }

    boost::optional<Intersection> TriMeshObject::find_intersection(const Ray& r) const {
        auto intersection = this->m_mesh->find_intersection(r);

        if (intersection) {
            intersection->material(this->material().get());
        }

        return intersection;
    }
}


#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/dense_lmi_constraint.h"
#include "conex/linear_constraint.h"
#include "conex/test/test_util.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using DenseMatrix = Eigen::MatrixXd;
#define TEST_OR_PROFILE 1
#if TEST_OR_PROFILE
int TestDiagonalSDP() {
  srand(1);
  int n = 5;
  int m = 2;
  SolverConfiguration config;
  config.inv_sqrt_mu_max = 25000;
  config.prepare_dual_variables = true;

  DenseMatrix affine2 = DenseMatrix::Identity(n, n);

  DenseMatrix Alinear = DenseMatrix::Random(n, m);
  DenseMatrix Clinear(n, 1);
  Clinear.setConstant(1);

  std::vector<DenseMatrix> constraints2;
  for (int i = 0; i < m; i++) {
    constraints2.push_back(Alinear.col(i).asDiagonal());
  }

  DenseLMIConstraint LMI{n, constraints2, affine2};
  LinearConstraint Linear{n, &Alinear, &Clinear};

  Program prog(m);
  Eigen::VectorXd b(m);
  prog.AddConstraint(LMI, {0, 1});
  b = GetFeasibleObjective(&prog);
  // b << 1.22, -.36;
  DenseMatrix y1(m, 1);
  Solve(b, prog, config, y1.data());

  Program prog2(m);
  prog2.AddConstraint(Linear, {0, 1});
  b = GetFeasibleObjective(&prog2);
  DenseMatrix y2(m, 1);
  Solve(b, prog2, config, y2.data());

  Program prog3(m);
  DenseMatrix y3(m, 1);
  prog3.AddConstraint(Linear);
  prog3.AddConstraint(Linear);
  Solve(b, prog3, config, y3.data());

  EXPECT_TRUE((y2 - y1).norm() < 1e-6);
  EXPECT_TRUE((y3 - y1).norm() < 1e-4);
  return 0;
}

TEST(SDP, DiagonalSDP) {
  for (int i = 0; i < 1; i++) {
    TestDiagonalSDP();
  }
}

TEST(SDP, SparseAndDenseAgree) {
  SolverConfiguration config;

  std::vector<int> variables_2{0, 2, 4, 6, 7, 8};
  std::vector<int> variables_1{1, 3, 5};

  int n1 = 5;
  int m1 = variables_1.size();

  int n2 = 5;
  int m2 = variables_2.size();

  int m = m1 + m2;

  vector<DenseMatrix> constraints_1 = GetRandomDenseMatrices(n1, m);
  vector<DenseMatrix> constraints_2 = GetRandomDenseMatrices(n2, m);
  vector<DenseMatrix> sparse_constraints_1;
  vector<DenseMatrix> sparse_constraints_2;

  for (const auto i : variables_1) {
    sparse_constraints_1.push_back(constraints_1.at(i));
    constraints_2.at(i).setZero();
  }

  for (const auto i : variables_2) {
    sparse_constraints_2.push_back(constraints_2.at(i));
    constraints_1.at(i).setZero();
  }

  DenseMatrix affine_1 = Eigen::MatrixXd::Identity(n1, n1);
  DenseMatrix affine_2 = Eigen::MatrixXd::Identity(n2, n2);
  DenseLMIConstraint LMI1{n1, constraints_1, affine_1};
  DenseLMIConstraint LMI2{n2, constraints_2, affine_2};

  Program prog(m);
  prog.AddConstraint(LMI1);
  prog.AddConstraint(LMI2);

  auto b = GetFeasibleObjective(&prog);

  DenseMatrix y(m1 + m2, 1);
  int success = Solve(b, prog, config, y.data());
  EXPECT_EQ(success, 1);

  SparseLMIConstraint sparse_LMI1{sparse_constraints_1, affine_1, variables_1};
  SparseLMIConstraint sparse_LMI2{sparse_constraints_2, affine_2, variables_2};

  Program sparse_prog(m);
  sparse_prog.AddConstraint(sparse_LMI1);
  sparse_prog.AddConstraint(sparse_LMI2);

  DenseMatrix y_sparse(m1 + m2, 1);
  success = Solve(b, sparse_prog, config, y_sparse.data());
  EXPECT_EQ(success, 1);

  EXPECT_NEAR((y - y_sparse).norm(), 0, 1e-8);
}
#else

int TestSDP(int i) {
  SolverConfiguration config;
  int n = 150;
  int m = 50;
  auto constraints2 = GetRandomDenseMatrices(n, m);

  DenseMatrix affine2 = Eigen::MatrixXd::Identity(n, n);
  DenseLMIConstraint LMI{n, constraints2, affine2};

  Program prog;
  DenseMatrix y(m, 1);
  prog.constraints.push_back(LMI);

  auto b = GetFeasibleObjective(m, prog.constraints);
  Solve(b, prog, config, y.data());
  return 0;

  DenseMatrix x(n, n);
  prog.constraints.at(0).get_dual_variable(x.data());
  x.array() /= prog.stats.sqrt_inv_mu[prog.stats.num_iter - 1];

  DenseMatrix slack = affine2;
  DenseMatrix res = b;
  for (int i = 0; i < m; i++) {
    slack -= constraints2.at(i) * y(i);
    res(i) -= (constraints2.at(i) * x).trace();
  }

  EXPECT_TRUE(conex::jordan_algebra::eig(slack).eigenvalues.minCoeff() > 1e-8);
  EXPECT_TRUE(res.norm() < 1e-7);
  EXPECT_TRUE((slack * x).trace() < 1e-4);

  return 0;
}

TEST(SDP, ProfileSDP) {
  for (int i = 0; i < 1; i++) {
    TestSDP(i);
  }
}
#endif

}  // namespace conex


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct thousand_ : boost::dispatch::constant_value_<thousand_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,thousand_,boost::dispatch::constant_value_<thousand_>);
      BOOST_SIMD_REGISTER_CONSTANT(1000, 0x447a0000UL, 0x408f400000000000ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, thousand_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::thousand_,thousand);
  }

  template<typename T> BOOST_FORCEINLINE auto Thousand()
  BOOST_NOEXCEPT_DECLTYPE(detail::thousand( boost::dispatch::as_<T>{}))
  {
    return detail::thousand( boost::dispatch::as_<T>{} );
  }
} }

#endif