#ifndef INCLUDE_SWIFT_VIO_BEARING_RESIDUALS_HPP_
#define INCLUDE_SWIFT_VIO_BEARING_RESIDUALS_HPP_
#include <Eigen/Core>
#include <memory>

#include <ceres/loss_function.h>
#include <ceres/sized_cost_function.h>

#include <swift_vio/ceres/corrector.h>
#include <swift_vio/DirectionFromParallaxAngleJacobian.hpp>
#include <swift_vio/VectorNormalizationJacobian.hpp>

namespace swift_vio {
/**
 * @brief orderedIndicesMAJ
 * @param maj indices of main anchor, associate anchor, and remaining frames.
 */
void orderedIndicesMAJ(const std::vector<size_t>& anchorIndices,
                       int totalObservations, std::vector<int>* maj);

class SimplePointSharedData {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SimplePointSharedData() {}

  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
      unitBearingList;
  std::vector<okvis::kinematics::Transformation,
              Eigen::aligned_allocator<okvis::kinematics::Transformation>>
      T_WC_list;
  std::vector<size_t>
      anchorIndices;  // Indices of main and associate anchors in the lists.
};

// e = \xi(N_{i,j}) - R_{WC(t_{i,j})} * f_{i, j} if m_i != j
// e = n_i - f_{i, m_i} otherwise
class BearingResiduals {
 public:
  typedef double Scalar;
  enum {
    NUM_RESIDUALS = Eigen::Dynamic,
    NUM_PARAMETERS = 6,
    NUM_LOCAL_PARAMETERS = 3,
  };
  /**
   * @brief BearingResiduals
   * @param pointDataPtr
   * @param huberEpsilon  0.01 ~ 2 sin(-.5\theta) ~ 5(pixels) : 500(focal length)
   */
  BearingResiduals(std::shared_ptr<const SimplePointSharedData> pointDataPtr,
                   double huberEpsilon)
      : pointDataPtr_(pointDataPtr),
        numResidualBlocks_(pointDataPtr->T_WC_list.size()),
        numResiduals_(pointDataPtr->T_WC_list.size() * 3),
        loss_function_(new ::ceres::HuberLoss(huberEpsilon)) {
    orderedIndicesMAJ(pointDataPtr->anchorIndices, numResidualBlocks_,
                      &majIndices_);
  }

  int NumResiduals() const { return numResiduals_; }

  void correct(Eigen::Vector3d* residual,
               Eigen::Matrix<double, 3, 3, Eigen::RowMajor>* jacobian) const {
    double squared_norm = residual->squaredNorm();
    double rho[3];
    loss_function_->Evaluate(squared_norm, rho);
    swift_vio::ceres::internal::Corrector correct(squared_norm, rho);
    // Correct the jacobians for the loss function.
    correct.CorrectJacobian(kResidualDim, kPapLocalDim, residual->data(),
                            jacobian->data());
    correct.CorrectResiduals(kResidualDim, residual->data());
  }

  /**
   * @brief operator ()
   * @param parameters
   * @param residuals
   * @param jacobian jacobian corresponds to the data of a COLUMN MAJOR Eigen
   * Matrix.
   * @return
   */
  bool operator()(const double* parameters, double* residuals,
                  double* jacobian) const;

 private:
  std::shared_ptr<const SimplePointSharedData> pointDataPtr_;
  int numResidualBlocks_;
  const int numResiduals_;  // Dim of the entire residual vector.
  std::vector<int> majIndices_;
  std::shared_ptr<::ceres::LossFunction> loss_function_;
  static const int kResidualDim = 3;
  static const int kPapLocalDim = 3;
};

}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_BEARING_RESIDUALS_HPP_


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

#include <boost/math/special_functions/beta.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;

//
// Force truncation to float precision of input values:
// we must ensure that the input values are exactly representable
// in whatever type we are testing, or the output values will all
// be thrown off:
//
float external_f;
float force_truncate(const float* f)
{
   external_f = *f;
   return external_f;
}

float truncate_to_float(mp_t r)
{
   float f = boost::math::tools::real_cast<float>(r);
   return force_truncate(&f);
}

boost::mt19937 rnd;
boost::uniform_real<float> ur_a(1.0F, 5.0F);
boost::variate_generator<boost::mt19937, boost::uniform_real<float> > gen(rnd, ur_a);
boost::uniform_real<float> ur_a2(0.0F, 100.0F);
boost::variate_generator<boost::mt19937, boost::uniform_real<float> > gen2(rnd, ur_a2);

struct ibeta_inv_data_generator
{
   boost::math::tuple<mp_t, mp_t, mp_t, mp_t, mp_t, mp_t, mp_t> operator()
      (mp_t bp, mp_t x_, mp_t p_)
   {
      float b = truncate_to_float(real_cast<float>(gen() * pow(mp_t(10), bp)));
      float x = truncate_to_float(real_cast<float>(x_));
      float p = truncate_to_float(real_cast<float>(p_));
      std::cout << b << " " << x << " " << p << std::flush;
      mp_t inv = boost::math::ibeta_inva(mp_t(b), mp_t(x), mp_t(p));
      std::cout << " " << inv << std::flush;
      mp_t invc = boost::math::ibetac_inva(mp_t(b), mp_t(x), mp_t(p));
      std::cout << " " << invc << std::endl;
      mp_t invb = boost::math::ibeta_invb(mp_t(b), mp_t(x), mp_t(p));
      std::cout << " " << invb << std::flush;
      mp_t invbc = boost::math::ibetac_invb(mp_t(b), mp_t(x), mp_t(p));
      std::cout << " " << invbc << std::endl;
      return boost::math::make_tuple(b, x, p, inv, invc, invb, invbc);
   }
};

int main(int argc, char*argv [])
{
   bool cont;
   std::string line;

   parameter_info<mp_t> arg1, arg2, arg3;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse incomplete beta function:\n"
      "  ibeta_inva(a, p) and ibetac_inva(a, q)\n\n";

   arg1 = make_periodic_param(mp_t(-5), mp_t(6), 11);
   arg2 = make_random_param(mp_t(0.0001), mp_t(1), 10);
   arg3 = make_random_param(mp_t(0.0001), mp_t(1), 10);

   arg1.type |= dummy_param;
   arg2.type |= dummy_param;
   arg3.type |= dummy_param;

   data.insert(ibeta_inv_data_generator(), arg1, arg2, arg3);

   line = "ibeta_inva_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "ibeta_inva_data");

   return 0;
}


#define _USE_MATH_DEFINES
#include <algorithm> // std::sort
#include <vector>

#include <Eigen/Dense>
#include <ccd/ccd.h>
#include <gtest/gtest.h>
#include <math.h>

#include "dart/collision/CollisionResult.hpp"
#include "dart/collision/dart/DARTCollide.hpp"
#include "dart/constraint/BoxedLcpConstraintSolver.hpp"
#include "dart/neural/RestorableSnapshot.hpp"
#include "dart/realtime/Ticker.hpp"
#include "dart/server/GUIWebsocketServer.hpp"
#include "dart/utils/DartResourceRetriever.hpp"
#include "dart/utils/sdf/sdf.hpp"
#include "dart/utils/urdf/urdf.hpp"

#include "GradientTestUtils.hpp"
#include "TestHelpers.hpp"

using namespace dart;
using namespace realtime;

// #define ALL_TESTS

#ifdef ALL_TESTS
TEST(DARTCollide, ATLAS_5_HIP_FOOT)
{
  // Create a world
  std::shared_ptr<simulation::World> world = simulation::World::create();

  // Set gravity of the world
  world->setPenetrationCorrectionEnabled(false);
  world->setGravity(Eigen::Vector3s(0.0, -9.81, 0));

  // Set up the LCP solver to be super super accurate, so our
  // finite-differencing tests don't fail due to LCP errors. This isn't
  // necessary during a real forward pass, but is helpful to make the
  // mathematical invarients in the tests more reliable.
  static_cast<constraint::BoxedLcpConstraintSolver*>(
      world->getConstraintSolver())
      ->makeHyperAccurateAndVerySlow();

  // Load the meshes

  auto retriever = std::make_shared<utils::CompositeResourceRetriever>();
  retriever->addSchemaRetriever(
      "file", std::make_shared<common::LocalResourceRetriever>());
  retriever->addSchemaRetriever("dart", utils::DartResourceRetriever::create());
  std::string leftFootPath = "dart://sample/sdf/atlas/l_foot.dae";
  dynamics::ShapePtr leftFootMesh = std::make_shared<dynamics::MeshShape>(
      Eigen::Vector3s::Ones(),
      dynamics::MeshShape::loadMesh(leftFootPath, retriever),
      leftFootPath,
      retriever);
  std::string rightFootPath = "dart://sample/sdf/atlas/r_foot.dae";
  dynamics::ShapePtr rightFootMesh = std::make_shared<dynamics::MeshShape>(
      Eigen::Vector3s::Ones(),
      dynamics::MeshShape::loadMesh(rightFootPath, retriever),
      rightFootPath,
      retriever);
  std::shared_ptr<BoxShape> rootShape(
      new BoxShape(Eigen::Vector3s::Ones() * 0.5));

  // Set up matrices

  Eigen::Matrix4d l_leg_hpx_WorldTransform;
  // clang-format off
  l_leg_hpx_WorldTransform << 1,           0,           0,           0,
                              0, 1.11022e-16,           1,       -0.01,
                              0,          -1, 1.11022e-16,      -0.089,
                              0,           0,           0,           1;
  // clang-format on
  Eigen::Vector3s l_leg_hpx_Axis = Eigen::Vector3s::UnitX();
  Eigen::Matrix4d r_leg_hpx_WorldTransform;
  // clang-format off
  r_leg_hpx_WorldTransform << 1,           0,           0,           0,
                              0, 1.11022e-16,           1,       -0.01,
                              0,          -1, 1.11022e-16,       0.089,
                              0,           0,           0,           1;
  // clang-format on
  Eigen::Vector3s r_leg_hpx_Axis = Eigen::Vector3s::UnitX();
  Eigen::Matrix4d l_foot_WorldTransform;
  // clang-format off
  l_foot_WorldTransform << 1,           0,           0,           0,
                           0, 1.11022e-16,           1,      -0.856,
                           0,          -1, 1.11022e-16,      -0.089,
                           0,           0,           0,           1;
  // clang-format on
  Eigen::Matrix4d r_foot_WorldTransform;
  // clang-format off
  r_foot_WorldTransform << 1,           0,           0,           0,
                           0, 1.11022e-16,           1,      -0.856,
                           0,          -1, 1.11022e-16,       0.089,
                           0,           0,           0,           1;
  // clang-format on

  std::shared_ptr<dynamics::Skeleton> singleJointAtlas
      = dynamics::Skeleton::create();

  Eigen::Isometry3s leftHipPos = Eigen::Isometry3s(l_leg_hpx_WorldTransform);
  Eigen::Isometry3s rightHipPos = Eigen::Isometry3s(r_leg_hpx_WorldTransform);
  Eigen::Isometry3s leftFootPos = Eigen::Isometry3s(l_foot_WorldTransform);
  Eigen::Isometry3s rightFootPos = Eigen::Isometry3s(r_foot_WorldTransform);
  Eigen::Isometry3s leftHipToLeftFoot = leftHipPos.inverse() * leftFootPos;
  Eigen::Isometry3s rightHipToRightFoot = rightHipPos.inverse() * rightFootPos;

  // Create root box

  std::pair<dynamics::FreeJoint*, dynamics::BodyNode*> rootPair
      = singleJointAtlas->createJointAndBodyNodePair<dynamics::FreeJoint>();
  dynamics::FreeJoint* rootJoint = rootPair.first;
  dynamics::BodyNode* rootBody = rootPair.second;
  rootBody->createShapeNodeWith<VisualAspect, CollisionAspect>(rootShape);

  // Create left leg

  std::pair<dynamics::RevoluteJoint*, dynamics::BodyNode*> leftHipPair
      = rootBody->createChildJointAndBodyNodePair<dynamics::RevoluteJoint>();
  dynamics::RevoluteJoint* leftHip = leftHipPair.first;
  leftHip->setTransformFromParentBodyNode(leftHipPos);
  leftHip->setTransformFromChildBodyNode(leftHipToLeftFoot.inverse());
  leftHip->setAxis(l_leg_hpx_Axis);
  dynamics::BodyNode* leftFoot = leftHipPair.second;
  leftFoot->createShapeNodeWith<VisualAspect, CollisionAspect>(leftFootMesh);
  assert(leftFoot->getWorldTransform().matrix() == leftFootPos.matrix());

  // Create right leg

  std::pair<dynamics::RevoluteJoint*, dynamics::BodyNode*> rightHipPair
      = rootBody->createChildJointAndBodyNodePair<dynamics::RevoluteJoint>();
  dynamics::RevoluteJoint* rightHip = rightHipPair.first;
  rightHip->setTransformFromParentBodyNode(rightHipPos);
  rightHip->setTransformFromChildBodyNode(rightHipToRightFoot.inverse());
  rightHip->setAxis(r_leg_hpx_Axis);
  dynamics::BodyNode* rightFoot = rightHipPair.second;
  rightFoot->createShapeNodeWith<VisualAspect, CollisionAspect>(rightFootMesh);
  assert(rightFoot->getWorldTransform().matrix() == rightFootPos.matrix());

  world->addSkeleton(singleJointAtlas);

  // Load ground
  dart::utils::DartLoader urdfLoader;
  std::shared_ptr<dynamics::Skeleton> ground
      = urdfLoader.parseSkeleton("dart://sample/sdf/atlas/ground.urdf");
  world->addSkeleton(ground);
  // Disable the ground from casting its own shadows
  ground->getBodyNode(0)->getShapeNode(0)->getVisualAspect()->setCastShadows(
      false);

  // Set up the world

  // give velocity parallel to the ground plane, to move friction away from
  // non-differentiable points
  // singleJointAtlas->setVelocity(0, 0.01);
  // singleJointAtlas->setVelocity(2, 0.01);

  world->step();
  Eigen::VectorXs worldVel = world->getVelocities();

  // Run the tests

  // EXPECT_TRUE(verifyAnalyticalJacobians(world));
  // std::cout << "Passed Analytical Jacobians" << std::endl;
  // EXPECT_TRUE(verifyScratch(world, WithRespectTo::FORCE));
  // EXPECT_TRUE(verifyF_c(world));
  // std::cout << "Passed F_c" << std::endl;
  EXPECT_TRUE(verifyVelGradients(world, worldVel));
  std::cout << "Passed Vel Gradients" << std::endl;
  EXPECT_TRUE(verifyPosGradients(world, 1, 1e-8));
  std::cout << "Passed Pos Gradients" << std::endl;
  EXPECT_TRUE(verifyWrtMass(world));
  std::cout << "Passed Mass Gradients" << std::endl;

  /*

  // Display everything out on a GUI

  server::GUIWebsocketServer server;
  server.renderWorld(world);
  server.serve(8070);

  Ticker ticker(0.01);
  ticker.registerTickListener([&](long time) {
    s_t diff = sin(((s_t)time / 2000));
    singleJointAtlas->setPosition(6, diff);
    singleJointAtlas->setPosition(7, -diff);
    // world->step();
    server.renderWorld(world);
  });
  server.registerConnectionListener([&]() { ticker.start(); });

  while (server.isServing())
  {
  }
  */
}
#endif

void testAtlas(bool withGroundContact)
{
  // Create a world
  std::shared_ptr<simulation::World> world = simulation::World::create();

  // Set gravity of the world
  world->setPenetrationCorrectionEnabled(false);
  world->setGravity(Eigen::Vector3s(0.0, -9.81, 0));

  // Set up the LCP solver to be super super accurate, so our
  // finite-differencing tests don't fail due to LCP errors. This isn't
  // necessary during a real forward pass, but is helpful to make the
  // mathematical invarients in the tests more reliable.
  static_cast<constraint::BoxedLcpConstraintSolver*>(
      world->getConstraintSolver())
      ->makeHyperAccurateAndVerySlow();

  // Load ground and Atlas robot and add them to the world
  dart::utils::DartLoader urdfLoader;

  std::shared_ptr<dynamics::Skeleton> atlas
      = dart::utils::SdfParser::readSkeleton(
          "dart://sample/sdf/atlas/atlas_v3_no_head.sdf");
  world->addSkeleton(atlas);

  if (withGroundContact)
  {
    std::shared_ptr<dynamics::Skeleton> ground
        = urdfLoader.parseSkeleton("dart://sample/sdf/atlas/ground.urdf");
    world->addSkeleton(ground);
  }

  // Set initial configuration for Atlas robot
  atlas->setPosition(0, -0.5 * dart::math::constantsd::pi());
  atlas->setPosition(4, -0.01);
  Eigen::VectorXs originalPos = atlas->getPositions();
  Eigen::VectorXs worldVel = world->getVelocities();

  atlas->setVelocities(Eigen::VectorXs::Zero(atlas->getNumDofs()));
  // Give a tiny bit of lateral motion, to keep the friction away from
  // non-differentiable points that can have lots of valid gradients
  // atlas->setVelocity(3, 0.01);
  // atlas->setVelocity(5, 0.01);

  /*
  world->step();
  auto& result = world->getLastCollisionResult();
  for (int i = 0; i < result.getNumContacts(); i++)
  {
    std::cout << "Depth[" << i << "]: " << result.getContact(i).penetrationDepth
              << std::endl;
  }
  return;
  */

  // EXPECT_TRUE(verifyAnalyticalJacobians(world));

  // EXPECT_TRUE(verifyPosVelJacobian(world, worldVel));

  EXPECT_TRUE(verifyF_c(world));
  std::cout << "Passed F_c" << std::endl;
  EXPECT_TRUE(verifyVelGradients(world, worldVel));
  std::cout << "Passed vel" << std::endl;
  EXPECT_TRUE(verifyPosGradients(world, 1, 1e-8));
  std::cout << "Passed pos" << std::endl;
  EXPECT_TRUE(verifyWrtMass(world));
  std::cout << "Passed mass" << std::endl;
  std::cout << "Passed everything except backprop" << std::endl;
  // This is outrageously slow
  // EXPECT_TRUE(verifyAnalyticalBackprop(world));
}

#ifdef ALL_TESTS
TEST(GRADIENTS, ATLAS_FLOATING)
{
  testAtlas(false);
}
#endif

// #ifdef ALL_TESTS
TEST(GRADIENTS, ATLAS_GROUND)
{
  testAtlas(true);
}
// #endif


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

#include <boost/simd/arithmetic/functions/fast_rec.hpp>
#include <boost/simd/include/functions/scalar/refine_rec.hpp>
#include <boost/simd/include/functions/scalar/raw_rec.hpp>
#include <boost/simd/sdk/config.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_rec_
                                    , boost::simd::tag::sse_
                                    , (A0)
                                    , (scalar_< single_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 a0) const
    {
      return refine_rec(a0, raw_rec( a0 ));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( fast_rec_
                                    , boost::simd::tag::sse_
                                    , (A0)
                                    , (scalar_< double_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 a0) const
    {
      float x  = a0;
      double e = static_cast<float>(raw_rec( x ));
             e = refine_rec(a0, e);

      return refine_rec(a0, e);
    }
  };
} } }

#endif
#endif


#include "NTinjection.h"
#include "write.h"
#include "messages.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "absorption.h"
#include <finjection/ppPionInj.h>
#include <finjection/pgammaPionInj.h>
#include <finjection/muonInj.h>
#include <finjection/neutrinoInj.h>
#include <flosses/lossesHadronics.h>
#include <flosses/lossesSyn.h>
#include <finjection/pairInjectionExact.h>
#include <finjection/pairgg.h>
#include <finjection/pairMuonDecay.h>
#include <finjection/pairBH.h>
#include <gsl/gsl_sf_bessel.h>
#include "losses.h"
#include <flosses/nonThermalLosses.h>

#include <fparameters/parameters.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/Dimension.h>
#include <fmath/fbisection.h>
#include <fmath/RungeKutta.h>
#include <fmath/physics.h>
#include <fmath/tridiagonal.h>
#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <fstream>

#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>

double eEmax(Particle& p, double r, double B, double v, double dens, int jR)
{
	double accE = GlobalConfig.get<double>("nonThermal.injection.PL.accEfficiency");
	double dr = r*(paso_r-1.0);
	double h = height_fun(r);
	double sumAdvTime = 0.0;
	for (int jj=nR-1;jj>=jR;jj--) {
		double rAux = p.ps[DIM_R][jj];
		double Baux = magneticField(rAux);
		double drAux = rAux*(sqrt(paso_r)-1.0/sqrt(paso_r));
		double tCellAux = drAux / abs(radialVel(rAux));
		sumAdvTime += Baux * tCellAux;
	}

    double Emax_adv = accE*cLight*electronCharge*sumAdvTime;
	Emax_adv = accE*cLight*electronCharge*B*accretionTime(r);
    double Emax_syn = p.mass*cLight2*sqrt(accE*6.0*pi*electronCharge / (thomson*B)) * p.mass/electronMass;
    double Emax_Hillas = electronCharge*B*h;
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double Emax_diff = pow(h*electronCharge*B,(q-3)/(2*q-5)) * pow(p.mass*cLight2,(q-2)/(2*q-5)) /
							pow(9*zeda*accE,1.0/(2*q-5));
	if (p.id == "ntProton") {
		double sigmapp = 34.3e-27;
		double Emax_pp = accE*electronCharge*B/(0.5*dens*sigmapp);
		return min(min(Emax_adv,Emax_pp),min(Emax_Hillas,Emax_syn));
	} else
		return min(min(min(Emax_syn,Emax_adv),Emax_Hillas),Emax_diff);
}

double eEmax_numerical(Particle& p, double r, double B, double v, double dens, int jR, State& st,
						SpaceCoord i)
{
	double accE = GlobalConfig.get<double>("nonThermal.injection.PL.accEfficiency");
	double dr = r*(1.0-1.0/paso_r);
	double h = height_fun(r);
	double rho = massDensityADAF(r);
	double vA = B / sqrt(4*pi*rho);

	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double sumAdvTime = (accMethod == 0) ?
	  integSimpsonLog(r,p.ps[DIM_R][nR-1],[&p,zeda,q] (double rAux)
		{
			double Baux = magneticField(rAux);
			double vR = radialVel(rAux);
			return Baux/abs(vR);
		},50)*accE*electronCharge*cLight
	: integSimpsonLog(r,p.ps[DIM_R][nR-1],[&p,zeda,q] (double rAux)
		{
			double Baux = magneticField(rAux);
			double height_aux = height_fun(rAux);
			double rhoAux = massDensityADAF(rAux);
			double vAlfv = Baux / sqrt(4.0*pi*rhoAux);
			double rL = p.mass*cLight2/(electronCharge*Baux);
			double kMin = 1.0/height_aux;
			double factor = zeda * (cLight*kMin) * P2(vAlfv/cLight) * pow(rL*kMin,q-2.0);
			return factor / abs(radialVel(rAux));
		},50);
	/*
	B = magneticField(r);
	double height = height_fun(r);
	double rL = p.mass*cLight2/(electronCharge*B);
	double kMin = 1.0/height;
	double factor = zeda * (cLight*kMin) * P2(vA/cLight) * pow(rL*kMin,q-2.0);
	sumAdvTime = factor / abs(radialVel(r)) * r; */
	double rL = p.mass*cLight2/(electronCharge*B);
	double gamma_min = 2.0*p.emin() / (p.mass*cLight2);
	double Emax_adv = (accMethod == 0) ? sumAdvTime :
				p.mass*cLight2 * (pow((4.0-q*q)*sumAdvTime + pow(gamma_min,2.0-q),1.0/(2.0-q)));
	double Emax_Hillas = electronCharge*B*h * (vA/cLight);
	Emax_adv = accE*electronCharge*cLight*B*accretionTime(r);
	fun1 rate_cool_acc = [&p,&st,&i,accE,B,h,rho,r] (double E)
						{
							double rate_cool = losses(E,p,st,i)/E;
							double rate_diff = 1.0/diffusionTimeTurbulence(E,h,p,B);
							double rate_acc = (accMethod == 0) ? accelerationRate(E,B) :
												accelerationRateSDA(E,p,B,h,rho);
							return rate_cool + rate_diff - rate_acc;
						};
	double Emax_cool = 10*p.emin()*cLight2;
	Bisect(rate_cool_acc,2.0*p.emin(),p.emax()/2.0,Emax_cool);
	return min(Emax_cool, min(Emax_Hillas,Emax_adv));
}

double cutOffPL(double E, double Emin, double Emax)
{
	return pow(E,-pIndex)*exp(-E/Emax)*exp(-Emin/E);
}

double SDA_PL(double E, double Emin, double Emax)
{
	return pow(E,0.5)*exp(-pow(27*E/Emax,1.0/3.0))*exp(-Emin/E);
}

double SDA_exact(double E, double Emin, double Emax, Particle& p, double r)
{
	double gmax2 = Emax / (p.mass*cLight2);
	double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
	double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
	double B = magneticField(r);
	double height = height_fun(r);
	double vA = B / sqrt(4.0*pi*massDensityADAF(r));
	double rL = p.mass*cLight2 / (electronCharge*B);
	double theta = pow(rL/height,4.0-2.0*q) / P2(3*vA/cLight * zeda);
	double g = E / (p.mass*cLight2);
	double ginj = Emin / (p.mass*cLight2);
	double gmin = min(ginj,g);
	double gmax = max(ginj,g);
	double beta = 3.0/(2.0-q);
	double arg1 = sqrt(theta)*pow(gmin,2.0-q)/(2.0-q);
	double arg2 = sqrt(theta)*pow(gmax,2.0-q)/(2.0-q);
	double Ifun = gsl_sf_bessel_Inu((beta-1.0)/2.0,arg1);
	double Kfun = (arg2 < 500.0) ? gsl_sf_bessel_Knu((beta-1.0)/2.0,arg2) : exp(-arg2) * sqrt(pi/(2.0*arg2));
	double gmax1 = pow(theta,-0.5/(2.0-q));
	return sqrt(g/ginj)*pow(g*ginj,(2.0-q)/2.0)*Ifun*Kfun;
	//* (gmax2 < gmax1 && g > gmax2 ? exp(3) * exp(-pow(27*g/gmax2,1.0/3.0)) : 1.0);
}

double auxFun(double g, double gammaMax)
{
	if (pIndex < 2.0) return pow(gammaMax,2.0-pIndex)*pow(g,pIndex+1.0);
	else if (pIndex > 2.0) return g*g*g;
	else return log(gammaMax/g)*g*g;
}
double func(double g, double LHS, double norm_temp, double gammaMax)
{
	return LHS - auxFun(g,gammaMax)*sqrt(g*g-1.0)*exp(-g/norm_temp);
}

double findGammaMin(double temp, double Emax)
{
	double norm_temp = boltzmann*temp / (electronMass*cLight2);
	double aTheta = 3.0 - 6.0/(4.0+5.0*norm_temp); // Gammie & Popham (1998)
	double gammaMax = (pIndex <= 2.0) ? Emax / (electronMass*cLight2) : 1.0;;
	double g1,g2;
	g2 = 100.0;
	double LHS = etaInj*norm_temp*norm_temp*aTheta*gsl_sf_bessel_Kn(2,1.0/norm_temp)*
					(pIndex != 2 ? abs(pIndex-2.0) : 1.0);
	double RHS2 = auxFun(g2,gammaMax)*sqrt(g2*g2-1.0)*exp(-g2/norm_temp);
	while (LHS < RHS2) {
		g2 *= 2.0;
		RHS2 = auxFun(g2,gammaMax)*sqrt(g2*g2-1.0)*exp(-g2/norm_temp);
	}
	g1 = g2/1.1;
	double RHS1 = auxFun(g1,gammaMax)*sqrt(g1*g1-1.0)*exp(-g1/norm_temp);
	while ((LHS-RHS2)*(LHS-RHS1) > 0.0) {
		g1 /= 1.1;
		RHS1 = auxFun(g1,gammaMax)*sqrt(g1*g1-1.0)*exp(-g1/norm_temp);
	}

	double g = g1;
	double tol = 1.0e-3;
	while ((g2-g1) >= tol) {
		g = (g1+g2)/2;
		if (func(g,LHS,norm_temp,gammaMax) == 0.0)
			return g;
		else if (func(g,LHS,norm_temp,gammaMax)*func(g1,LHS,norm_temp,gammaMax) < 0.0)
			g2 = g;
		else
			g1 = g;
    }
	return 0.5*(g2+g1);
}

void injection(Particle& p, State& st)
{
	pIndex = GlobalConfig.get<double>("nonThermal.injection.primaryIndex");
    double sumQ = 0.0;

	if (p.id == "ntProton")
		etaInj = GlobalConfig.get<double>("nonThermal.injection.energyFraction_i");
	else if (p.id == "ntElectron")
		etaInj = GlobalConfig.get<double>("nonThermal.injection.energyFraction_e");

	std::ofstream file;
	if (p.id == "ntElectron")
		file.open("gammaMin.txt",ios::out);

	// CALCULATION OF THE NORMALIZATION FACTOR
	double sum = 0.0;
	Vector Qfactor(nR,1.0);
	p.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double vol = volume(r);
		double norm_temp, dens;
		if (p.id == "ntElectron") {
			norm_temp = boltzmann*st.tempElectrons.get(iR)/(p.mass*cLight2);
			dens = st.denf_e.get(iR);
		} else if (p.id == "ntProton") {
			norm_temp = boltzmann*st.tempIons.get(iR)/(p.mass*cLight2);
			dens = st.denf_i.get(iR);
		}
		double aTheta = 3.0 - 6.0/(4.0+5.0*norm_temp); // Gammie & Popham (1998)
		double uth = aTheta * dens * p.mass*cLight2 * norm_temp;
		double vA = st.magf.get(iR)/sqrt(4.0*pi*massDensityADAF(r));
		double h = height_fun(r);
		Qfactor[iR.coord[DIM_R]] = (accMethod == 0 ? (r/schwRadius < 1000.0 ? uth * st.magf.get(iR) : 0.0) : //	 * abs(radialVel(r)) / r :
										dens * p.mass*cLight2 * cLight/schwRadius * P2(vA/cLight) );
		sum += vol * Qfactor[iR.coord[DIM_R]];
	},{0,-1,0});
	Ainjection = etaInj*accRateOut*cLight2 / sum;		// [non-dimensional]

////////////////////////////////////////////////////////

	int jR = 0;
	p.ps.iterate([&](const SpaceIterator& iR) {

		const double r = iR.val(DIM_R);
		double rB1 = r/sqrt(paso_r);
		double rB2 = rB1*paso_r;
		const double vol = volume(r);

		double zeda = GlobalConfig.get<double>("nonThermal.injection.SDA.fractionTurbulent");
		double q = GlobalConfig.get<double>("nonThermal.injection.SDA.powerSpectrumIndex");
		double B = magneticField(r);
		double height = height_fun(r);
		double vA = B / sqrt(4.0*pi*massDensityADAF(r));
		double rL = p.mass*cLight2 / (electronCharge*B);
		double theta = pow(rL/height,4.0-2.0*q) / P2(3*vA/cLight * zeda);

		double gammaMin = 2.0;
		double Emax = eEmax_numerical(p,r,st.magf.get(iR),-radialVel(r),st.denf_i.get(iR),jR,st,iR.coord);
		double Emin = gammaMin*p.mass*cLight2;
		if (p.id == "ntElectron") {
			double temp = st.tempElectrons.get(iR);
			gammaMin = findGammaMin(temp,Emax);
			Emin = gammaMin*electronRestEnergy;
			file << r/schwRadius << "\t" << gammaMin << endl;
		}
		double int_E = integSimpsonLog(p.emin(),p.emax(),[Emin,Emax,r,&p](double e)
				{
					if (accMethod == 0)
						return e*cutOffPL(e,Emin,Emax);
					else
						return e*SDA_exact(e,Emin,Emax,p,r);
				},100);

		double Q0p = Ainjection * Qfactor[iR.coord[DIM_R]] / int_E;

		p.ps.iterate([&](const SpaceIterator& iRE) {
			const double E = iRE.val(DIM_E);
			double total = 0.0;
			if (accMethod == 0)
				total = cutOffPL(E,Emin,Emax)*Q0p;
			else
				total = SDA_exact(E,Emin,Emax,p,r)*Q0p;
			p.injection.set(iRE,total); //en unidades de erg^-1 s^-1 cm^-3
		},{-1,iR.coord[DIM_R],0});
		jR++;
	},{0,-1,0});
	file.close();
	cout << "etaInj Mdot c^2 = " << etaInj*accRateOut*cLight2 << endl;

	writeEandRParamSpace(p.id + "Injection_vol", p.injection, 0, 1);
	writeEandRParamSpace(p.id + "Injection", p.injection, 0, 0);
	writeRParamSpace(p.id + "Injection_R", p.injection, 0, 0);
}

void injectionChargedPion(Particle& p, State& st)
{
	show_message(msgStart,Module_pionInjection);

	double sumTotp = 0.0;
	double sumTotpp = 0.0;
	double sumTot = 0.0;
	p.ps.iterate([&](const SpaceIterator& iR) {
		p.ps.iterate([&](const SpaceIterator& iRE) {
			double Epi = iRE.val(DIM_E);
			double injection = ppPionInj(Epi,st.ntProton,st.denf_i.get(iRE),iRE);
			injection += pgammaPionInj(Epi,st.ntProton,st.photon.distribution,iRE,
										st.photon.emin(),st.photon.emax());
			p.injection.set(iRE,injection);
			double tdecay = chargedPionMeanLife*(Epi/(chargedPionMass*cLight2));
			double ratecool = lossesHadronics(Epi,st.denf_i.get(iR),p)/Epi;
			//p.distribution.set(iRE,injection*pow(ratecool+pow(tdecay,-1),-1));
		},{-1,iR.coord[DIM_R],0});
		sumTotp += integSimpsonLog(st.ntProton.emin(),st.ntProton.emax(),[&](double ep)
					{
						return ep*st.ntProton.injection.interpolate({{DIM_E,ep}},&iR.coord);
					},100)*volume(iR.val(DIM_R));
		sumTotpp += integSimpsonLog(st.ntProton.emin(),st.ntProton.emax(),[&](double ep)
					{
						return ep*st.ntProton.distribution.interpolate({{DIM_E,ep}},&iR.coord)
						* lossesHadronics(ep,st.denf_i.get(iR),st.ntProton)/ep ;
					},100)*volume(iR.val(DIM_R));
		sumTot += integSimpsonLog(p.emin(),p.emax(),[&](double e)
					{
						return e*p.injection.interpolate({{DIM_E,e}},&iR.coord);
					},100)*volume(iR.val(DIM_R));
	},{0,-1,0});
	cout << "Total power injected in " << st.ntProton.id << " = " << sumTotp << "\t" << endl;
	cout << "Total power in pp interactions" << " = " << sumTotpp << "\t" << endl;
	cout << "Total power injected in " << p.id << " = " << sumTot << "\t" << endl;
	show_message(msgEnd,Module_pionInjection);
}

void injectionMuon(Particle& p, State& st)
{
    double sumTot = 0.0;
	show_message(msgStart, Module_muonInjection);
	p.ps.iterate([&](const SpaceIterator& iR) {
		p.ps.iterate([&](const SpaceIterator& iRE) {
			const double Emu = iRE.val(DIM_E);
			double injection = muonInjNew(Emu,p,st.ntChargedPion,iRE);
			p.injection.set(iRE,injection);
			double ratecool = lossesSyn(Emu,st.magf.get(iR),p)/Emu;
			double tdecay = muonMeanLife*(Emu/(muonMass*cLight2));
			//p.distribution.set(iRE,injection*pow(ratecool+pow(tdecay,-1),-1));
		},{-1,iR.coord[DIM_R],0});

		sumTot += integSimpson(log(p.emin()),log(p.emax()),[&](double loge)
					{
						double e = exp(loge);
						return e*e*p.injection.interpolate({{DIM_E,e}},&iR.coord);
					},100)*volume(iR.val(DIM_R));

	},{0,-1,0});
	cout << "Total power injected in " << p.id << " = " << sumTot << "\t" << endl;
	writeEandRParamSpace("muonInjection",p.injection,0,1);
	show_message(msgEnd, Module_muonInjection);
}

void injectionPair(Particle& p, State& st, int it)
{
	show_message(msgStart, Module_pairInjection);

	std::ofstream secondariesMuonFile, secondariesBHFile, secondariesGammaGammaFile;

	if (it == 1) {
		secondariesMuonFile.open("secondariesInjectionMuon.dat", std::ios::out);
		secondariesBHFile.open("secondariesInjectionBH.dat", std::ios::out);
		secondariesGammaGammaFile.open("secondariesInjectionGammaGamma.dat", std::ios::out);
	}

	//p.ps.iterate([&](const SpaceIterator& iR) {
	double sumTot = 0.0;
	double sumTotPh = 0.0;
	#pragma omp parallel for
	for (size_t jR=0;jR<nR;jR++) {
		SpaceCoord iR = {0,jR,0};
		double r = st.denf_e.ps[DIM_R][jR];
		//double r = iR.val(DIM_R);
		double vol = volume(r);
		double height = height_fun(r);
		p.ps.iterate([&](const SpaceIterator& iRE) {
			double Eee = iRE.val(DIM_E);
			if (it == 1) {
				double injectionMuon = pairMuonDecayNew(Eee,st.ntMuon,iR);
				double injectionBH = pairBH(Eee,st.ntProton,st.photon.distribution,iR,
								st.photon.emin(),st.photon.emax());
				p.injection.set(iRE, injectionMuon + injectionBH);
				secondariesMuonFile << safeLog10(Eee/EV_TO_ERG) << "\t" << iRE.coord[DIM_R] << "\t"
								<< r/schwRadius << "\t" << injectionMuon*vol << endl;
				secondariesBHFile << safeLog10(Eee/EV_TO_ERG) << "\t" << iRE.coord[DIM_R] << "\t"
								<< r/schwRadius << "\t" << injectionBH*vol << endl;
			}
			double injectionGammaGamma = pairGammaGammaNew(Eee,st.ntPhoton.distribution,st.photon.distribution,
						iRE,st.photon.emin(),st.photon.emax(),st.ntPhoton.emin(),st.ntPhoton.emax());

			//double tcool = Eee/lossesSyn(Eee,st.magf.get(iR),p);
			p.injection.set(iRE,p.injection.get(iRE)+injectionGammaGamma);
			secondariesGammaGammaFile << safeLog10(Eee/EV_TO_ERG) << "\t" << vol << "\t"
								<< r/schwRadius << "\t" << injectionGammaGamma << endl;
		},{-1,jR,0});
		sumTot += integSimpsonLog(p.emin(),p.emax(),[&st,&p,iR](double e)
					{
						//double tcool = e/lossesSyn(e,st.magf.get(iR),p);
						double inj = p.injection.interpolate({{DIM_E,e}},&iR);
						return e*inj;
					},100)*volume(r);
		sumTotPh += integSimpsonLog(st.ntPhoton.emin(),st.ntPhoton.emax(),
					[&st,&iR,height](double Eg)
					{
						double ntPh = st.ntPhoton.distribution.interpolate({{DIM_E,Eg}},&iR);
						double kappa_gg = 2.0/sqrt(pi)*st.tau_gg.interpolate({{DIM_E,Eg}},&iR)/height;
						double rategg = cLight*kappa_gg;
						return Eg*ntPh*rategg;
					},100)*volume(r);
	};
	cout << "Total photon power absorbed to create pairs = " << sumTotPh << "\t" << endl;
	cout << "Total power injected in " << p.id << " = " << sumTot << "\t" << endl;
	writeEandRParamSpace("secondaryPairInjection",p.injection,0,1);

	if (it == 1) {
		secondariesMuonFile.close();
		secondariesBHFile.close();
	}
	secondariesGammaGammaFile.close();

	show_message(msgEnd, Module_pairInjection);
}

void injectionNeutrino(Particle& nu, State& st)
{
	show_message(msgStart,Module_muonInjection);

	double sumTot = 0.0;
	nu.ps.iterate([&](const SpaceIterator& iR) {
		nu.ps.iterate([&](const SpaceIterator& iRE) {
			const double Enu = iRE.val(DIM_E);
			double injection1 = muonNeutrinoInjection(Enu,nu,st.ntMuon,st.ntChargedPion,iRE);
			double injection2 = electronNeutrinoInjection(Enu,nu,st.ntMuon,iRE);
			nu.injection.set(iRE,injection1+injection2);
		},{-1,iR.coord[DIM_R],0});
		sumTot += integSimpsonLog(nu.emin(),nu.emax(),[&](double e)
					{
						return e*nu.injection.interpolate({{DIM_E,e}},&iR.coord);
					},100)*volume(iR.val(DIM_R));

	},{0,-1,0});
	cout << "Total power injected in " << nu.id << " = " << sumTot << "\t" << endl;
	writeEandRParamSpace("neutrinoInjection", nu.injection, 0, 1);

	show_message(msgEnd, Module_neutrinoInjection);
}

void injectionFokkerPlanckOneZone(Particle& p, State& st)
// This routine solves the transport equation by considering separated one zones at each
// radius in the RIAF. The escape rate is the sum of the accretion rate (1/tacc) and the
// diffusion rate. It considers stochastic acceleration by turbulence at each zone, where
// the steady transport equation is an ordinary Fokker-Plack diff. equation in the energy.
// It is solved by finite differences (CC70, PP96).
// Problems: It does not treat re-acceleration.
{

	// CALCULATION OF THE NORMALIZATION FACTOR
	if (p.id == "ntProton")
		etaInj = GlobalConfig.get<double>("nonThermal.injection.energyFraction_i");
	else if (p.id == "ntElectron")
		etaInj = GlobalConfig.get<double>("nonThermal.injection.energyFraction_e");

	double sum = 0.0;
	Vector Qfactor(nR,1.0);
	p.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double vol = volume(r);
		double dens = 0.0;
		double temp = 0.0;
		if (p.id == "ntElectron") {
			dens = st.denf_e.get(iR);
			temp = st.tempElectrons.get(iR);
		} else if (p.id == "ntProton") {
			dens = st.denf_i.get(iR);
			temp = st.tempIons.get(iR);
		}
		Qfactor[iR.coord[DIM_R]] = dens * boltzmann * temp * abs(radialVel(r))/r;
		Qfactor[iR.coord[DIM_R]] = dens;
		sum += vol * Qfactor[iR.coord[DIM_R]];
	},{0,-1,0});
	Ainjection = etaInj*accRateOut*cLight2 / sum;		// [non-dimensional]

////////////////////////////////////////////////////////

	p.ps.iterate([&](const SpaceIterator& iR) {

		double r = iR.val(DIM_R);
		double rB2 = r*sqrt(paso_r);
		double rB1 = rB2/paso_r;
		double vR = radialVel(r);
		double tCell = (rB2-rB1)/abs(vR);
		double vol = volume(r);
		double height = height_fun(r);
		double B = st.magf.get(iR);
		double rho = massDensityADAF(r);
		double tAccretion = accretionTime(r);
		double rateAccretion = 1.0/tAccretion;
		double rateWind = 2.0*s*abs(radialVel(r))/r;

		// We define a new mesh of points
		size_t M = 199;
		double g_min = p.emin()/(p.mass*cLight2);
		double g_max = p.emax()/(p.mass*cLight2);

		double paso_g = pow(g_max/g_min,1.0/M);
		Vector g_au(M+3,g_min/paso_g);
		Vector delta_g_m_au(M+2,0.0);
		Vector delta_g(M+1,0.0);

		double g_inj = 2.0*g_min;
		double E_inj = g_inj*p.mass*cLight2;

		double dens = 0.0;
		if (p.id == "ntProton")
			dens = st.denf_i.get(iR);
		else if (p.id == "ntElectron")
			dens = st.denf_e.get(iR);

		double Q0 = Ainjection * Qfactor[iR.coord[DIM_R]] / g_inj;
		double sigma = g_inj / 5.0;

		for (size_t jg=1;jg<M+3;jg++)	g_au[jg] = g_au[jg-1]*paso_g;
		for (size_t jg=0;jg<M+2;jg++)	delta_g_m_au[jg] = g_au[jg+1]-g_au[jg];
		for (size_t jg=0;jg<M+1;jg++)	delta_g[jg] = 0.5*(g_au[jg+2]-g_au[jg]);

		fun1 Bfun = [&p,&st,&iR,B,rho,height,vR,r] (double g)
						{
							double E = g*p.mass*cLight2;
							double tCool = E/abs(losses(E,p,st,iR));
							double Dg = diffCoeff_g(g,p,height,B,rho);
							return g/tCool - 2.0*Dg/g - 0.5*g*abs(vR)/r;
						};

		fun1 Cfun = [&p,B,height,rho] (double g)
						{
							double Dg = diffCoeff_g(g,p,height,B,rho);
							return Dg;
						};

		fun1 Tfun = [r,&p,height,B,rateAccretion,&rateWind] (double g)
						{
							double E = g*p.mass*cLight2;
							double rateDiff = 1.0/diffusionTimeTurbulence(E,height,p,B);
							return pow(rateDiff,-1);
						};

		fun1 Qfun = [Q0,g_inj,sigma] (double g)
						{
							return Q0*gsl_ran_gaussian_pdf(g-g_inj,sigma);
						};

		Vector d(M+1,0.0);
		Vector a(M+1,0.0), b(M+1,0.0), c(M+1,0.0);
		for (size_t m=0;m<M+1;m++) {
			double Bm_minusHalf = 0.5 * (Bfun(g_au[m]) + Bfun(g_au[m+1]));
			double Bm_plusHalf = 0.5 * (Bfun(g_au[m+1]) + Bfun(g_au[m+2]));
			double Cm_minusHalf = 0.5 * (Cfun(g_au[m]) + Cfun(g_au[m+1]));
			double Cm_plusHalf = 0.5 * (Cfun(g_au[m+1]) + Cfun(g_au[m+2]));
			double Qm = Qfun(g_au[m+1]);
			double Tm = Tfun(g_au[m+1]);
			double wm_minusHalf = Bm_minusHalf/Cm_minusHalf * delta_g_m_au[m];
			double wm_plusHalf = Bm_plusHalf/Cm_plusHalf * delta_g_m_au[m+1];
			double Wm_minusHalf(0.0), Wm_plusHalf(0.0);
			double Wplus_m_minusHalf(0.0), Wminus_m_minusHalf(0.0);
			double Wplus_m_plusHalf(0.0), Wminus_m_plusHalf(0.0);

			if ( abs(wm_minusHalf) < 1.0e-3 ) {
				Wm_minusHalf = pow(1.0+gsl_pow_2(wm_minusHalf)/24+gsl_pow_4(wm_minusHalf)/1920,-1);
				Wplus_m_minusHalf = Wm_minusHalf * exp(0.5*wm_minusHalf);
				Wminus_m_minusHalf = Wm_minusHalf * exp(-0.5*wm_minusHalf);
			} else {
				Wm_minusHalf = abs(wm_minusHalf)*exp(-0.5*abs(wm_minusHalf)) /
								(1.0-exp(-abs(wm_minusHalf)));
				if (wm_minusHalf > 0.0) {
					Wplus_m_minusHalf = abs(wm_minusHalf) / (1.0-exp(-abs(wm_minusHalf)));
					Wminus_m_minusHalf = Wm_minusHalf * exp(-0.5*wm_minusHalf);
				} else {
					Wplus_m_minusHalf = Wm_minusHalf * exp(0.5*wm_minusHalf);
					Wminus_m_minusHalf = abs(wm_minusHalf) / (1.0-exp(-abs(wm_minusHalf)));
				}
			}
			if ( abs(wm_plusHalf) < 1.0e-3 ) {
				Wm_plusHalf = pow(1.0+gsl_pow_2(wm_plusHalf)/24+gsl_pow_4(wm_plusHalf)/1920,-1);
				Wplus_m_plusHalf = Wm_plusHalf * exp(0.5*wm_plusHalf);
				Wminus_m_plusHalf = Wm_plusHalf * exp(-0.5*wm_plusHalf);
			} else {
				Wm_plusHalf = abs(wm_plusHalf)*exp(-0.5*abs(wm_plusHalf)) /
								(1.0-exp(-abs(wm_plusHalf)));
				if (wm_plusHalf > 0.0) {
					Wplus_m_plusHalf = abs(wm_plusHalf) / (1.0-exp(-abs(wm_plusHalf)));
					Wminus_m_plusHalf = Wm_plusHalf * exp(-0.5*wm_plusHalf);
				} else {
					Wplus_m_plusHalf = Wm_plusHalf * exp(0.5*wm_plusHalf);
					Wminus_m_plusHalf = abs(wm_plusHalf) / (1.0-exp(-abs(wm_plusHalf)));
				}
			}

			if (m >= 1)
				a[m] = - Cm_minusHalf * Wminus_m_minusHalf / ( delta_g[m] * delta_g_m_au[m] );
			if (m <= M-1)
				c[m] = - Cm_plusHalf * Wplus_m_plusHalf / ( delta_g[m] * delta_g_m_au[m+1] );
			b[m] = 1.0/delta_g[m] * ( Cm_minusHalf * Wplus_m_minusHalf / delta_g_m_au[m] +
						Cm_plusHalf * Wminus_m_plusHalf / delta_g_m_au[m+1] ) + 1.0/Tm;
			d[m] = Qm;
		}
		TriDiagSys(a,b,c,d,M);
		size_t m = 1;
		p.ps.iterate([&](const SpaceIterator &iRE) {
			double logg = log10(iRE.val(DIM_E)/(p.mass*cLight2));
			while (log10(g_au[m]) < logg)
				m++;
			double slope = (safeLog10(d[m]/d[m-1]))/log10(g_au[m]/g_au[m-1]);
			double dist = slope * (logg-log10(g_au[m-1])) + safeLog10(d[m-1]);
			dist = pow(10,dist);
			p.injection.set(iRE,dist/(p.mass*cLight2));
		},{-1,iR.coord[DIM_R],0});
	},{0,-1,0});
}


#include <sophiatx/protocol/asset.hpp>

#include <fc/io/json.hpp>

#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace sophiatx { namespace protocol {

//1USD/5SPHTX   10SPHTX/2USD
#define BQ(a) \
   std::tie( a.base.symbol, a.quote.symbol )

#define DEFINE_PRICE_COMPARISON_OPERATOR( op ) \
bool operator op ( const price& a, const price& b ) \
{ \
   price a_(a.quote, a.base) ;\
   if( BQ(a) != BQ(b) ){ \
     if( BQ(a_) != BQ(b) ){ \
       return false; \
     }else{ \
       return a_ op b;\
     } \
   } \
   const uint128_t amult = uint128_t( b.quote.amount.value ) * a.base.amount.value; \
   const uint128_t bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value; \
   \
   return amult op bmult;  \
}

DEFINE_PRICE_COMPARISON_OPERATOR( == )
DEFINE_PRICE_COMPARISON_OPERATOR( != )
DEFINE_PRICE_COMPARISON_OPERATOR( <  )
DEFINE_PRICE_COMPARISON_OPERATOR( <= )
DEFINE_PRICE_COMPARISON_OPERATOR( >  )
DEFINE_PRICE_COMPARISON_OPERATOR( >= )

      asset operator * ( const asset& a, const price& b )
      {
         if( a.symbol == b.base.symbol )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value)/b.base.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.quote.symbol );
         }
         else if( a.symbol == b.quote.symbol )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value)/b.quote.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.base.symbol );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
      }

      price operator / ( const asset& base, const asset& quote )
      { try {
         FC_ASSERT( base.symbol != quote.symbol );
         return price{ base, quote };
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      price price::max( asset_symbol_type base, asset_symbol_type quote ) { return asset( share_type(SOPHIATX_MAX_SATOSHIS), base ) / asset( share_type(1), quote); }
      price price::min( asset_symbol_type base, asset_symbol_type quote ) { return asset( 1, base ) / asset( SOPHIATX_MAX_SATOSHIS, quote); }

      bool price::is_null() const { return *this == price(); }

      void price::validate() const
      { try {
         FC_ASSERT( base.amount > share_type(0) );
         FC_ASSERT( quote.amount > share_type(0) );
         FC_ASSERT( base.symbol != quote.symbol );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

string asset::to_string()const
{
   string result = std::to_string(amount.value / SOPHIATX_SATOSHIS);
   auto fract = amount.value % SOPHIATX_SATOSHIS;
      // prec is a power of ten, so for example when working with
      // 7.005 we have fract = 5, prec = 1000.  So prec+fract=1005
      // has the correct number of zeros and we can simply trim the
      // leading 1.
   result += "." + std::to_string(SOPHIATX_SATOSHIS + fract).erase(0,1);

   return result + " " + symbol.to_string();
}

asset asset::from_string( const std::string& from )
{
   try
   {
      std::string s = boost::algorithm::trim_copy( from );
      auto space_pos = s.find( " " );
      auto dot_pos = s.find( "." );

      FC_ASSERT( space_pos != std::string::npos, "Missing asset symbol" );

      if(dot_pos != std::string::npos)
      {
         FC_ASSERT( SOPHIATX_DECIMALS >= (space_pos - dot_pos - 1));
      }

      asset result;
      std::string str_symbol = s.substr( space_pos + 1 );

      auto numpart = s.substr( 0, space_pos );
      double dvalue = 0.0;

        try
        {
           dvalue = boost::lexical_cast<double>(numpart.c_str());
        }
        catch( const boost::bad_lexical_cast& e )
        {
           FC_THROW_EXCEPTION( fc::parse_error_exception, "Couldn't parse double" );
        }

      auto ivalue = static_cast<int64_t>(round(dvalue * SOPHIATX_SATOSHIS));

      FC_ASSERT( ivalue >= 0);
      FC_ASSERT( SOPHIATX_MAX_SHARE_SUPPLY >= ivalue);

      result.amount = ivalue;
      result.symbol = asset_symbol_type::from_string( str_symbol.c_str() );
      return result;
   }
   FC_CAPTURE_AND_RETHROW( (from) )
}

} } // sophiatx::protocol

namespace fc {
   void to_variant( const sophiatx::protocol::asset& var, fc::variant& vo )
   {
      vo = var.to_string();
      /*try
      {
         std::vector< variant > v( 2 );
         v[0] = boost::lexical_cast< std::string >( var.amount.value );
         v[1] = var.symbol.to_string();
         vo = v;
      } FC_CAPTURE_AND_RETHROW()*/
   }

   void from_variant( const fc::variant& var, sophiatx::protocol::asset& vo )
   {
      vo = sophiatx::protocol::asset::from_string(var.as< std::string >());
      /*try
      {
         auto v = var.as< std::vector< variant > >();
         FC_ASSERT( v.size() == 2, "Expected tuple of length 2." );

         // share_type is safe< int64_t >
         vo.amount = boost::lexical_cast< int64_t >( v[0].as< std::string >() );
         FC_ASSERT( vo.amount >= 0, "Asset amount cannot be negative" );
         vo.symbol = sophiatx::protocol::asset_symbol_type::from_string( v[1].as< std::string >() );
      } FC_CAPTURE_AND_RETHROW((var))*/
   }
}


#ifndef D_OT_H__
#define D_OT_H__
#include "emp-ot/ot.h"
#include <openssl/rand.h>
#include <boost/align/aligned_alloc.hpp>
#include <immintrin.h>
namespace emp {

void rand_bn(PRG &prg,Group *G,BigInt &a){
	unsigned char buf[512];
	prg.random_data(buf,G->order.size());
	a.from_bin(buf,G->order.size());
}

template<typename IO>
class BOT: public OT<BOT<IO>> { public:
	IO* io;
	Group *G = nullptr;
	bool delete_G = true;
	PRG prg;
	BOT(IO* io, PRG &prng, Group * _G = nullptr) {

		block seed;
		prng.random_block(&seed,1);
		prg.reseed(&seed);

		this->io = io;
		if (_G == nullptr)
			G = new Group();
		else {
			G = _G;
			delete_G = false;
		}
	}
	~BOT() {
		if (delete_G)
			delete G;
	}

	void send_impl(const block* data0, const block* data1, int length) {
		BigInt a;
		Point A, AaInv;
		block res[2];
		Point * B = new Point[length];
		Point * BA = new Point[length];

		rand_bn(prg,G,a);

		A = G->mul_gen(a);
		io->send_pt(&A);
		AaInv = A.mul(a);
		AaInv = AaInv.inv();

		for(int i = 0; i < length; ++i) {
			io->recv_pt(G, &B[i]);
			B[i] = B[i].mul(a);
			BA[i] = B[i].add(AaInv);
		}

		for(int i = 0; i < length; ++i) {
			res[0] = Hash::KDF(B[i], i);
			res[1] = Hash::KDF(BA[i], i);
			res[0] = xorBlocks(res[0], data0[i]);
			res[1] = xorBlocks(res[1], data1[i]);
			io->send_data(res, 2*sizeof(block));
		}

		delete[] BA;
		delete[] B;
	}

	void recv_impl(block* data, const bool* b, int length) {
		BigInt * bb = new BigInt[length];
		Point * B = new Point[length],
				* As = new Point[length],
				A;

		for(int i = 0; i < length; ++i)
			rand_bn(prg,G,bb[i]);

		io->recv_pt(G, &A);

		for(int i = 0; i < length; ++i) {
			B[i] = G->mul_gen(bb[i]);
			if (b[i])
				B[i] = B[i].add(A);
			io->send_pt(&B[i]);io->flush();
		}

		for(int i = 0; i < length; ++i)
			As[i] = A.mul(bb[i]);

		block res[2];
		for(int i = 0; i < length; ++i) {
			io->recv_data(res, 2*sizeof(block));
			data[i] = Hash::KDF(As[i], i);
			if(b[i])
				data[i] = xorBlocks(data[i], res[1]);
			else
				data[i] = xorBlocks(data[i], res[0]);
		}

		delete[] bb;
		delete[] B;
	}
};

template<typename T>
	T* aalloc(int length) {
		return (T*)boost::alignment::aligned_alloc(sizeof(T), sizeof(T)*length);
	}
inline void afree(void* p) {
	boost::alignment::aligned_free(p);
}

template<class IO>
class DOT {
public:

#ifdef __GNUC__
	#ifndef __clang__
		#pragma GCC push_options
		#pragma GCC optimize ("unroll-loops")
	#endif
#endif

	inline block bit_matrix_mul(const block * input) {
		uint8_t * tmp = (uint8_t *) (input);
		block res = pre_table[tmp[0] + (0<<8)];
		for(int i = 1; i < l/8; ++i)
			res = xorBlocks(res, pre_table[tmp[i] + (i<<8)]);
		return res;
	}

#ifdef __GNUC_
	#ifndef __clang___
		#pragma GCC pop_options
	#endif
#endif

	block *pre_table = nullptr;
	IO * io = nullptr;
	BOT<IO> * base_ot = nullptr;
	PRG prg, *G0 = nullptr, *G1 = nullptr;
	PRP pi;
	bool setup = false;
	block *k0 = nullptr, *k1 = nullptr, *tmp = nullptr, *t = nullptr;
	bool *s = nullptr, *extended_r = nullptr;
	block  *tT = nullptr;
	block block_s[2];
	block Delta;
	int l = 128, ssp, sspover8;
	const static int block_size = 1024;
	DOT(IO * io, block * pretable, PRG &prng,int ssp = 40) {
		this->pre_table = pretable;
		this->io = io;
		this->ssp = ssp;
		this->base_ot = new BOT<IO>(io,prng);
		this->sspover8 = ssp/8;
		this->l +=ssp;
		this->s = new bool[l];
		this->k0 = aalloc<block>(l);
		this->k1 = aalloc<block>(l);
		this->G0 = new PRG[l];
		this->G1 = new PRG[l];
		this->tT = aalloc<block>(block_size*2);
		this->t = aalloc<block>(block_size*2);
		this->tmp = aalloc<block>(block_size/128);

		block seed;
		prng.random_block(&seed,1);
		prg.reseed(&seed);
		for(int i=0;i<l;i++){
			prng.random_block(&seed,1);
			G0[i].reseed(&seed);
			prng.random_block(&seed,1);
			G1[i].reseed(&seed);
		}

		memset(t, 0, block_size * 32);
	}
	static block * preTable(int ssp) {
		block *pretable = aalloc<block>((1<<8) * 256/8);
		block * R = aalloc<block>(128*2);
		PRG prg2(fix_key);
		prg2.random_data(R, 128*256/8);
		uint64_t hi = 0ULL, lo = 0ULL;
		if(ssp <=64) {hi=0;lo=((1ULL<<ssp)-1);}
		block mask = makeBlock(hi,lo);
		for(int i = 0; i < 128; ++i)
			R[2*i+1] = andBlocks(R[2*i+1], mask);
		block tR[256];
		sse_trans((uint8_t *)(tR), (uint8_t*)R, 128, 256);
		for(int i = 0; i < 256/8; ++i)
			for (int j = 0; j < (1<<8); ++j) {
				pretable[j + (i<<8)] = zero_block();
				for(int k = 0; k < 8; ++k) {
					if (((j >> k ) & 0x1) == 1)
						pretable[j + (i<<8)] = xorBlocks(pretable[j+ (i<<8)], tR[i*8+k]);
				}
			}
		afree(R);
		return pretable;
	}

	block * out = nullptr;
	~DOT() {
		delete base_ot;
		delete_array_null(s);
		delete_array_null(G0);
		delete_array_null(G1);
		afree(this->t);
		afree(this->tT);
		afree(k0);
		afree(k1);
		afree(tmp);
		delete_array_null(extended_r);
	}

	void bool_to256(const bool * in, block res[2]) {
		bool tmpB[256];
		for(int i = 0; i < 256; ++i)tmpB[i] = false;
		memcpy(tmpB, in, l);
		res[0] = bool_to128(tmpB);
		res[1] = bool_to128(tmpB+128);
	}

	void setup_send(const bool* in_s, block * in_k0 = nullptr) {
		setup = true;
		memcpy(s, in_s, l);
		if(in_k0 != nullptr) {
			memcpy(k0, in_k0, l*sizeof(block));
		} else {
			base_ot->recv(k0, s, l);
		}
		for(int i = 0; i < l; ++i)
			G0[i].reseed(&k0[i]);

		bool_to256(s, block_s);
		Delta = bit_matrix_mul(block_s);
	}

	void setup_recv(block * in_k0 = nullptr, block * in_k1 =nullptr) {
		setup = true;
		if(in_k0 !=nullptr) {
			memcpy(k0, in_k0, l*sizeof(block));
			memcpy(k1, in_k1, l*sizeof(block));
		} else {
			prg.random_block(k0, l);
			prg.random_block(k1, l);
			base_ot->send(k0, k1, l);
		}
		for(int i = 0; i < l; ++i) {
			G0[i].reseed(&k0[i]);
			G1[i].reseed(&k1[i]);
		}
	}
	int padded_length(int length){
		return ((length+128+ssp+block_size-1)/block_size)*block_size;
	}
	void send_pre(int length) {
		length = padded_length(length);
		assert(setup);

		for (int j = 0; j < length/block_size; ++j) {
			for(int i = 0; i < l; ++i) {
				io->recv_data(tmp, block_size/8);
				G0[i].random_data(t+(i*block_size/128), block_size/8);
				if (s[i])
					xorBlocks_arr(t+(i*block_size/128), t+(i*block_size/128), tmp, block_size/128);
			}
			sse_trans((uint8_t *)(tT), (uint8_t*)t, 256, block_size);
			for(int i = 0; i < block_size; ++i)
				out[j*block_size + i] = bit_matrix_mul(tT+2*i);
		}
	}

	void recv_pre(const bool* r, int length) {
		int old_length = length;
		length = padded_length(length);

		assert(setup);

		bool * r2 = new bool[length];
		memcpy(r2, r, old_length);
		delete_array_null(extended_r);
		extended_r = new bool[length - old_length];
		prg.random_bool(extended_r, length- old_length);
		memcpy(r2+old_length, extended_r, length - old_length);

		block *block_r = aalloc<block>(length/128);
		for(int i = 0; i < length/128; ++i) {
			block_r[i] = bool_to128(r2+i*128);
		}

		for (int j = 0; j * block_size < length; ++j) {
			for(int i = 0; i < l; ++i) {
				G0[i].random_data(t+(i*block_size/128), block_size/8);
				G1[i].random_data(tmp, block_size/8);
				xorBlocks_arr(tmp, t+(i*block_size/128), tmp, block_size/128);
				xorBlocks_arr(tmp, block_r+(j*block_size/128), tmp, block_size/128);
				io->send_data(tmp, block_size/8);
			}
			sse_trans((uint8_t *)tT, (uint8_t*)t, 256, block_size);

			for(int i = 0; i < block_size; ++i)
				out[j*block_size + i] = bit_matrix_mul(tT+2*i);
		}
		free(block_r);
		delete[] r2;
	}

	void send(block * data, int length) {
		out = aalloc<block>(padded_length(length));
		send_pre(length);
		if(!send_check(length))
			error("OT Extension check failed");
		memcpy(data, out, sizeof(block)*length);
		free(out);
	}
	void recv(block* data, const bool* b, int length) {
		out = aalloc<block>(padded_length(length));
		recv_pre(b, length);
		recv_check(b, length);
		memcpy(data, out, sizeof(block)*length);
		free(out);
	}

	bool send_check(int length) {
		int extended_length = padded_length(length);
		block seed2, x, t[2], q[2], tmp1, tmp2;
		io->recv_block(&seed2, 1);
		block *chi = aalloc<block>(extended_length);
		PRG prg2(&seed2);
		prg2.random_block(chi, extended_length);

		q[0] = zero_block();
		q[1] = zero_block();
		for(int i = 0; i < extended_length; ++i) {
			mul128(out[i], chi[i], &tmp1, &tmp2);
			q[0] = xorBlocks(q[0], tmp1);
			q[1] = xorBlocks(q[1], tmp2);
		}
		io->recv_block(&x, 1);
		io->recv_block(t, 2);

		mul128(x, Delta, &tmp1, &tmp2);
		q[0] = xorBlocks(q[0], tmp1);
		q[1] = xorBlocks(q[1], tmp2);

		free(chi);
		return block_cmp(q, t, 2);
	}
	void recv_check(const bool* r, int length) {
		int extended_length = padded_length(length);
		block *chi = aalloc<block>(extended_length);
		block seed2, x = zero_block(), t[2], tmp1, tmp2;
		prg.random_block(&seed2,1);
		io->send_block(&seed2, 1);
		PRG prg2(&seed2);
		t[0] = t[1] = zero_block();
		prg2.random_block(chi,extended_length);
		for(int i = 0 ; i < length; ++i) {
			if(r[i])
				x = xorBlocks(x, chi[i]);
		}
		for(int i = 0 ; i < extended_length - length; ++i) {
			if(extended_r[i])
				x = xorBlocks(x, chi[i+length]);
		}

		io->send_block(&x, 1);
		for(int i = 0 ; i < extended_length; ++i) {
			mul128(chi[i], out[i], &tmp1, &tmp2);
			t[0] = xorBlocks(t[0], tmp1);
			t[1] = xorBlocks(t[1], tmp2);
		}
		io->send_block(t, 2);

		free(chi);
	}
};
}
#endif// ABIT_H__


#pragma once
#include <Eigen/Sparse>

#include "mtao/eigen/mat_to_triplets.hpp"
#include "mtao/eigen/type_info.hpp"

namespace mtao::eigen {
// TODO: see if doing these separately is better than the block ops
// makes a matrices of the form A,B,C
// and makes
// A   B
// C   D
template <typename T>
Eigen::SparseMatrix<T> merge_sparse_blocks(const Eigen::SparseMatrix<T>& A,
                                           const Eigen::SparseMatrix<T>& B,
                                           const Eigen::SparseMatrix<T>& C,
                                           const Eigen::SparseMatrix<T>& D) {
    using SPMat = Eigen::SparseMatrix<T>;
    std::vector<Eigen::Triplet<T>> triplets;
    triplets.reserve(A.nonZeros() + B.nonZeros() + C.nonZeros() + D.nonZeros());

    assert(A.rows() == B.rows());
    assert(C.rows() == D.rows());

    assert(A.cols() == C.cols());
    assert(B.cols() == D.cols());
    int row_offset = A.rows();
    int col_offset = A.cols();

    // TODO: see if doing these separately is better than the block ops

    for (int k = 0; k < A.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(A, k); it; ++it) {
            triplets.emplace_back(it.row(), col_offset + it.col(), it.value());
        }
    }

    for (int k = 0; k < B.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(B, k); it; ++it) {
            triplets.emplace_back(it.row(), col_offset + it.col(), it.value());
        }
    }

    for (int k = 0; k < C.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(C, k); it; ++it) {
            triplets.emplace_back(row_offset + it.row(), it.col(), it.value());
        }
    }
    for (int k = 0; k < D.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(D, k); it; ++it) {
            triplets.emplace_back(row_offset + it.row(), col_offset + it.col(),
                                  it.value());
        }
    }
    Eigen::SparseMatrix<T> R(A.rows() + C.rows(), A.cols() + B.cols());
    R.setFromTriplets(triplets.begin(), triplets.end());
    return R;
}

// A   C
// C^* B

template <typename T>
Eigen::SparseMatrix<T> merge_sparse_blocks(const Eigen::SparseMatrix<T>& A,
                                           const Eigen::SparseMatrix<T>& B,
                                           const Eigen::SparseMatrix<T>& C) {
    using SPMat = Eigen::SparseMatrix<T>;
    std::vector<Eigen::Triplet<T>> triplets;
    triplets.reserve(A.nonZeros() + B.nonZeros() + 2 * C.nonZeros());

    assert(A.rows() == A.cols());
    assert(B.rows() == B.cols());

    assert(A.rows() == C.rows());
    assert(C.cols() == B.cols());
    int row_offset = A.rows();
    int col_offset = A.cols();
    for (int k = 0; k < A.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(A, k); it; ++it) {
            triplets.emplace_back(it.row(), col_offset + it.col(), it.value());
        }
    }

    for (int k = 0; k < C.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(C, k); it; ++it) {
            triplets.emplace_back(it.row(), col_offset + it.col(), it.value());
            triplets.emplace_back(row_offset + it.col(), it.row(), it.value());
        }
    }

    for (int k = 0; k < B.outerSize(); ++k) {
        for (typename SPMat::InnerIterator it(B, k); it; ++it) {
            triplets.emplace_back(row_offset + it.row(), col_offset + it.col(),
                                  it.value());
        }
    }
    Eigen::SparseMatrix<T> R(A.rows() + B.rows(), A.cols() + B.cols());
    R.setFromTriplets(triplets.begin(), triplets.end());
    return R;
}

}  // namespace mtao::eigen


//
// Created by kostas on 24/12/20.
//
#include <boost/date_time/gregorian/gregorian.hpp>
#include "Calendar.h"

using namespace boost::gregorian;

std::ostream &operator<<(std::ostream &os, const Calendar &calendar) {
    os << calendar.month << "/" << calendar.year << std::endl;
    int eom_day = gregorian_calendar::end_of_month_day(calendar.year, calendar.month);
    date endOfMonth(calendar.year, calendar.month, eom_day);
    std::string WeekDays[] = {"Sun", "Mon","Tue", "Wed", "Thu","Fri","Sat"};
    for(auto &it:WeekDays){
        os << std::setw(10) << it;
    }

    day_iterator ditr(date(calendar.year, calendar.month, 1));
    os << std::endl;
    for (; ditr <= endOfMonth; ++ditr){
        greg_weekday dayOfWeek = ditr->day_of_week();
        if (dayOfWeek.as_number() == 0){
            os << std::endl;
        }
        os << std::setw(10) << ditr->day();
    }
    return os;
}

void Calendar::set(int month, int year) {
    Calendar::month = month;
    Calendar::year = year;
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, int N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a[N], b[N];

  for(int i = 0; i < N; ++i)
  {
    a[i] = ((i%2) ? T(i*2) : T(-2*i))/T(3);
    b[i] = bs::ceil(a[i]) ;
  }

  p_t aa(&a[0], &a[0]+N);
  p_t bb(&b[0], &b[0]+N);

  STF_IEEE_EQUAL(bs::ceil(aa), bb);
}

STF_CASE_TPL("Check ceil behavior on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL( "Check return type from ceil", STF_NUMERIC_TYPES)
{
  using bs::ceil;
  STF_TYPE_IS(decltype(ceil(bs::pack<T>())), bs::pack<T>);
}

STF_CASE_TPL( "Check ceil behavior on limit cases", STF_NUMERIC_TYPES)
{
  using bs::ceil;
  using bs::Valmin;
  using bs::Valmax;

  STF_EQUAL(ceil(Valmin<bs::pack<T>>()), Valmin<bs::pack<T>>());
  STF_EQUAL(ceil(Valmax<bs::pack<T>>()), Valmax<bs::pack<T>>());
}

STF_CASE_TPL( "Check ceil behavior on IEEE limit cases", STF_IEEE_TYPES)
{
  using bs::ceil;
  using bs::Valmin;
  using bs::Valmax;
  using bs::Inf;
  using bs::Minf;
  using bs::Nan;
  using bs::Maxflint;

  STF_EQUAL(ceil(Inf<bs::pack<T>>())       , Inf<bs::pack<T>>()        );
  STF_EQUAL(ceil(Minf<bs::pack<T>>())      , Minf<bs::pack<T>>()       );
  STF_IEEE_EQUAL(ceil(Nan<bs::pack<T>>())  , Nan<bs::pack<T>>()        );
  STF_EQUAL(ceil(Maxflint<bs::pack<T>>()+1), Maxflint<bs::pack<T>>()+1 );
  STF_EQUAL(ceil(Maxflint<bs::pack<T>>()  ), Maxflint<bs::pack<T>>()   );
  STF_EQUAL(ceil(Maxflint<bs::pack<T>>()-1), Maxflint<bs::pack<T>>()-1 );
}


#ifndef utils_hh
#define utils_hh

#ifdef PMTSIM_STANDALONE
#define LogError std::cerr
#endif

#include <cctype>
#include <vector>
#include <sstream>
#include <boost/filesystem.hpp>
#include <boost/tokenizer.hpp>
#include "boost/tuple/tuple.hpp"
#include "boost/tuple/tuple_io.hpp"

#include "G4UnitsTable.hh"
#include "G4SystemOfUnits.hh"
#include "G4Types.hh"
static const bool MYDEBUG = false;

static std::map<std::string, G4double> unit2value = {
{"g/cm2/MeV",1*g/cm2/MeV},
{"(g/cm2/MeV)*(g/cm2/MeV)",1*(g/cm2/MeV)*(g/cm2/MeV)}
};

template<typename T1>
bool with_units(T1& val, std::string unit) {
    // detect
    if (MYDEBUG) std::cout << "(I) detect unit: " << unit << std::endl;
    // remove start * or /
    bool op = true; // by default, multiply
    if (unit[0] == '*') {
        unit.erase(0, 1);
    } else if (unit[0] == '/') {
        op = false;
        unit.erase(0, 1);
    }
    if (MYDEBUG) std::cout << "(II) detect unit: " << unit << std::endl;

    double unit_val = 0;
    if(unit2value.count(unit)){
       unit_val = unit2value[unit];
    }
    else{
       unit_val = G4UnitDefinition::GetValueOf(unit);
    }

    if(!unit_val){
       LogError<<"can't find unit "<<unit<<" in MCParamsFileSvc::unit2value. Please register it at first!!!"<<std::endl;
       exit(-1);
    }
    if (MYDEBUG) std::cout << "(III) detect unit: " << unit << " " << unit_val << std::endl;

    if (op) {
        val *= unit_val;
    } else {
        val /= unit_val;
    }
    if (MYDEBUG) std::cout << "(IV) detect unit: " << unit << " final value: " << val << std::endl;

    return true;
}
template<>
inline bool with_units(std::string& /* val */, std::string /*unit*/) {
    std::cout << "string val don't support arithmetic." << std::endl;
    return false;
}

// template function helper
template<typename T1, typename T2>
bool get_implv1(const std::string& path,
        std::vector< boost::tuple<T1, T2> >& props)
{
    namespace fs = boost::filesystem;

    fs::path fullp(path);

    typename boost::tuple<T1, T2> elem;

    std::ifstream ifs(fullp.string().c_str());
    while (ifs.good()) {
            std::string temp_line;
            std::getline(ifs, temp_line);

            // skip comments, starts with '#'
            temp_line.erase( std::find( temp_line.begin(), temp_line.end(), '#' ), temp_line.end() );

            // std::cout << "TEMP LINE: " << temp_line << std::endl;
            std::stringstream ss;
            ss << temp_line;
            // ============================
            // First Element
            // ============================
            ss >> elem.template get<0>();
            // ss >> elem;
            if (ss.fail()) { continue; }

            char c = ss.get();
            while (isspace(c)) {
                c = ss.get();
                if (ss.fail()) { return false; }
            }
            ss.unget();
            if (!isdigit(c)) {
                // could be unit
                std::string unit_1st;
                ss >> unit_1st;
                if (!ss.fail()) {
                     with_units(elem.template get<0>(), unit_1st);
                }
            }

            // ============================
            // Second Element
            // ============================
            ss >> elem.template get<1>();
            if (ss.fail()) { continue; }
            // try detect unit
            std::string unit_2nd;
            ss >> unit_2nd;
            if (!ss.fail()) {
                with_units(elem.template get<1>(), unit_2nd);
            }

            // std::cout << "After parsed: " << elem << std::endl;
            props.push_back(elem);
        }
    return true;
}

// Parse the whole string as stringstream
template<typename T1, typename T2>
bool parse_implv1(const char* strcontent,
        std::vector< boost::tuple<T1, T2> >& props)
{
    std::stringstream ifs(strcontent);

    typename boost::tuple<T1, T2> elem;

    while (ifs.good()) {
        std::string temp_line;
        std::getline(ifs, temp_line);

        // skip comments, starts with '#'
        temp_line.erase( std::find( temp_line.begin(), temp_line.end(), '#' ), temp_line.end() );

        // std::cout << "TEMP LINE: " << temp_line << std::endl;
        std::stringstream ss;
        ss << temp_line;
        // ============================
        // First Element
        // ============================
        ss >> elem.template get<0>();
        // ss >> elem;
        if (ss.fail()) { continue; }

        char c = ss.get();
        while (isspace(c)) {
            c = ss.get();
            if (ss.fail()) { return false; }
        }
        ss.unget();
        if (!isdigit(c)) {
            // could be unit
            std::string unit_1st;
            ss >> unit_1st;
            if (!ss.fail()) {
                with_units(elem.template get<0>(), unit_1st);
            }
        }

        // ============================
        // Second Element
        // ============================
        ss >> elem.template get<1>();
        if (ss.fail()) { continue; }
        // try detect unit
        std::string unit_2nd;
        ss >> unit_2nd;
        if (!ss.fail()) {
            with_units(elem.template get<1>(), unit_2nd);
        }

        // std::cout << "After parsed: " << elem << std::endl;
        props.push_back(elem);
    }

    return true;
}

#endif


#ifndef OPENGM_PYTHON_INTERFACE
#define OPENGM_PYTHON_INTERFACE 1
#endif

#include <stdexcept>
#include <string>
#include <sstream>
#include <stddef.h>
#include <boost/python.hpp>
#include <opengm/graphicalmodel/graphicalmodel.hxx>
#include "export_typedes.hxx"

using namespace boost::python;

namespace pyspace {
   template<class SPACE>
   std::string asString(const SPACE & space){
      std::stringstream ss;
      for(size_t i=0;i<space.numberOfVariables();++i){
         if(i==space.numberOfVariables()-1)
            ss<<"vi="<<i<<", number of labels="<<space.numberOfLabels(i);
         else
            ss<<"vi="<<i<<", number of labels="<<space.numberOfLabels(i)<<"\n";
      }
      return ss.str();
   }
}

template<class INDEX_TYPE>
void export_space() {

   typedef INDEX_TYPE IndexType;
   typedef opengm::DiscreteSpace<IndexType,IndexType> PySpace;

   class_<PySpace > ("Space", init< >())
   .def("__str__",&pyspace::asString<PySpace>)
   .add_property("size", &PySpace::numberOfVariables)
   .add_property("numberOfVariables", &PySpace::numberOfVariables)
   .def("__len__",&PySpace::numberOfVariables)
   .def("__getitem__", &PySpace::numberOfLabels, return_value_policy<return_by_value>())
   ;
}

template void export_space<GmIndexType>();


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_ACTIVATION_BASE_HPP_
#define CROCODDYL_CORE_ACTIVATION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/core/demangle.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/utils/to-string.hpp"

namespace crocoddyl {

template <typename _Scalar>
class ActivationModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  explicit ActivationModelAbstractTpl(const std::size_t nr) : nr_(nr){};
  virtual ~ActivationModelAbstractTpl(){};

  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) = 0;
  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
                        const Eigen::Ref<const VectorXs>& r) = 0;
  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
    return boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(), this);
  };

  std::size_t get_nr() const { return nr_; };

  /**
   * @brief Print information on the activation model
   */
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os, const ActivationModelAbstractTpl<Scalar>& model) {
    model.print(os);
    return os;
  }

  /**
   * @brief Print relevant information of the activation model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream& os) const { os << boost::core::demangle(typeid(*this).name()); }

 protected:
  std::size_t nr_;
};

template <typename _Scalar>
struct ActivationDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;

  template <template <typename Scalar> class Activation>
  explicit ActivationDataAbstractTpl(Activation<Scalar>* const activation)
      : a_value(0.), Ar(VectorXs::Zero(activation->get_nr())), Arr(DiagonalMatrixXs(activation->get_nr())) {
    Arr.setZero();
  }
  virtual ~ActivationDataAbstractTpl() {}

  Scalar a_value;
  VectorXs Ar;
  DiagonalMatrixXs Arr;

  static MatrixXs getHessianMatrix(const ActivationDataAbstractTpl<Scalar>& data) {
    return data.Arr.diagonal().asDiagonal();
  }
  static void setHessianMatrix(ActivationDataAbstractTpl<Scalar>& data, const MatrixXs& Arr) {
    data.Arr.diagonal() = Arr.diagonal();
  }
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_ACTIVATION_BASE_HPP_


#ifndef CELLWISESOURCEMORPHOGENPDE_HPP_
#define CELLWISESOURCEMORPHOGENPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "CellwiseSourceParabolicPde.hpp"

/**
 *  A PDE which has a source at each labelled non-apoptotic cell.
 *
 *  Modified from CellwiseSourceParabolicPde so only labelled cells have a source
 *
 */
template<unsigned DIM>
class MorphogenCellwiseSourceParabolicPde : public CellwiseSourceParabolicPde<DIM>
{

private:
    /*
     * Stores how side the central region of source cells is.
     */
    double mSourceWidth;

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<CellwiseSourceParabolicPde<DIM> >(*this);
       archive & mSourceWidth;
    }

public:

    /**
	 * Constructor.
	 *
	 * @param rCellPopulation reference to the cell population
	 * @param duDtCoefficient rate of reaction (defaults to 1.0)
	 * @param diffusionCoefficient rate of diffusion (defaults to 1.0)
	 * @param uptakeCoefficient the coefficient of consumption of nutrient by cells (defaults to 0.0)
	 * @param sourceWidth the width of the source of morphgen in the centte of the mesh (defaults to 1.0)
	 */
    MorphogenCellwiseSourceParabolicPde(AbstractCellPopulation<DIM, DIM>& rCellPopulation,
                                   double duDtCoefficient = 1.0,
                                   double diffusionCoefficient = 1.0,
                                   double uptakeCoefficient = 0.0,
                                   double sourceWidth = 2.0);

    /**
         * @return const reference to the cell population (used in archiving).
         */
        const AbstractCellPopulation<DIM>& rGetCellPopulation() const;

    /**
	 * Overridden ComputeDuDtCoefficientFunction() method
	 *
	 * @return the function c(x) in "c(x) du/dt = Grad.(DiffusionTerm(x)*Grad(u))+LinearSourceTerm(x)+NonlinearSourceTerm(x, u)"
	 *
	 * @param rX the point in space at which the function c is computed
	 */
	virtual double ComputeDuDtCoefficientFunction(const ChastePoint<DIM>& rX);

	/**
	 * Overridden ComputeSourceTerm() method.
	 *
	 * @return computed source term.
	 *
	 * @param rX the point in space at which the nonlinear source term is computed
	 * @param u the value of the dependent variable at the point
	 */
	virtual double ComputeSourceTerm(const ChastePoint<DIM>& rX,
									 double u,
									 Element<DIM,DIM>* pElement=NULL);

	/**
	 * Overridden ComputeSourceTermAtNode() method.
	 *
	 * @return computed source term at a node.
	 *
	 * @param rNode the node at which the nonlinear source term is computed
	 * @param u the value of the dependent variable at the node
	 */
	virtual double ComputeSourceTermAtNode(const Node<DIM>& rNode, double u);

	/**
	 * Overridden ComputeDiffusionTerm() method.
	 *
	 * @param rX The point in space at which the diffusion term is computed
	 * @param pElement The mesh element that x is contained in (optional).
	 *
	 * @return a matrix.
	 */
	virtual c_matrix<double,DIM,DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement=NULL);

};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(MorphogenCellwiseSourceParabolicPde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a MorphogenCellwiseSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const MorphogenCellwiseSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM, DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise a MorphogenCellwiseSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, MorphogenCellwiseSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM, DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)MorphogenCellwiseSourceParabolicPde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*CELLWISESOURCEMORPHOGENPDE_HPP_*/


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborX_BoostRangeAdapters.hpp"
#include "ArborX_EnableViewComparison.hpp"
#include <ArborX_Box.hpp>
#include <ArborX_DetailsAlgorithms.hpp>
#include <ArborX_Point.hpp>

#include <Kokkos_Core.hpp>

#include <boost/range/algorithm.hpp> // reverse_copy, replace_if, count, generate, count_if
#include <boost/range/algorithm_ext.hpp> // iota
#include <boost/range/numeric.hpp>       // accumulate
#include <boost/test/unit_test.hpp>

#include <random>
#include <sstream>

#define BOOST_TEST_MODULE BoostRangeAdapters

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(range_algorithms)
{
  Kokkos::View<int[4], Kokkos::HostSpace> w("w");

  boost::iota(w, 0);
  std::stringstream ss;
  boost::reverse_copy(w, std::ostream_iterator<int>(ss, " "));
  BOOST_TEST(ss.str() == "3 2 1 0 ");

  boost::replace_if(w, [](int i) { return (i > 1); }, -1);
  BOOST_TEST(w == std::vector<int>({0, 1, -1, -1}), tt::per_element());

  BOOST_TEST(boost::count(w, -1), 2);
  BOOST_TEST(boost::accumulate(w, 5), 4);
}

BOOST_AUTO_TEST_CASE(point_cloud)
{
  using ArborX::Point;
  using ArborX::Details::distance;
  double const seed = 3.14;
  std::default_random_engine generator(seed);
  std::uniform_real_distribution<double> distribution(-1., 1.);
  int const n = 10000;
  Kokkos::View<Point *, Kokkos::HostSpace> cloud("cloud", n);
  boost::generate(cloud, [&distribution, &generator]() {
    Point p;
    p[0] = distribution(generator);
    p[1] = distribution(generator);
    p[2] = distribution(generator);
    return p;
  });

  Point const origin = {0., 0., 0.};
  double const radius = 1.;
  // 4/3 pi 1^3 / 2^3
  double const pi = 6. *
                    static_cast<double>(boost::count_if(
                        cloud,
                        [origin, radius](Point point) {
                          return (distance(point, origin) <= radius);
                        })) /
                    static_cast<double>(n);

  double const relative_tolerance = .05;
  BOOST_TEST(pi == 3.14, tt::tolerance(relative_tolerance));
}


/*
 * rsxgltest - occlusion
 */

#define GL3_PROTOTYPES
#include <GL3/gl3.h>
#include <GL3/gl3ext.h>

#include "rsxgltest.h"
#include "math3d.h"
#include "sine_wave.h"

#include <stddef.h>
#include "occlusion_vpo.h"
#include "occlusion_fpo.h"

#include <io/pad.h>

#include <math.h>
#include <Eigen/Geometry>

const char * rsxgltest_name = "occlusion";

struct sine_wave_t rgb_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f
  },
  {
    0.5f,
    0.5f,
    1.5f
  },
  {
    0.5f,
    0.5f,
    2.5f
  }
};

struct sine_wave_t xyz_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f / 2.0f
  },
  {
    0.5f,
    0.5f,
    1.5f / 2.0f
  },
  {
    0.5f,
    0.5f,
    2.5f / 2.0f
  }
};

GLuint buffers[2] = { 0,0 };

GLuint shaders[2] = { 0,0 };
GLuint program = 0;

GLuint queries[2] = { 0,0 };

GLint ProjMatrix_location = -1, TransMatrix_location = -1, color_location = -1;

#define DTOR(X) ((X)*0.01745329f)
#define RTOD(d) ((d)*57.295788f)

Eigen::Projective3f ProjMatrix(perspective(DTOR(54.3),1920.0 / 1080.0,0.1,1000.0));

Eigen::Affine3f ViewMatrixInv =
  lookat(Eigen::Vector3f(0,0,-10),
	 Eigen::Vector3f(0,0,0),
	 Eigen::Vector3f(0,1,0)).inverse();

extern "C"
void
rsxgltest_pad(unsigned int,const padData * paddata)
{
  if(paddata -> BTN_UP) {
    tcp_printf("up\n");
  }
  else if(paddata -> BTN_DOWN) {
    tcp_printf("down\n");
  }
}

extern "C"
void
rsxgltest_init(int argc,const char ** argv)
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);

  // Set up us the program:
  shaders[0] = glCreateShader(GL_VERTEX_SHADER);
  shaders[1] = glCreateShader(GL_FRAGMENT_SHADER);

  program = glCreateProgram();

  glAttachShader(program,shaders[0]);
  glAttachShader(program,shaders[1]);

  // Supply shader binaries:
  glShaderBinary(1,shaders,0,occlusion_vpo,occlusion_vpo_size);
  glShaderBinary(1,shaders + 1,0,occlusion_fpo,occlusion_fpo_size);

  // Link the program for real:
  glLinkProgram(program);
  glValidateProgram(program);

  summarize_program("draw",program);

  GLint
    vertex_location = glGetAttribLocation(program,"position");

  ProjMatrix_location = glGetUniformLocation(program,"ProjMatrix");
  TransMatrix_location = glGetUniformLocation(program,"TransMatrix");
  color_location = glGetUniformLocation(program,"c");

  tcp_printf("vertex_location: %i\n",vertex_location);
  tcp_printf("ProjMatrix_location: %i TransMatrix_location: %i color_location: %i\n",
	     ProjMatrix_location,TransMatrix_location,color_location);

  glUseProgram(program);

  glUniformMatrix4fv(ProjMatrix_location,1,GL_FALSE,ProjMatrix.data());

  glUniform3f(color_location,1,1,1);

  // Set up us the vertex data:
  glGenBuffers(2,buffers);

  {
    const float geometry[] = {
      -10.0, -10.0, 0.0,
      10.0, -10.0, 0.0,
      10.0, 10.0, 0.0,

      10.0, 10.0, 0.0,
      -10.0, 10.0, 0.0,
      -10.0, -10.0, 0.0
    };

    glBindBuffer(GL_ARRAY_BUFFER,buffers[0]);

    glBufferData(GL_ARRAY_BUFFER,sizeof(float) * 3 * 6,geometry,GL_STATIC_DRAW);
    glEnableVertexAttribArray(vertex_location);
    glVertexAttribPointer(vertex_location,3,GL_FLOAT,GL_FALSE,0,0);

    glBindBuffer(GL_ARRAY_BUFFER,0);
  }

  glGenQueries(2,queries);
}

extern "C"
int
rsxgltest_draw()
{
  float rgb[3] = {
    compute_sine_wave(rgb_waves,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 2,rsxgltest_elapsed_time)
  };

  glClearColor(rgb[0],rgb[1],rgb[2],1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  float xyz[3] = {
    compute_sine_wave(xyz_waves,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 2,rsxgltest_elapsed_time)
  };

  {
    glUniform3f(color_location,1,1,1);

    Eigen::Affine3f transmat =
      Eigen::Affine3f::Identity() *
      Eigen::Translation3f(xyz[0] * 20 - 10.0,0,0);

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * transmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());

    glDrawArrays(GL_TRIANGLES,0,6);
  }

  {
    glBeginQuery(GL_ANY_SAMPLES_PASSED,queries[0]);

    glUniform3f(color_location,1,0,0);

    Eigen::Affine3f transmat =
      Eigen::Affine3f::Identity() *
      Eigen::Translation3f(0,0,5.0);

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * transmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());

    glDrawArrays(GL_TRIANGLES,0,6);

    glEndQuery(GL_ANY_SAMPLES_PASSED);
  }

  {
    glBeginQuery(GL_TIME_ELAPSED,queries[1]);

    glBeginConditionalRender(queries[0],GL_QUERY_NO_WAIT);

    glUniform3f(color_location,0,0,1);

    Eigen::Affine3f transmat =
      Eigen::Affine3f::Identity() *
      Eigen::Translation3f(0,-15,-5.0);

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * transmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());

    glDrawArrays(GL_TRIANGLES,0,6);

    glEndConditionalRender();
    //glQueryCounter(queries[1],GL_TIMESTAMP);

    glEndQuery(GL_TIME_ELAPSED);

    GLuint64 elapsed_time = 0;
    glGetQueryObjectui64v(queries[1],GL_QUERY_RESULT,&elapsed_time);
    tcp_printf("elapsed time: %lu\n",(unsigned long)elapsed_time);
  }

  {
    glUniform3f(color_location,1,0,1);

    Eigen::Affine3f transmat =
      Eigen::Affine3f::Identity() *
      Eigen::Translation3f(0,15,5.0);

    Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * transmat);
    glUniformMatrix4fv(TransMatrix_location,1,GL_FALSE,modelview.data());

    glDrawArrays(GL_TRIANGLES,0,6);
  }

  return 1;
}

extern "C"
void
rsxgltest_exit()
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glDeleteShader(shaders[0]);
  glDeleteProgram(program);
  glDeleteShader(shaders[1]);
}