text:
/****************************************************************************
 * Moreno2011Cell.h - defines cell_param struct - i.e. Cell
 *
 * Based on code from Moreno et al. 2011
 *
 *  Created on: Sep 30, 2014
 *      Author: Anthony Varghese
 ***************************************************************************/

#ifndef CELL_H_
#define CELL_H_

#define M_PI 3.14159265358979323846 /* pi - needed if using -std=c++11 */
#include <cmath>
#include <gsl/gsl_blas.h>

namespace cellmodel{
	class Cells;
}

enum NaChannelMMSolver{
	NotUsed,
	Iterative,
	LUDecomp
};

class Moreno2011Cell {
public:
	Moreno2011Cell();
	virtual ~Moreno2011Cell();
	Moreno2011Cell(const Moreno2011Cell& source);
	Moreno2011Cell& operator=(const Moreno2011Cell& rhs);

	void setParams(const double* par);
	void setXpos(unsigned int p){ pos_x = p; }
	void Copy(const Moreno2011Cell& in);
	void setEnvironment( cellmodel::Cells* env );
	void setState(double* newstate) {
		for (unsigned int i = 0; i < nstates; i++)
			state[i] = newstate[i];
	}
	/*
	 * Calling method is responsible for allocating Y array
	 */
	void getState(double* Y) {
		for (unsigned int i = 0; i < nstates; i++)
			Y[i] = state[i];
	}
	/*
	 * Calling method is responsible for allocating dYdt array
	 */
	void getRates(double* dYdt) {
		for (unsigned int i = 0; i < nstates; i++)
			dYdt[i] = rates[i];
	}
	void Calculate_Points(double t);
	double CalculateCurrents(double t);
	double CalculateCurrents(double t, const double Vx);
	void Calculate_All(const double t);
	void SetInitialConditions();
	void Calculate_Reset(double t);
	void setI_stimulus(double t);
	void updatedVdt(const double Iaxnm1, const double Iaxnp1){
		dV_old = dVdt;
		I_axnm1 = Iaxnm1;
		I_axnp1 = Iaxnp1;
		I_axial = Iaxnp1 + Iaxnm1;
		dVdt = -I_axial - I_total; // Rate of change of Vm
	}
	double getV()	const {	return V;	}
	double getdVdt()const {	return dVdt;	}
	double getI_total()const {	return I_total;	}
	void   setV	  (const double Vnew)	{ V = Vnew; }
	void   setdVdt(const double dVdtnew){ dVdt = dVdtnew; }
	/*
	 * Forward Euler update of V
	 */
	void   fe_updateV(const double dt)	{ V = V + dVdt * dt; }
	double getI_K1()	const {	return I_K1; }
	double getI_Na()	const {	return I_Na; }
	double getI_axnm1() const {	return I_axnm1; }
	double getI_axnp1() const {	return I_axnp1; }
	double getI_axial() const {	return I_axial; }
	// Na Channel States
	double getNaClosed()const { return C1 + C2 + C3; }
	double getNaIC23()	const { return IC2 + IC3; }
	double getNaIF()	const { return IF; }
	double getNaOS()	const { return OS; }
	double getNaO()		const { return O; }
	double getNaDClosed() const { return DC1 + DC2 + DC3; }
	double getNaDIC23()	const { return DIC2 + DIC3; }
	double getNaDIF()	const { return DIF; }
	double getNaDOS()	const { return DOS; }
	double getNaDO()	const { return DO; }
	double getNaDIM1()	const { return DIM1; }
	double getNaD_Closed() const { return D_C1 + D_C2 + D_C3; }
	double getNaD_IC23()const { return D_IC2 + D_IC3; }
	double getNaD_IF()	const { return D_IF; }
	double getNaD_OS()	const { return D_OS; }
	double getNaD_O()	const { return D_O; }
	double getNaD_IM1()	const { return D_IM1; }

	/*
	 * computeRushLarsenStepState
	 * - take a time step using Rush Larsen for gating variables
	 *   and semi-implicit for Markov State models
	 * Calling method is responsible for allocating Y array
	 */
	void computeRushLarsenStepState( const double t, const double dt );
	/*
	 * computeRushLarsen2StepState
	 * - take a time step using Second-order Rush Larsen for all states
	 *   and semi-implicit for Markov State models
	 * Calling method is responsible for allocating Y array
	 */
	void computeRushLarsen2StepState( const double t, const double dt, const int step, const double Vhalf );

	// Indices into the state array
	static const unsigned int nNaChFirstState = 0;
	// Number of states for Na Channel Markov state model:
	static const unsigned int nNaChLastState = 29;
	static const unsigned int nNaChStates = nNaChLastState + 1;
	// start of gating variables:
	static const unsigned int ngatem = nNaChStates;
	static const unsigned int nLastState = ngatem + 20;		// last state
	// Currently, the cell transmembrane potential is the last state
	static const unsigned int nV = nLastState;
	static const unsigned int nstates = nV + 1;	// Total number of states

	// Cell States
	double state[nstates]; // state variables for integration

	double peak_slope, t_min, V_min, t_thr, V_thr, t_max, V_max, t_EAD, V_EAD;
	double t_EAD2, V_EAD2, t_90, V_90, dV_old;
	bool flag2;
	double APD_90, DI, L_EAD;

	// To measure the work involved in the iterations:
	int 	MarkovIterations;
	double	MarkovIterationError;

private:
	// Need a pointer back to the container class to
	// find out conditions like waitTime, drug conc, etc.
	cellmodel::Cells* cellenv = nullptr;

	/****************************************************************************
	 Universal Constants
	 *****************************************************************************/
	static constexpr double R = 8314.472;		// J/mol*K
	static constexpr double T = 310;			// K
	static constexpr double F = 96485.3415;
	static constexpr double RTF = R*T/F;

	// const double Cm = 2.0;					// uF/ cm^2 -- not used anywhere
	static constexpr double CAP = 0.185;		// Cellular capacitance
	static constexpr double rho = 162;			// ohm*cm

	// Channel conductances - these can be set externally
	// used to be:
	// static constexpr double G_Na = 15.0;	//27.5  //18.5
	double G_Na = 15.0;		// 15.0 in Moreno et al 2011
	double GK1	= 5.405;	// 5.405 in Moreno et al 2011

	// Ion Valences
	static constexpr double z_Na = 1;			// Valence of Na
	static constexpr double z_Ca = 2;			// Valence of Ca
	static constexpr double z_K = 1;			// Valence of K

	//Cell Geometry
	static constexpr double pi = M_PI;			// Should be 3.141592
	static constexpr double S_cg = 0.2;			// Surface to volume ratio (um^-1)

	//Intracellular volumes
	static constexpr double V_cyto=0.016404;	//16404 uL
	static constexpr double V_sr=0.001094;
	static constexpr double V_ss=0.00005468;

	// initial conditions of ion concentrations
	static constexpr double Na_out = 140;
	static constexpr double Ca_out = 2.0;
	static constexpr double K_out = 5.4;

	static constexpr double Vmin_default =	 0.0;	// mV - a "large" value so that minimums (expected to be < 0) can be detected
	static constexpr double Vthr_default = -90.0;	// mV - a "low" value so that the threshold can be detected

	bool initializedExternally = false;
	static const NaChannelMMSolver NaChanSolver = Iterative; // LUDecomp; // Iterative;

	// Arrays for each cell:
	double rates[nstates]; // rates of change of state - derivative
	// Total number of currents and auxilliary values:
	static const unsigned int naux = 26;
	double currents[naux];

	int Cell_type;
	unsigned int pos_x=0;
//	unsigned int pos_y=0, pos_z=0; // Position of cell in 3-d

	// Na Channel constants
	static constexpr double Q10  = 3;
	// This does not work in clang, ok in gcc
	//static constexpr double Tfactor = 1.0/(pow(Q10, (37.0-(T-273))/10.0));
	static constexpr double pH   = 7.4;

	// Na channel markov state model - the individual states are only needed for outputs
	double &IC3		= state[ 0], &IC2	= state[1];
	double &IF		= state[ 2], &IM1	= state[3];
	double &IM2		= state[ 4], &C3	= state[5];
	double &C2		= state[ 6], &O		= state[7];
	double &OS		= state[ 8], &C1	= state[9];

	double &DIC3	= state[10], &DIC2	= state[11];
	double &DIF 	= state[12], &DIM1	= state[13];
	double &DIM2	= state[14], &DC3 	= state[15];
	double &DC2		= state[16], &DO	= state[17];
	double &DOS		= state[18], &DC1	= state[19];

	double &D_IC3	= state[20], &D_IC2 = state[21];
	double &D_IF	= state[22], &D_IM1 = state[23];
	double &D_IM2	= state[24], &D_C3  = state[25];
	double &D_C2	= state[26], &D_O   = state[27];
	double &D_OS	= state[28], &D_C1  = state[29];
	double &V  = state[ngatem + 20];

	// Gating variables
	double &m	= state[ngatem],		&h	  = state[ngatem + 1];
	double &j   = state[ngatem + 2];
	double &d   = state[ngatem + 3],	&f	  = state[ngatem + 4];
	double &f2  = state[ngatem + 5],	&f_Ca = state[ngatem + 6];
	double &r   = state[ngatem + 7],	&s	  = state[ngatem + 8];
	double &xr1 = state[ngatem + 9],	&xr2  = state[ngatem + 10];
	double &xs  = state[ngatem + 11];
	double &R_bar = state[ngatem + 12];
	double &mL    = state[ngatem + 13], &hL	  = state[ngatem + 14];
	double &Ca_in = state[ngatem + 15], &Ca_sr = state[ngatem + 16];
	double &Ca_ss = state[ngatem + 17];
	double &Na_in = state[ngatem + 18], &K_in = state[ngatem + 19];
	double &dVdt  = rates[ngatem + 20];

	// old gating variable values before half-step in 2nd order Rush Larsen
	double oldgates[20];
	double &oldm	= oldgates[0],	&oldh		= oldgates[1];
	double &oldj	= oldgates[2];
	double &oldd	= oldgates[3],	&oldf		= oldgates[4];
	double &oldf2	= oldgates[5],	&oldf_Ca	= oldgates[6];
	double &oldr	= oldgates[7],	&olds		= oldgates[8];
	double &oldxr1	= oldgates[9],	&oldxr2 	= oldgates[10];
	double &oldxs	= oldgates[11];
	double &oldR_bar = oldgates[12];
	double &oldmL    = oldgates[13], &oldhL		= oldgates[14];
	double &oldCa_in = oldgates[15], &oldCa_sr	= oldgates[16];
	double &oldCa_ss = oldgates[17];
	double &oldNa_in = oldgates[18], &oldK_in	= oldgates[19];

	// Currents and auxilliary values - anything that may be useful to print out for plotting
	double &I_total = currents[0];
	double &I_axial = currents[1];
	double &I_axnm1 = currents[2], &I_axnp1 = currents[3];
	double &I_stim	= currents[4];
	double &I_Na	= currents[5], &I_Na_L = currents[6];
	double &mI_Na	= currents[7], &I_Na_b = currents[8];
	double &I_Ca_L	= currents[9], &I_Ca_b = currents[10];
	double &I_Kr	= currents[11], &I_Ks = currents[12];
	double &I_K1	= currents[13], &I_Kp = currents[14];
	double &I_to	= currents[15];
	double &I_Na_Ca = currents[16], &I_Na_K = currents[17];
	double &I_p_Ca	= currents[18];
	double &I_Na_ion_total	= currents[19];
	double &I_Ca_ion_total	= currents[20];
	double &I_K_ion_total	= currents[21];
	double &I_tr	= currents[22], &I_leak = currents[23];
	double &I_up	= currents[24], &I_rel = currents[25];

	// RATES
	// Na channel markov state model
	double &dIC3	= rates[ 0], &dIC2	= rates[1];
	double &dIF		= rates[ 2], &dIM1	= rates[3];
	double &dIM2	= rates[ 4], &dC3	= rates[5];
	double &dC2		= rates[ 6], &dO	= rates[7];
	double &dOS		= rates[ 8], &dC1	= rates[9];
	double &dDIC3	= rates[10], &dDIC2 = rates[11];
	double &dDIF	= rates[12], &dDIM1 = rates[13];
	double &dDIM2	= rates[14], &dDC3	= rates[15];
	double &dDC2	= rates[16], &dDO	= rates[17];
	double &dDOS	= rates[18], &dDC1	= rates[19];
	double &dD_IC3	= rates[20], &dD_IC2 = rates[21];
	double &dD_IF	= rates[22], &dD_IM1 = rates[23];
	double &dD_IM2	= rates[24], &dD_C3	= rates[25];
	double &dD_C2	= rates[26], &dD_O	= rates[27];
	double &dD_OS	= rates[28], &dD_C1	= rates[29];

	// Gating variable rates
	double &dm		= rates[ngatem     ],	&dh		= rates[ngatem +  1];
	double &dj		= rates[ngatem +  2];
	double &dd		= rates[ngatem +  3],	&df		= rates[ngatem +  4];
	double &df2		= rates[ngatem +  5],	&df_Ca	= rates[ngatem +  6];
	double &dr		= rates[ngatem +  7],	&ds		= rates[ngatem +  8];
	double &dxr1	= rates[ngatem +  9],	&dxr2	= rates[ngatem + 10];
	double &dxs		= rates[ngatem + 11];
	double &dR_bar	= rates[ngatem + 12];
	double &dmL		= rates[ngatem + 13],	&dhL	= rates[ngatem + 14];
	double &dCa_in	= rates[ngatem + 15],	&dCa_sr	= rates[ngatem + 16];
	double &dCa_ss	= rates[ngatem + 17];
	double &dNa_in	= rates[ngatem + 18],	&dK_in	= rates[ngatem + 19];

	// Cell data points - markers of cell function
	double I_total_old, dI_total, dI_total_old, t_I_total, I_total_pt;
	double V_I_total;
	bool   flagI_total, flag_EAD, flag_EAD2;
	double t_90_old;
	double CV, CV_EAD;

	// The temporary array is used for a modified-Newton iteration to solve
	// for the markov channel state variables.
	static const unsigned int	i3  = 0,  i2  = 1,	iF  = 2,  im  = 3,
								iM  = 4,  c3  = 5,	c2  = 6,  op  = 7,
								oS  = 8,  c1  = 9;
	static const unsigned int	di3 = 10, di2 = 11,	diF = 12, dim = 13,
								diM	= 14, dc3 = 15,	dc2	= 16, dop = 17,
								doS	= 18, dc1 = 19;
	static const unsigned int	Di3	= 20, Di2 = 21,	DiF	= 22, Dim = 23,
								DiM	= 24, Dc3 = 25,	Dc2	= 26, Dop = 27,
								DoS	= 28, Dc1 = 29;

	// BLAS vectors and methods to allocate, free and copy them
	gsl_vector* Yn   = nullptr;
	gsl_vector* Ynph = nullptr;
	gsl_vector* Ynp1 = nullptr;
	gsl_vector* Ynp1i= nullptr;
	gsl_vector* DiagonalAn = nullptr;
	gsl_vector* DiagonalAnInv = nullptr;
	gsl_matrix* OffDiagonalAn = nullptr;
	void allocTemps();
	void freeTemps();
	void copyTemps(const Moreno2011Cell& source);

	/*********************************************************************************************************
	 Current Function Prototypes
	 *********************************************************************************************************/
	void WT_SCN5A_Initial_Conditions();
	void WT_SCN5A_Lidocaine();
	void WT_SCN5A_Flecainide();
	void checkNaChannelStates();
	void calcI_total(double t);
	void calcI_Na(double t);	//Fast sodium current
	void calcI_Na_L();
	void calcI_K1();			//Time independent potassium current
	void calcI_Kp();			//Plateau K current, time independent, K_out insensitive
	void calcI_Na_Ca();			//Sodium calcium exchanger
	void calcI_Na_K();			//Sodium potassium pump
	void calcI_Ca_L();			//L type calcium channel Ca contribution
	void calcI_Kr();			//Rapid rectifier
	void calcI_Ks();			//Slow rectifier
	void calcI_to();			//Transient outward current
	void calcI_p_Ca();			//Sarcolemmal calcium pump
	void calcI_Ca_b();			//Calcium background current
	void calcI_Na_b();			//Sodium background current

	void calcI_tr();
	void calcI_leak();			//Leak from SR
	void calcI_up();			//Uptake to SR
	void calcI_rel();			//Release from SR

	//Dynamic Concentrations
	void Calculate_Na_in();			//Dynamic [Na] in Myoplasm
	void Calculate_K_in();			//Dynamic [K] in Myoplasm
	void Calculate_Ca_in();			//Dynamic [Ca] in Myoplasm
	void Calculate_Ca_sr();			//Dynamic [Ca] in JSR
	void Calculate_Ca_ss();			//Dynamic [Ca] in NSR

	// Semi-implicit and Rush-Larsen
	struct NaChannelParameters{
		double pKa, portion, diffusion,
				drug_charged, drug_neutral,
				drug_distance, kd0, kd_open;
		double	a11, a12, a13, b11, b12, b13,
				a3,	 b3,  a2,  b2,  a4,  b4, a5, b5,
				ax,	 bx,  ax1, bx1,
				a13c,a22, b33, a33,
				a44, b44, a55, b55,
				ax2, a13n, a_22, b_33,
				a_44,b_44, a_55, b_55,
				kon, koff, kcoff, kcon,
				b13c,b22,  k_on,  k_off,
				ki_on, ki_off, kc_on, kc_off,
				a_33, b13n,	b_22, bx2;
		void Initialize( cellmodel::Cells* cellenv );
		void ComputeVDepTerms(const double V, cellmodel::Cells* env );
		void set_NaCh_Matrices( gsl_matrix* M, gsl_vector* D);
	};
	NaChannelParameters NaChParams;
	static constexpr double tolerance = 1e-7;
	void semi_impl_I_Na(double t, const double dt);	//Fast sodium current
	void semi_impl_WT_SCN5A(const double dt);
	void semi_impl_WT_SCN5A_LU(const double dt);
	void exponential_I_Na1(double t, const double dt);
	void exponential_I_Na2(double t, const double dt, const double Vhalf);

	void rush_I_Na_L(const double dt);
	void rush_I_Ca_L(const double dt);			//L type calcium channel Ca contribution
	void rush_I_Kr(const double dt);			//Rapid rectifier
	void rush_I_Ks(const double dt);			//Slow rectifier
	void rush_I_to(const double dt);			//Transient outward current

	void rush_I_rel(const double dt);			//Release from SR
	void rush_Na_in(const double dt);			//Dynamic [Na] in Myoplasm
	void rush_K_in(const double dt);			//Dynamic [K] in Myoplasm
	void rush_Ca_in(const double dt);			//Dynamic [Ca] in Myoplasm
	void rush_Ca_sr(const double dt);			//Dynamic [Ca] in JSR
	void rush_Ca_ss(const double dt);			//Dynamic [Ca] in NSR

	// Old code - not used and not needed - new code is working
	void semi_impl_WT_SCN5A_Lidocaine(const double dt);
	void semi_impl_WT_SCN5A_Flecainide(const double dt);
	void set_NaCh_Matrices( gsl_matrix* M, gsl_vector* D,
			const double a11,	const double a12,	const double a13,
			const double b11,	const double b12,	const double b13,
			const double a3,	const double b3,	const double a2, const double b2,
			const double a4,	const double b4,	const double a5, const double b5,
			const double ax,	const double bx,	const double ax1,const double bx1,
			const double a13c,  const double a22,	const double b33,const double a33,
			const double a44,   const double b44,	const double a55,const double b55,
			const double ax2,   const double a13n,  const double a_22, const double b_33,
			const double a_44,	const double b_44,  const double a_55, const double b_55,
			const double kon,   const double koff,  const double kcoff, const double kcon,
			const double b13c,	const double b22,	const double k_on,	const double k_off,
			const double ki_on,	const double ki_off,const double kc_on, const double kc_off,
			const double a_33,	const double b13n,	const double b_22,	const double bx2 );
	double MCtemp[nNaChStates];
	double &mIC3	= MCtemp[ 0], &mIC2 	= MCtemp[1];
	double &mIF		= MCtemp[ 2], &mIM1		= MCtemp[3];
	double &mIM2	= MCtemp[ 4], &mC3		= MCtemp[5];
	double &mC2		= MCtemp[ 6], &mO		= MCtemp[7];
	double &mOS		= MCtemp[ 8], &mC1		= MCtemp[9];
	double &mDIC3	= MCtemp[10], &mDIC2	= MCtemp[11];
	double &mDIF	= MCtemp[12], &mDIM1	= MCtemp[13];
	double &mDIM2	= MCtemp[14], &mDC3		= MCtemp[15];
	double &mDC2	= MCtemp[16], &mDO		= MCtemp[17];
	double &mDOS	= MCtemp[18], &mDC1		= MCtemp[19];
	double &mD_IC3	= MCtemp[20], &mD_IC2	= MCtemp[21];
	double &mD_IF	= MCtemp[22], &mD_IM1	= MCtemp[23];
	double &mD_IM2	= MCtemp[24], &mD_C3	= MCtemp[25];
	double &mD_C2	= MCtemp[26], &mD_O		= MCtemp[27];
	double &mD_OS	= MCtemp[28], &mD_C1	= MCtemp[29];
	void semi_impl_WT_SCN5A_Lidocaine_compare_BLAS_old(const double dt);
	void semi_impl_WT_SCN5A_Flecainide_old (const double dt);
};

#endif /* CELL_H_ */

max_stars_repo_name: AnthonyVa/Cardcell

max_issues_repo_name: AnthonyVa/Cardcell

max_forks_repo_name: AnthonyVa/Cardcell


text:
#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_math.h>

#include "allvars.h"
#include "proto.h"

#ifdef RADTRANSFER

#ifdef EDDINGTON_TENSOR_STARS
static struct stardata_in
{
  MyDouble Pos[3], Density, Mass;
  MyFloat Hsml;
  int NodeList[NODELISTLENGTH];
}
 *StarDataIn, *StarDataGet;
#endif

void star_density(void)
{
  int j;

#ifdef EDDINGTON_TENSOR_STARS
  int i, dummy;
  int ngrp, sendTask, recvTask, place, nexport, nimport, ndone, ndone_flag;
#endif

  /* clear Je in all gas particles */

  for(j = 0; j < N_gas; j++)
    {
      if(P[j].Type == 0)
	SphP[j].Je = 0;

#ifdef SFR
      if(P[j].Type == 0)
	{
	  SphP[j].Je += SphP[j].Sfr * All.IonizingLumPerSFR *
	    (PROTONMASS / (P[j].Mass * All.UnitMass_in_g / All.HubbleParam)) * All.UnitTime_in_s /
	    All.HubbleParam;
	  printf("ET get_JE ID: %d Proc %d", P[j].ID, ThisTask);
	}
#endif
    }

#ifdef EDDINGTON_TENSOR_STARS

  /* allocate buffers to arrange communication */

  Ngblist = (int *) mymalloc(NumPart * sizeof(int));

  All.BunchSize =
    (int) ((All.BufferSize * 1024 * 1024) / (sizeof(struct data_index) + sizeof(struct data_nodelist) +
					     2 * sizeof(struct stardata_in)));
  DataIndexTable = (struct data_index *) mymalloc(All.BunchSize * sizeof(struct data_index));
  DataNodeList = (struct data_nodelist *) mymalloc(All.BunchSize * sizeof(struct data_nodelist));

  i = FirstActiveParticle;	/* beginn with this index */

  do
    {
      for(j = 0; j < NTask; j++)
	{
	  Send_count[j] = 0;
	  Exportflag[j] = -1;
	}

      /* do local particles and prepare export list */
      for(nexport = 0; i >= 0; i = NextActiveParticle[i])
	{
	  if(P[i].Type == 4)
	    {
	      if(star_density_evaluate(i, 0, &nexport, Send_count) < 0)
		break;
	    }
	}

#ifdef MYSORT
      mysort_dataindex(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);
#else
      qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);
#endif

      MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

      for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
	{
	  Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
	  nimport += Recv_count[j];

	  if(j > 0)
	    {
	      Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
	      Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
	    }
	}

      StarDataGet = (struct stardata_in *) mymalloc(nimport * sizeof(struct stardata_in));
      StarDataIn = (struct stardata_in *) mymalloc(nexport * sizeof(struct stardata_in));

      /* prepare particle data for export */
      for(j = 0; j < nexport; j++)
	{
	  place = DataIndexTable[j].Index;

	  StarDataIn[j].Pos[0] = P[place].Pos[0];
	  StarDataIn[j].Pos[1] = P[place].Pos[1];
	  StarDataIn[j].Pos[2] = P[place].Pos[2];
	  StarDataIn[j].Hsml = PPP[place].Hsml;
	  StarDataIn[j].Density = P[place].DensAroundStar;
	  StarDataIn[j].Mass = P[place].Mass;

	  memcpy(StarDataIn[j].NodeList,
		 DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));

	}

      /* exchange particle data */
      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	{
	  sendTask = ThisTask;
	  recvTask = ThisTask ^ ngrp;

	  if(recvTask < NTask)
	    {
	      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		{
		  /* get the particles */
		  MPI_Sendrecv(&StarDataIn[Send_offset[recvTask]],
			       Send_count[recvTask] * sizeof(struct stardata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A,
			       &StarDataGet[Recv_offset[recvTask]],
			       Recv_count[recvTask] * sizeof(struct stardata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		}
	    }
	}

      myfree(StarDataIn);

      /* now do the particles that were sent to us */

      for(j = 0; j < nimport; j++)
	star_density_evaluate(j, 1, &dummy, &dummy);

      /* check whether this is the last iteration */
      if(i < 0)
	ndone_flag = 1;
      else
	ndone_flag = 0;

      MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

      myfree(StarDataGet);
    }
  while(ndone < NTask);

  myfree(DataNodeList);
  myfree(DataIndexTable);
  myfree(Ngblist);

#endif //for EDDINGTON_TENSOR_STARS

}

#ifdef EDDINGTON_TENSOR_STARS
/*! This function represents the core of the SPH density computation. The
 *  target particle may either be local, or reside in the communication
 *  buffer.
 */
int star_density_evaluate(int target, int mode, int *nexport, int *nsend_local)
{
  int j, n, numngb;
  int startnode, listindex = 0;
  double h, hinv, h2, mass_j, weight, hinv3;
  double wk, mass, density, lum;
  double dx, dy, dz, r, r2, u, a3inv;
  MyDouble *pos;

#ifdef PERIODIC
  double boxsize, boxhalf;

  boxsize = All.BoxSize;
  boxhalf = 0.5 * All.BoxSize;
#endif

  if(All.ComovingIntegrationOn)
    a3inv = 1.0 / (All.Time * All.Time * All.Time);
  else
    a3inv = 1.0;

  if(mode == 0)
    {
      pos = P[target].Pos;
      h = PPP[target].Hsml;
      density = P[target].DensAroundStar;
      mass = P[target].Mass;
    }
  else
    {
      pos = StarDataGet[target].Pos;
      h = StarDataGet[target].Hsml;
      density = StarDataGet[target].Density;
      mass = StarDataGet[target].Mass;
    }

  h2 = h * h;
  hinv = 1.0 / h;
  hinv3 = hinv * hinv * hinv;

  lum = mass * All.IonizingLumPerSolarMass * (PROTONMASS / SOLAR_MASS) * All.UnitTime_in_s / All.HubbleParam;

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = StarDataGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  numngb = ngb_treefind_variable(pos, h, target, &startnode, mode, nexport, nsend_local);

	  if(numngb < 0)
	    return -1;

	  for(n = 0; n < numngb; n++)
	    {
	      j = Ngblist[n];

	      dx = pos[0] - P[j].Pos[0];
	      dy = pos[1] - P[j].Pos[1];
	      dz = pos[2] - P[j].Pos[2];
#ifdef PERIODIC
	      if(dx > boxHalf_X)
		dx -= boxSize_X;
	      if(dx < -boxHalf_X)
		dx += boxSize_X;
	      if(dy > boxHalf_Y)
		dy -= boxSize_Y;
	      if(dy < -boxHalf_Y)
		dy += boxSize_Y;
	      if(dz > boxHalf_Z)
		dz -= boxSize_Z;
	      if(dz < -boxHalf_Z)
		dz += boxSize_Z;
#endif
	      r2 = dx * dx + dy * dy + dz * dz;
	      r = sqrt(r2);

	      if(r2 < h2)
		{
		  u = r * hinv;

		  if(u < 0.5)
		    wk = hinv3 * (KERNEL_COEFF_1 + KERNEL_COEFF_2 * (u - 1) * u * u);
		  else
		    wk = hinv3 * KERNEL_COEFF_5 * (1.0 - u) * (1.0 - u) * (1.0 - u);

		}
	      else
		wk = 0;

	      mass_j = P[j].Mass;

	      weight = mass_j * wk / density;

	      SphP[j].Je += lum * weight / mass_j;
	    }
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = StarDataGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  return 0;
}
#endif

#endif

ext: c

max_stars_repo_name: egpbos/egp

max_issues_repo_name: egpbos/egp

max_forks_repo_name: egpbos/egp


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

#include <gsl/gsl_sf_bessel.h>

int main(void) {
    double x = 5.0;
    printf("J0(%g) = %.18e\n", x, gsl_sf_bessel_J0(x));
    return EXIT_SUCCESS;
}

max_stars_repo_name: liujiamingustc/phd

max_issues_repo_name: liujiamingustc/phd

max_forks_repo_name: liujiamingustc/phd


text:
#pragma once

#include <gsl/gsl>

#include <boost/noncopyable.hpp>

namespace hash_calculator
{

class FileMapper : private boost::noncopyable
{
public:
    FileMapper(int fd, size_t startPosition, size_t maxBufsize);
    ~FileMapper();

    void* getPtr(size_t currentPosition, size_t offset) const;
    void mapping();
    void remapping(size_t startPosition, size_t offset);

private:
    int m_fd;
    size_t m_startPosition;
    size_t m_maxBufsize;
    void* m_ptr;
};

} // hash_calculator

ext: h

max_stars_repo_name: nicledomaS/HashCalculator

max_issues_repo_name: nicledomaS/HashCalculator

max_forks_repo_name: nicledomaS/HashCalculator


text:
#pragma once
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_statistics.h>
#include <vector>
#include <utility>
#include <cmath>

class BSplineFit
{
public:
	BSplineFit(const BSplineFit&);
	BSplineFit();
	~BSplineFit();
	void initialize(unsigned dataSize, std::vector<double> datax, std::vector<double> datay,
		unsigned orderBSpline, unsigned nBreak);
	void freeAll();
	void solve_system();
	double calculateY(double x);
	std::vector<double> calculateY(std::vector<double> x);

	std::vector<double> getConfidentInterval(double level = 0.95);
	std::vector<double> getBoudaryPolyCoef(double boundaryPos);
	std::vector<double> getfittedPara();
	//return in the order of {orderBSpline, dataSize, nBreak, nBasis}
	std::vector<unsigned> getBSplinePara();
	std::pair<double, double> getxlim();

	static std::vector<double> calculateY(unsigned orderBSpline, unsigned nBreak,
		std::pair<double, double> xlim, std::vector<double> coef, std::vector<double> xpts);

private:

private:
	bool emptyStart;
	std::vector<double> datax, datay; // datax should be sorted in the ascending order

	gsl_matrix* X; // design matrix in linear least square regression, dataSize x nBasis, each row is evaluated for the same x with different basis function
	gsl_vector* y; // datay for fitting
	gsl_vector* coef; // coefficient in front of basis function
	gsl_matrix* cov; // variance-covariance matrix of linear regression, cov = RSS/(dataSize-nBasis) * (X^T*X)^(-1), RSS is called chisq in GSL
					 // where RSS is the sum of square residual = sum(yi-\hat{yi})^2, RSS/(dataSize-nBasis) = s^2, the unbiased estimator of system variance

	gsl_vector* B; // Bspline vector for a particular point x, to facilitate eval inside functions
	double RSS, TSS, Rsq;
	std::vector<double> confi95;
	std::vector<double> leftPoly, rightPoly;

	gsl_bspline_workspace* bw;
	gsl_multifit_linear_workspace* mw;
	unsigned orderBSpline; // order of B spline, gives a polynomial of order = orderBSpline-1
	unsigned dataSize; // number of data points to be fit
	unsigned nBreak; // number of break points, including left and right end points
	unsigned nBasis; // number of basis function in the interval = nbreak - 2 + orderBSpline
	// note this is the same as the number of parameter in the final linear least square fitting
	// when orderBSpline = 1, nBasis = nBreak-1, i.e. nBreak-1 step function fill
	// in each interval. When orderBSpline increase, the basis function number also increase
	// since the basis function is now extending over several interval and the left/right boundary will
	// see tails of basis function whose center point is outside the total range
};

max_stars_repo_name: zzpwahaha/Chimera-Control-Trim

max_issues_repo_name: zzpwahaha/Chimera-Control-Trim

max_forks_repo_name: zzpwahaha/Chimera-Control-Trim


text:
// Precompiled headers go in here
#pragma once

// Standard Library Header
#include <format>
#include <fstream>
#include <memory>
#include <stdexcept>
#include <random>
#include <algorithm>
#include <utility>
#include <mutex>
#include <shared_mutex>
#include <execution>
#include <optional>
#include <iostream>

// Data structures
#include <string>
#include <vector>
#include <array>

// GSL: Guidelines Support Library
//#include <gsl/gsl>

// Extern libs
#include "fmt/core.h"
#include "nlohmann/json.hpp"

// My
#include "Enums.h"
#include "RandomNumbers.h"
#include "Statistics.h"
#include "Disease/Disease.h"
#include "Disease/Infection.h"
#include "Person/Person.h"
#include "Places/Places.h"
#include "Places/Community.h"
#include "Simulation/MeasureTime.h"

max_stars_repo_name: Neithari/DiseaseSpreadSimulator

max_issues_repo_name: Neithari/DiseaseSpreadSimulator

max_forks_repo_name: Neithari/DiseaseSpreadSimulator


text:
#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <Python.h>
#include <numpy/arrayobject.h>
#include <lapacke.h>

/* DGESVD prototype */
extern void LAPACK_dgesvd( char* jobu, char* jobvt, int* m, int* n, double* a,
                    int* lda, double* s, double* u, int* ldu, double* vt, int* ldvt,
                    double* work, int* lwork, int* info );

double** An(double *pi, double *x, int labs, int dims);
double** A(double **PI, double **X, int labs, int dims, int n_pool);
double** Fisher(double *pi, double *x, double sigma, int labs, int dims);

static char estVar_docstring[] =
    "Calculate the A and Fisher matrix.";

static PyObject *varRedu_estVar(PyObject *self, PyObject *args);

static PyMethodDef module_methods[] = {
    {"estVar", varRedu_estVar, METH_VARARGS, estVar_docstring},
    {NULL, NULL, 0, NULL}
};

static struct PyModuleDef moduledef = {
    PyModuleDef_HEAD_INIT,
    "_variance_reduction",  /* m_name */
    "This module provides calculate A and Fisher matrix using C.",  /* m_doc */
    -1,                  /* m_size */
    module_methods,      /* m_methods */
    NULL,                /* m_reload */
    NULL,                /* m_traverse */
    NULL,                /* m_clear */
    NULL,                /* m_free */
};

PyMODINIT_FUNC PyInit__variance_reduction(void){
    PyObject *m = PyModule_Create(&moduledef);
    if(m==NULL){
        return NULL;
    }

    /* Load 'numpy' */
    import_array();

    return m;
}

double* matrix_mul(double* a, double* b, int m1, int n1, int m2, int n2){
    double *ret = (double*) malloc(m1 * n2 * sizeof(double));
    if(n1 != m2){
        return NULL;
    }
    for(int i=0; i<m1; i++)
        for(int j=0; j<n2; j++){
            double temp = 0.0;
            for(int p=0; p<n1; p++)
                temp += a[i*n1 + p] * b[p*n2 + j];
            ret[i*n2 + j] = temp;
        }
    return ret;
}

void pinv(double** X, int labs, int dims){
    int m = labs*dims, n = labs*dims, lda = labs*dims, ldu = labs*dims,
        ldvt = labs*dims, lwork, info;
    double wkopt;
    double *work;
    double *s  = (double*) malloc(labs*dims * sizeof(double));
    double *u  = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    double *vt = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    double *a  = (double*) malloc(labs*dims * labs*dims * sizeof(double));

    for(int i=0; i<m; i++)
        for(int j=0; j<n; j++)
            a[i*labs*dims + j] = X[i][j];

     /* Query and allocate the optimal workspace */
    lwork = -1;
    LAPACK_dgesvd("All", "All", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork,
        &info);
    lwork = (int)wkopt;
    work = (double*)malloc( lwork*sizeof(double) );
    /* Compute SVD */
    LAPACK_dgesvd("All", "All", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork,
        &info);
    /* Check for convergence  */
    if(info > 0) {
        printf("The algorithm computing SVD failed to converge. %d\n", info);
    }
    if(info < 0) {
        printf("Has illegal value. %d\n", info);
    }

    int numSigular = 0;
    double *si  = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    memset(si, 0, labs*dims * labs*dims * sizeof(double));

    for(int i=0; i<ldu; i++){
        if(s[i] > 1e-30){
            si[i*ldu + i] = 1.0 / s[i];
            numSigular += 1;
        }else{
            si[i+ldu + i] = 0.0;
        }
    }

    /* calculating transpose */
    double *ret = matrix_mul(vt, si, labs*dims, numSigular, numSigular, numSigular);
    double *ret_pinv = matrix_mul(ret, u, labs*dims, numSigular, numSigular, labs*dims);

    for(int i=0; i<m; i++)
        for(int j=0; j<n; j++)
            X[i][j] = ret_pinv[i*n + j];

    free(ret);
    free(ret_pinv);
    free(work);
    free(a);
    free(s);
    free(vt);
    free(u);
    free(si);

    return;
}

static PyObject *varRedu_estVar(PyObject *self, PyObject *args)
{
    int dims, n_pool, labs, sigma;
    PyObject *PI_obj, *X_obj, *ePI_obj, *eX_obj;

    if (!PyArg_ParseTuple(args, "dOOOO", &sigma, &PI_obj, &X_obj, &ePI_obj, &eX_obj))
        return NULL;

    PyArrayObject *PI_array  =  (PyArrayObject*)PyArray_FROM_OTF(PI_obj,  NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *X_array   =  (PyArrayObject*)PyArray_FROM_OTF(X_obj,   NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *ePI_array =  (PyArrayObject*)PyArray_FROM_OTF(ePI_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *eX_array  =  (PyArrayObject*)PyArray_FROM_OTF(eX_obj,  NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);

    if (PI_array == NULL || X_array == NULL || ePI_array == NULL || eX_array == NULL) {
        Py_XDECREF(PI_array);
        Py_XDECREF(X_array);
        Py_XDECREF(ePI_array);
        Py_XDECREF(eX_array);
        return NULL;
    }

    labs   = (int)PyArray_DIM(PI_array, 1);
    n_pool = (int)PyArray_DIM(X_array, 0);
    dims   = (int)PyArray_DIM(X_array, 1);

    double **PI  =  (double**) malloc(n_pool * sizeof(double*));
    double **X   =  (double**) malloc(n_pool * sizeof(double*));
    for(int i=0; i<n_pool; i++){
        PI[i] = (double*) malloc(labs * sizeof(double));
        X[i]  = (double*) malloc(dims * sizeof(double));
    }
    for(int i=0; i<n_pool; i++){
        for(int j=0; j<labs; j++){
            PI[i][j] = *(double*)PyArray_GETPTR2(PI_array, i, j);
        }
        for(int j=0; j<dims; j++){
            X[i][j] = *(double*)PyArray_GETPTR2(X_array, i, j);
        }
    }
    double *ePI =  (double*) PyArray_DATA(ePI_array);
    double *eX  =  (double*) PyArray_DATA(eX_array);

    double **retF = Fisher(ePI, eX, sigma, labs, dims);
    double **retA = A(PI, X, labs, dims, n_pool);

    pinv(retF, labs, dims);

    /* calculates the trace of the multiply of pinv(F) and A  */
    double score = 0.0;
    for(int i=0; i<dims*labs; i++){
        for(int k=0; k<dims*labs; k++){
            score += retA[i][k] * retF[k][i];
        }
    }

    Py_DECREF(PI_array);
    Py_DECREF(X_array);
    Py_DECREF(ePI_array);
    Py_DECREF(eX_array);

    PyObject* ret = Py_BuildValue("d", score);

    for(int i=0; i<n_pool; i++){
        free(PI[i]);
        free(X[i]);
    }
    free(PI);
    free(X);

    for(int i=0; i<labs*dims; i++){
        free(retF[i]);
        free(retA[i]);
    }
    free(retF);
    free(retA);

    return ret;
}

double** An(double *pi, double *x, int labs, int dims){
    double **g = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        g[i] = (double*) malloc(labs * sizeof(double));
        memset(g[i], 0, labs * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++){
            for(int c=0; c<labs; c++)
                if(p == c) g[p*dims + i][c] = pi[p] * (1.0-pi[p]) * x[i];
                else g[p*dims + i][c] = -1.0 * pi[p] * pi[c] * x[i];
        }

    double **an = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        an[i] = (double*) malloc(labs*dims * sizeof(double));
        memset(an[i], 0, labs*dims * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++)
            for(int q=0; q<labs; q++)
                for(int j=0; j<dims; j++){
                    /* inner product */
                    double tmp = 0.0;
                    for(int k=0; k<labs; k++){
                        tmp += g[p*dims + i][k] * g[q*dims + j][k];
                    }
                    an[p*dims + i][q*dims + j] = tmp;
                }

    for(int i=0; i<labs*dims; i++)
        free(g[i]);
    free(g);

    return an;
}

double** A(double **PI, double **X, int labs, int dims, int n_pool){
    double **ret = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        ret[i] = (double*) malloc(labs*dims * sizeof(double));
        memset(ret[i], 0, labs*dims * sizeof(double));
    }

    for(int n=0; n<n_pool; n++){
        double **an = An(PI[n], X[n], labs, dims);

        for(int p=0; p<labs; p++)
            for(int i=0; i<dims; i++)
                for(int q=0; q<labs; q++)
                    for(int j=0; j<dims; j++)
                        ret[p*dims + i][q*dims + j] += an[p*dims + i][q*dims + j];
        for(int i=0; i<labs*dims; i++)
            free(an[i]);
        free(an);
    }
    return ret;
}

double** Fisher(double *pi, double *x, double sigma, int labs, int dims){
    double **ret = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        ret[i] = (double*) malloc(labs*dims * sizeof(double*));
        memset(ret[i], 0, labs*dims * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++)
            for(int q=0; q<labs; q++)
                for(int j=0; j<dims; j++){
                    if(p == q && i == j)
                        ret[p*dims + i][q*dims + j] = x[i]*x[i]*pi[p]*(1.0-pi[p]) + 1.0/sigma*sigma;
                    else if(p == q && i != j)
                        ret[p*dims + i][q*dims + j] = x[i]*x[j]*pi[p]*(1.0-pi[p]);
                    else
                        ret[p*dims + i][q*dims + j] = x[i]*x[j]*pi[p]*pi[q];
                }
    return ret;
}

max_stars_repo_name: crowdEEG/libact

max_issues_repo_name: crowdEEG/libact

max_forks_repo_name: crowdEEG/libact


text:
/* specfunc/test_dilog.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_sf.h>
#include "test_sf.h"

int test_dilog(void)
{
  gsl_sf_result r;
  gsl_sf_result r1, r2;
  int s = 0;

  /* real dilog */

  TEST_SF(s, gsl_sf_dilog_e, (-3.0, &r),   -1.9393754207667089531,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (-0.5, &r),   -0.4484142069236462024,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (-0.001, &r), -0.0009997501110486510834,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (0.1, &r),     0.1026177910993911,        TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (0.7, &r),     0.8893776242860387386,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (1.0, &r),     1.6449340668482260,        TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (1.5, &r),     2.3743952702724802007,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (2.0, &r),     2.4674011002723397,        TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, ( 5.0, &r),    1.7837191612666306277,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, ( 11.0, &r),   0.3218540439999117111,     TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (12.59, &r),   0.0010060918167266208634,  TEST_TOL3, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (12.595, &r),  0.00003314826006436236810, TEST_TOL5, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (13.0, &r),   -0.07806971248458575855,    TEST_TOL2, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (20.0, &r),   -1.2479770861745251168,     TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (150.0, &r),  -9.270042702348657270,      TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_dilog_e, (1100.0, &r), -21.232504073931749553,     TEST_TOL0, GSL_SUCCESS);

  /* complex dilog */
  /* FIXME: probably need more tests here...
   * also need to work on accuracy for r->1; need to
   * adjust the switch-over point I suppose.
   */

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (1.00001, M_PI/2.0, &r1, &r2),
            -0.20562022409960237363, TEST_TOL1,
             0.91597344814458309320, TEST_TOL1,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.99999, M_PI/2.0, &r1, &r2),
            -0.20561329262779687646, TEST_TOL0,
             0.91595774018131512060, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.991, M_PI/2.0, &r1, &r2),
            -0.20250384721077806127, TEST_TOL0,
             0.90888544355846447810, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.98, M_PI/2.0, &r1, &r2),
            -0.19871638377785918403, TEST_TOL2,
             0.90020045882981847610, TEST_TOL2,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.95, M_PI/2.0, &r1, &r2),
            -0.18848636456893572091, TEST_TOL1,
             0.87633754133420277830, TEST_TOL1,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.8, M_PI/2.0, &r1, &r2),
            -0.13980800855429037810, TEST_TOL0,
             0.75310609092419884460, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.5, M_PI/2.0, &r1, &r2),
            -0.05897507442156586346, TEST_TOL0,
             0.48722235829452235710, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.01, M_PI/2.0, &r1, &r2),
            -0.000024999375027776215378, TEST_TOL3,
             0.009999888892888684820, TEST_TOL3,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (10.0, M_PI/2.0, &r1, &r2),
            -3.0596887943287347304, TEST_TOL0,
             3.7167814930680685900, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (100.0, M_PI/2.0, &r1, &r2),
            -11.015004738293824854, TEST_TOL0,
             7.2437843013083534970, TEST_TOL0,
	     GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.99, M_PI/8.0, &r1, &r2),
            1.0571539648820244720, TEST_TOL0,
            0.7469145254610851318, TEST_TOL0,
	    GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.99, M_PI/64.0, &r1, &r2),
            1.5381800285902999666, TEST_TOL0,
            0.1825271634987756651, TEST_TOL0,
	    GSL_SUCCESS);

  TEST_SF_2(s, gsl_sf_complex_dilog_e, (0.9, 3.0*M_PI/4.0, &r1, &r2),
            -0.6062840301356530985, TEST_TOL1,
             0.4836632833122775721, TEST_TOL1,
	    GSL_SUCCESS);

  return s;
}


text:
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_vector_double.h>

#ifndef UTILS_H
#define UTILS_H
#define max
int get_nlines(char *);
void print_gsl_vector(gsl_vector *);
void print_intarray(int *, int);
gsl_vector *vector_sub(gsl_vector *, gsl_vector *);
double vector_dist(gsl_vector *, gsl_vector *);
gsl_vector **load_vectors_from_file(char *, int);
int **permutations(int);
int factorial(int);
double min(double, double);
void crit_err(char *);

#endif


text:
#ifndef REACHOBJMULTIDEMO_H
#define REACHOBJMULTIDEMO_H

#include <cstdlib>
#include <ctime>
#include <vector>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/math/Math.h>

#include <iCub/iKin/iKinFwd.h>
#include <iCub/ctrl/math.h>

#include <gsl/gsl_math.h>

#include "reachObjMultiDemo_IDL.h"

class reachObjMultiDemo : public yarp::os::RFModule, public reachObjMultiDemo_IDL
{
protected:
    yarp::os::RpcServer             rpcPort;

    yarp::dev::IPositionControl*    posArm;
    yarp::dev::IVelocityControl*    velArm;
    yarp::dev::ITorqueControl *     itrqArm;
    yarp::dev::IEncoders*           encsArm;
    yarp::dev::IControlMode*        iCtrlArm;
    yarp::dev::IControlLimits*      iCtrlLimArm;

    yarp::dev::IPositionControl*    posRightArm;
    yarp::dev::IVelocityControl*    velRightArm;
    yarp::dev::ITorqueControl*      itrqRightArm;
    yarp::dev::IEncoders*           encsRightArm;
    yarp::dev::IControlMode*        iCtrlRightArm;
    yarp::dev::IControlLimits*      iCtrlLimRightArm;

    yarp::dev::IPositionControl*    posTorso;
    yarp::dev::IVelocityControl*    velTorso;
    yarp::dev::ITorqueControl*      itrqTorso;
    yarp::dev::IEncoders*           encsTorso;
    yarp::dev::IControlMode*        iCtrlTorso;
    yarp::dev::IControlLimits*      iCtrlLimTorso;

    iCub::iKin::iCubArm*            arm;
    yarp::sig::Vector               qA;

    yarp::dev::IGazeControl*        igaze;
    int                             contextGaze;
    yarp::dev::ICartesianControl   *iCartCtrlR;
    yarp::dev::ICartesianControl   *iCartCtrl;

    yarp::dev::PolyDriver           torsoDev;
    yarp::dev::PolyDriver           rightArmDev;
    yarp::dev::PolyDriver           headDev;

    yarp::dev::PolyDriver           rightCartDev;

    yarp::sig::Vector               encodersRightArm;
    yarp::sig::Vector               encodersTorso;

    std::string part;               //!< Should be "left_arm" or "right_arm"
    std::string robot;              //!< Should be "icub" or "icubSim"
    double      period;
    std::string name;
    bool        changeTipFrame;     //!<
    bool        useFakeTarget;
    bool        useHandAngle;
    yarp::sig::Vector               fakeTarget;
    double                          tiltAngle, yawAngle;

    yarp::sig::Vector   new_command_arm;
    yarp::sig::Vector   new_command_head;
    double              start_command_arm[16];  //!< Start command for the 16 arm joints
    double              away_command_arm[16];   //!< Away command for the 16 arm joints
    yarp::sig::Vector   start_command_head;     //!< Target for head in start position
    double              neck_range_min[3];
    double              neck_range_max[3];
    double              minLimArm[16];
    double              maxLimArm[16];

    bool                hasCommand;
    bool                hasTarget;

    yarp::os::BufferedPort<yarp::os::Bottle>    cmdJointsPort;
    yarp::os::Bottle                            *cmdJointsBottle;
    yarp::sig::Vector                           cmdJointsAng;

    yarp::os::BufferedPort<yarp::os::Bottle>    objPosDimPort;
    yarp::os::Bottle                            *objPoseDimBottle;
    yarp::sig::Vector                           objPoseDim;

    yarp::os::BufferedPort<yarp::os::Bottle>    tactilePort;
    yarp::os::Bottle                            *tactileBottle;

    yarp::os::BufferedPort<yarp::os::Bottle>    targetDumpedData;             //!< buffered port of dumped target data
    yarp::os::Stamp                             ts;

    yarp::sig::Vector                           joints_arm_min, joints_arm_max;

    yarp::os::RpcClient     rpcToMotorBabbling;
    yarp::os::RpcClient     rpcToLearning;

    bool    init_right_arm(); //!< Create PolyDriver for left arm
    bool    initRobot();
//    bool    moveHeadToStartPos(bool ranPos);
    bool    moveHeadToCentralPos();
    bool    moveArmToStartPos(const std::string &partName);
    bool    moveArmAway(const std::string &partName);
    bool    gotoStartPos(bool moveAway);
    bool    reachTarget();

    void    updateArmChain(const yarp::sig::Vector &q, yarp::sig::Vector &xEE_t);

    bool    sendCmdBabbling();
    bool    sendCmdLearningPredict();
    bool    sendCmdLearningMove(const int &id, const std::string &partName);

    std::vector<int> reprTaxelsForearm_sim =
    {3, 15, 27, 39, 51, 75, 87, 183,
    207, 255, 291, 303, 315, 339, 351}; //23 taxels

    std::vector<int> reprTaxelsHandL =
    {3, 15, 27, 39, 51, 99, 101, 109, 122, 134};

    std::vector<int> reprTaxelsHandR =
    {3, 15, 39, 51, 101, 118, 137};

public:
    bool    configure(yarp::os::ResourceFinder &rf);
    bool    interruptModule();
    bool    close();
    bool    attach(yarp::os::RpcServer &source);
    double  getPeriod();
    bool    updateModule();
//    reachBallDemo();

    //Thrift
    bool home_arms()
    {
        bool ok = false;
//        ok = moveArmToStartPos("left");
        ok = moveArmToStartPos("right");
        if (ok)
            yDebug("[%s] Moved all arms home sucessfully!!",name.c_str());
        else
            yDebug("[%s] Failed moving all arms home!!",name.c_str());
        return ok;
    }

    bool home_all()
    {
        bool ok = false;
        ok = moveHeadToCentralPos();
        ok = ok & home_arms();
        if (ok)
            yDebug("[%s] Moved all parts home sucessfully!!",name.c_str());
        else
            yDebug("[%s] Failed moving all parts home!!",name.c_str());
        return ok;
    }

    bool reach(bool _useCalib)
    {
        yDebug("[%s] Start reaching the target",name.c_str());
        hasCommand = true;
//        if (hasTarget)
//            return reachTarget();
//        else
//        {
//            yWarning("[%s] No target", name.c_str());
//            return false;
//        }
    }

    bool babble_then_reach(int32_t _id, const std::string &_part)
    {
        bool ok = sendCmdBabbling();
        yarp::os::Time::delay(3);
        ok = ok & sendCmdLearningPredict();
        yarp::os::Time::delay(1);

        if (_id==-1)    // reach all taxel of part
        {
//            if (_part=="b" or _part=="both")
//            {
//                for (int i=0; i<=reprTaxelsHandR.size(); i++)
//                {
//                    sendCmdLearningMove(reprTaxelsHandR[i], "h");

//                }
//            }
//            else if (_part=="h" or _part=="hand")
//            {

//            }
//            else if (_part=="a" or _part=="arm")
//            {

//            }
        }
        else
            ok = ok & sendCmdLearningMove(_id,_part);
        return ok;
    }

    bool set_tilt_angle(const double _ang)
    {
        tiltAngle = _ang;
        return true;
    }

    double get_tilt_angle()
    {
        return tiltAngle;
    }

    bool set_yaw_angle(const double _ang)
    {
        yawAngle = _ang;
        return true;
    }

    double get_yaw_angle()
    {
        return yawAngle;
    }

    bool enable_hand_angle()
    {
        useHandAngle = true;
        return true;
    }

    bool disable_hand_angle()
    {
        useHandAngle = false;
        return true;
    }

};

#endif // REACHBALLDEMO_H

max_stars_repo_name: robotology/visuomotor-learning

max_issues_repo_name: robotology/visuomotor-learning

max_forks_repo_name: robotology/visuomotor-learning


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

#ifndef RANDOM_H_
#define RANDOM_H_

#include <gsl/gsl_rng.h>
#include <vector>
#include <cassert>
#include <algorithm> // for lower_bound
// only for debugging
#include <iostream>
#include "libplump/utils.h"

namespace gatsby { namespace libplump {

/*******************************************************************************
 * RANDOM NUMBER GENERATION AND SAMPLING BASED ON GSL
 ******************************************************************************/

/**
 * Initialize global RNG.
 *
 * This function has to be called before using any of the sampling functions.
 *
 * The RNG type and seed are determined by the environment variables
 * GSL_RNG_TYPE and GSL_RNG_SEED.
 */
void init_rng();

/**
 * Free the global RNG.
 */
void free_rng();

/**
 * Returns true with probability true_prob.
 */
bool coin(double true_prob);

/**
 * Returns a uniform integer between 0 and max-1.
 */
long int uniform_int(long int max);

/**
 * Sample from a discrete distribution on 0,...,MAX with the given PDF.
 *
 * The probability if returning x for x \in 0,...,MAX is given by
 *   pdf[x] / (\sum_{i=0,...,end_pos} pdf[i])
 * i.e. pdf is normalized so that the sum of all elements up to and including
 * element end_pos is 1.
 *
 * Algorithm:
 *   1) Compute CDF; normalizing constant Z = CDF[end_pos]
 *   2) Sample z ~ Uniform(0,Z)
 *   3) find the smallest element x of CDF that is larger than i using binary
 *      search
 *   4) return x
 *
 * Complexity: O(log MAX)
 */
int sample_unnormalized_pdf(std::vector<double> pdf, int end_pos = 0);

////////////////////////////////////////////////////////////////////////////////
//////////////////////    INLINE FUNCTION DEFINITIONS   ////////////////////////
////////////////////////////////////////////////////////////////////////////////

extern gsl_rng* global_rng;

/**
 * Returns true with probability true_prob.
 */
inline bool coin(double true_prob) {
    return (true_prob>gsl_rng_uniform(global_rng));
}

/**
 * Returns a uniform integer between 0 and max-1.
 */
inline long int uniform_int(long int max) {
    return gsl_rng_uniform_int(global_rng, max);
}

/**
 * Sample from a discrete distribution on 0,...,MAX with the given PDF.
 *
 * The probability if returning x for x \in 0,...,MAX is given by
 *   pdf[x] / (\sum_{i=0,...,end_pos} pdf[i])
 * i.e. pdf is normalized so that the sum of all elements up to and including
 * element end_pos is 1.
 *
 * Algorithm:
 *   1) Compute CDF; normalizing constant Z = CDF[end_pos]
 *   2) Sample z ~ Uniform(0,Z)
 *   3) find the smallest element x of CDF that is larger than i using binary
 *      search
 *   4) return x
 *
 * Complexity: O(log MAX)
 */
inline int sample_unnormalized_pdf(std::vector<double> pdf, int end_pos) {
    assert(pdf.size() > 0);
    assert(end_pos < pdf.size());
    assert(end_pos >= 0);

    // if end_pos == 0, use entire vector
    if (end_pos == 0) {
        end_pos = pdf.size()-1;
    }

    // compute CDF (inplace)
    for (int i = 0; i < end_pos; ++i) {
        assert(pdf[i] >= 0);
        pdf[i+1] += pdf[i];
    }

    assert(pdf[end_pos] > 0);

    // sample pos ~ Unigorm(0,Z)
    double z = gsl_rng_uniform_pos(global_rng)*pdf[end_pos];

    assert((z >= 0) && (z <= pdf[end_pos]));

    // Perform binary search for z using std::lower_bound.
    // lower_bound(begin, end, x) returns the first element within [begin,end)
    // that is equal or larger than x.
    int x = std::lower_bound(pdf.begin(), pdf.begin() + end_pos + 1, z)
         - pdf.begin();

    assert(x == 0 || pdf[x-1] != pdf[x]);
    assert(x < pdf.size());
    return x;

}

} } // namespace gatsby::libplump

#endif // RANDOM_H_

max_stars_repo_name: jgasthaus/libPLUMP

max_issues_repo_name: jgasthaus/libPLUMP

max_forks_repo_name: jgasthaus/libPLUMP


text:
#ifndef TEST_COMMAND_INCLUDE
#define TEST_COMMAND_INCLUDE

#include <memory>
#include <string>
#include <vector>

#include <gsl/gsl>

#include "statement.h"
#include "yaml.h"

namespace execHelper {
namespace test {
namespace baseUtils {
using CommandKey = std::string;
using CommandKeys = std::vector<CommandKey>;
using Statements = std::vector<std::shared_ptr<Statement>>;

class TestCommand {
  public:
    TestCommand(CommandKey commandKey,
                Statements initialStatements = {}) noexcept;

    std::shared_ptr<Statement> operator[](size_t index) const noexcept;

    Statements::const_iterator begin() const noexcept;
    Statements::const_iterator end() const noexcept;

    size_t size() const noexcept;
    std::string get() const noexcept;
    unsigned int getNbOfStatements() const noexcept;
    unsigned int getNumberOfStatementExecutions()
        const noexcept; // Returns the sum of executions of all statements
    void add(std::shared_ptr<Statement> statement) noexcept;
    void resetExecutions() noexcept;

    void write(gsl::not_null<YamlWriter*> yaml) const noexcept;

  private:
    std::string m_command;
    Statements m_statements;
};
using Commands = std::vector<TestCommand>;

template <typename T, typename... Args>
inline std::shared_ptr<Statement> createStatement(Args... args) noexcept {
    return std::static_pointer_cast<Statement>(std::make_shared<T>(args...));
}
} // namespace baseUtils
} // namespace test
} // namespace execHelper

#endif /* TEST_COMMAND_INCLUDE */

max_stars_repo_name: exec-helper/source

max_issues_repo_name: exec-helper/source

max_forks_repo_name: exec-helper/source


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

#ifndef AUGUR_MCMCLIB_H
#define AUGUR_MCMCLIB_H

#include "augur_util.h"
#include "augur_math.h"
#include "augur_vecop.h"
#include "augur_blkop.h"
#include "augur_blkstk.h"
#include "augur_dist.h"
#include <augur_iface.h>
#include <math.h>
#include <gsl/gsl_rng.h>

typedef struct mcmc {
  AugurMod_t curr;
  AugurMod_t prop;
  AugurAux_t aux;
  int accept;
  double currLL;
  double propLL;
  double eps;
  double L;
  double leapLen;
  double deltaMax;
  int nutsalloc;
} mcmc_t;

mcmc_t MCMC;
gsl_rng* h_rng;

void dump_state(mcmc_t state) {
  printf("currLL: %f\n", state.currLL);
  printf("propLL: %f\n", state.propLL);
  printf("eps: %f\n", state.eps);
  printf("leapLen: %f\n", state.leapLen);
  printf("L: %f\n", state.L);

}

// double modObjFn(AugurMod_t curr, AugurAux_t aux);
EXTERNC void mcmcStep();

/*
void acceptAlways() {
  MCMC.currLL = modObjFn(MCMC.curr, MCMC.aux);
}
*/

/**
 * Returns TRUE if we should accept, FALSE if we should reject.
 */
Bool_t augur_mcmc_ar( double auxll
		    , double (*objFn)(AugurAux_t, AugurMod_t)
		    ) {
  double currll = objFn(MCMC.aux, MCMC.curr);
  double propll = objFn(MCMC.aux, MCMC.prop);
  double u = gsl_rng_uniform(h_rng);

  if (augur_log(u) < propll - currll + auxll) {
    return TRUE;
  }
  else {
    return FALSE;
  }
}

/**
 * Returns TRUE if we should accept, FALSE if we should reject.
 */
__HOSTORDEV__ Bool_t augur_mcmc_ar_idx
       ( double auxll
       , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
       , double (*objFn)(AugurAux_t, AugurMod_t)
       ) {
  double currll = objFn(aux, curr);
  double propll = objFn(aux, prop);
  // double u = gsl_rng_uniform(h_rng);
  double u = augur_std_uniform_sample(aux.rng);

  if (augur_log(u) < propll - currll + auxll) {
    return TRUE;
  }
  else {
    return FALSE;
  }
}

int acceptReject(double auxLL, double (*objFn)(AugurAux_t, AugurMod_t)) {
  MCMC.propLL = objFn(MCMC.aux, MCMC.prop);
  double u = gsl_rng_uniform(h_rng);
  // printf("currLL: %f  propLL: %f  auxLL: %f   ar: %d\n", MCMC.currLL, MCMC.propLL, auxLL, log(u) < MCMC.propLL - MCMC.currLL + auxLL);
  if (log(u) < MCMC.propLL - MCMC.currLL + auxLL) {
    MCMC.accept += 1;
    AugurMod_t tmp = MCMC.curr;
    MCMC.curr = MCMC.prop;
    MCMC.prop = tmp;
    // double tmp2 = MCMC.currLL;
    MCMC.currLL = MCMC.propLL;
    // MCMC.propLL = tmp2;
    return 1;
  }
  else {
    return 0;
  }
}

void swapCurrProp(double (*objFn)(AugurAux_t, AugurMod_t)) {
  MCMC.propLL = objFn(MCMC.aux, MCMC.prop);
  AugurMod_t tmp = MCMC.curr;
  MCMC.curr = MCMC.prop;
  MCMC.prop = tmp;
  double tmp2 = MCMC.currLL;
  MCMC.currLL = MCMC.propLL;
  MCMC.propLL = tmp2;
}

void initMcmc() {
  const gsl_rng_type * T;
  gsl_rng_env_setup();
  T = gsl_rng_default;
  h_rng = gsl_rng_alloc (T);

  // MCMC.curr = curr;
  // MCMC.prop = prop;
  // MCMC.aux = aux;
  MCMC.accept = 0;
  // MCMC.currLL = objFn(MCMC.aux, MCMC.curr);
  MCMC.propLL = MCMC.currLL;
  MCMC.eps = 1.0;
  MCMC.L = 20;
  MCMC.leapLen = 10.0;
  MCMC.deltaMax = 1000.0;
  MCMC.nutsalloc = 0;
  return;
}

EXTERNC void setHmcParams(double eps, double leapLen) {
  MCMC.eps = eps;
  MCMC.leapLen = leapLen;
}

EXTERNC void setHmcParams2(double eps, int L) {
  MCMC.eps = eps;
  MCMC.L = L;
}

#define max(x, y) ( x < y ? y : x)

double h_sample_uniform(gsl_rng* rng, double left, double right) {
  double u = gsl_rng_uniform(rng);
  return left + u * (right - left);
}

/**
 * Metropololis-within-gibbs Kernel. (Note it is host/dev code)
 */
__HOSTORDEV__ void augur_mcmc_mwg
      ( AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , double* llCorrect
      , void (*proposal)(AugurAux_t, AugurMod_t, AugurMod_t)
      , void (*swap)(AugurAux_t, AugurMod_t, AugurMod_t, int)
      , double (*objFn)(AugurAux_t, AugurMod_t) ) {
  proposal(aux, curr, prop);
  if (augur_mcmc_ar_idx(*llCorrect, aux, curr, prop, objFn)) {
    // Accept, so copy proposal theta into current theta (maintain invariant)
    swap(aux, curr, prop, 0);
  }
  else {
    // Reject, so copy current theta into proposal theta (maintain invariant)
    swap(aux, curr, prop, 1);
  }
}

/**
 * Gradient checking. (Call from host code)
 */
void h_augur_chk_grad
      ( AugurMemLoc_t loc, AugurBlk_t* pt, AugurBlk_t* grad
      , AugurAux_t aux, AugurMod_t curr
      , double (*objFn)(AugurAux_t, AugurMod_t) ) {
  double eps = 10e-8;
  printf("Evaluating gradient at point:\n");
  h_augur_blk_dump(loc, pt);

  for (uint_t i = 0; i < pt->num_blks; i++) {
    switch (pt->typs[i]) {
    case AUGUR_DBL: {
      // TODO
      break;
    }
    case AUGUR_VEC: {
      AugurFlatVec_t* fvec = (AugurFlatVec_t*) pt->blks[i];
      for (int j = 0; j < fvec->base_elems; j++) {
	h_augur_flat_vec_basis_add(loc, fvec, j, eps);
	double plus = objFn(aux, curr);
	h_augur_flat_vec_basis_add(loc, fvec, j, -2.0 * eps);
	double minus = objFn(aux, curr);
	double fin_diff = (plus - minus) / (2.0 * eps);
	double ad;
	h_augur_flat_vec_getd_idx(loc, &ad, (AugurFlatVec_t*) (grad->blks[i]), j);
	printf("plus: %f, minus: %f\n", plus, minus);
	printf("auto-diff: %f, finite-diff: %f, error: %f\n", ad, fin_diff, ad - fin_diff);
	h_augur_flat_vec_basis_add(loc, fvec, j, eps);
      }
      break;
    }
    case AUGUR_MAT: {
      AugurMat_t* mat = (AugurMat_t*) pt->blks[i];
      for (int j = 0; j < mat->row * mat->col; j++) {
	h_augur_mat_basis_add(loc, mat, j, eps);
	double plus = objFn(aux, curr);
	h_augur_mat_basis_add(loc, mat, j, -2.0 * eps);
	double minus = objFn(aux, curr);
	double fin_diff = (plus - minus) / (2.0 * eps);
	double ad;
	h_augur_mat_getd_idx(loc, &ad, (AugurMat_t*) (grad->blks[i]), j);
	printf("auto-diff: %f, finite-diff: %f, error: %f\n", ad, fin_diff, ad - fin_diff);
	h_augur_mat_basis_add(loc, mat, j, eps);
      }
      break;
    }
    default: {
      // TODO
      break;
    }
    }
  }
}

/**
 * Calls a gradient function. (Call from host code)
 *
 * The struct curr contains "the point" we are evaluating gradfn at.
 * The result of the gradient computation is in grad, which is part of
 * the struct aux.
 */
void h_augur_mcmc_call_grad
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr
      , AugurBlk_t* grad
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      ) {
  h_augur_blk_zero(loc, grad);
  gradfn(aux, curr);
}

/**
 * The struct curr contains pt, "the point" we are evaluating gradfn at.
 */
void h_augur_mcmc_chkcall_grad
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr
      , AugurBlk_t* grad
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , AugurBlk_t* pt
      , double (*objfn) (AugurAux_t, AugurMod_t)
      , Bool_t f_chkgrad
      ) {
  h_augur_mcmc_call_grad(loc, aux, curr, grad, gradfn);
  if (f_chkgrad) {
    h_augur_chk_grad(loc, pt, grad, aux, curr, objfn);
  }
}

/**
 * Leapfrog simulation. (Call from host code)
 *
 * Leapfrog with (POSITIVE) gradient of log-likelihood of objfn.
 * (Signs are important!!)
 *
 * theta0 / thetaStar: initial / proposal point
 * grad: result of gradient
 * p0 / pStar: initial / proposal momentum
 *
 * mod.thetaStar = thetaStar
 * aux.grad = grad
 *
 * gradfn(aux, mod) evaluates gradient at thetaStar, putting result in grad
 *
 */
void h_augur_mcmc_leapfrog1
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t mod
      , AugurBlk_t* grad, AugurBlk_t* thetaStar, AugurBlk_t* pStar
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , double stepsize
      ) {
  // Simulate Hamiltonian dynamics first half step of momentum
  // pStar = p0 + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, mod, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);

  // Full step for position
  // Position: thetaStar = thetaStar + stepsize * pStar
  h_augur_blk_scale_plus(loc, thetaStar, thetaStar, stepsize, pStar);

  // Simulate last half step of momentum
  // pStar = pStar + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, mod, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);
}

/**
 * Leapfrog simulation. (Call from host code)
 *
 * Leapfrog with (POSITIVE) gradient of log-likelihood of objfn.
 * (Signs are important!!)
 *
 * theta0 / thetaStar: initial / proposal point
 * grad: result of gradient
 * p0 / pStar: initial / proposal momentum
 *
 * curr.theta0 = theta0, prop.theta0 = thetaStar, aux.grad = grad
 *
 * gradfn(aux, curr) evaluates gradient at theta0, putting result in grad
 * gradfn(aux, prop) evaluates gradient at thetaStar, putting result in grad
 *
 */
void h_augur_mcmc_leapfrog
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurBlk_t* theta0, AugurBlk_t* thetaStar
      , AugurBlk_t* grad, AugurBlk_t* p0, AugurBlk_t* pStar
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , double (*objfn) (AugurAux_t, AugurMod_t)
      , double stepsize, int simsteps
      ) {
  // Simulate Hamiltonian dynamics first half step of momentum
  // pStar = p0 + stepsize / 2.0 * d_like(x, theta0)
  h_augur_mcmc_call_grad(loc, aux, curr, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, p0, stepsize / 2.0, grad);

  // Simulate full steps
  for (uint_t l = 0; l < simsteps; l++) {
    // Full step for position
    // Position: thetaStar = thetaStar + stepsize * pStar
    h_augur_blk_scale_plus(loc, thetaStar, thetaStar, stepsize, pStar);

    // Move momentum except at last step
    if (l != simsteps-1) {
      // Momentum: pStar = pStar + stepsize * d_like(x, thetaStar)
      h_augur_mcmc_call_grad(loc, aux, prop, grad, gradfn);
      h_augur_blk_scale_plus(loc, pStar, pStar, stepsize, grad);
    }
  }

  // Simulate last half step of momentum
  // pStar = pStar + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, prop, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);
}

/**
 * Hamiltonian Monte Carlo Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_hmc
     ( AugurMemLoc_t loc
     , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
     , AugurBlk_t* theta0, AugurBlk_t* thetaStar
     , AugurBlk_t* grad, AugurBlk_t* p0, AugurBlk_t* pStar
     , double simlen, double eps
     , void (*computeGrad)(AugurAux_t, AugurMod_t)
     , double (*objFn)(AugurAux_t, AugurMod_t)
     ) {
  // Initialize momentum
  h_augur_blk_randn(aux.rng, p0);

  // Randomize simsteps
  double scale_eps = h_augur_uniform_sample(h_rng, 0.8, 1.2);
  double stepsize = scale_eps * eps;
  double s = h_augur_uniform_sample(h_rng, 0.9, 1.1);
  uint_t simsteps = augur_max(1, round(s * simlen / stepsize));

  // Simulate Hamiltonian dynamics with Leapfrog method
  h_augur_mcmc_leapfrog(loc, aux, curr, prop, theta0, thetaStar, grad, p0, pStar, computeGrad, objFn, stepsize, simsteps);

  // Compute correction
  double initMomLL = h_augur_blk_dot(loc, p0, p0) / 2.0;
  double propMomLL = h_augur_blk_dot(loc, pStar, pStar) / 2.0;
  double auxll = initMomLL - propMomLL;

  // After trajectory, compute whether we should accept or reject
  if (augur_mcmc_ar(auxll, objFn)) {
    // Accept, so copy proposal theta into current theta (for invariant)
    h_augur_blk_cpy(loc, theta0, thetaStar);
  }
  else {
    // Reject, so copy current theta into proposal theta (for invariant)
    h_augur_blk_cpy(loc, thetaStar, theta0);
  }
}

/**
 * Elliptical Slice Sampling Kernel. (Note it is host/dev code)
 *
 * Suppose this kernel moves param = currTheta.
 * Then we have the following equalities on the inputs:
 *
 * curr.param = currTheta
 * prop.param = propTheta
 *
 * For compositionality, we maintain the invariant that:
 * curr.param = prop.param
 */
__HOSTORDEV__ void augur_mcmc_eslice
      ( AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurVec_t* curr_theta, AugurVec_t* prop_theta
      , AugurVec_t* mean, AugurMat_t* cov
      , AugurMat_t* L, AugurVec_t* nu
      , double (*objfn)(AugurAux_t, AugurMod_t) ) {
  augur_mvnormal_sample(aux.rng, nu, mean, cov, L, nu);
  double u = augur_std_uniform_sample(aux.rng);
  double log_y = objfn(aux, curr) + augur_log(u);

  double theta = augur_uniform_sample(aux.rng, 0.0, 2.0 * AUGUR_PI);
  double theta_min = theta - 2.0 * AUGUR_PI;
  double theta_max = theta;

  while (TRUE) {
    double diff = theta_max - theta_min;
    if (diff < 1e-8) {
      // Maintain invariant that curr == prop
      augur_vec_cpy(prop_theta, curr_theta);
      break;
    }

    // propSt = (currSt - mean) * cos(theta) + (nu - mean) * sin(theta) + mean;
    for (uint_t i = 0; i < prop_theta->elems; i++) {
      double mean_i = AUGUR_VEC_GETD(mean, i);
      double a = (AUGUR_VEC_GETD(curr_theta, i) - mean_i) * augur_cos(theta);
      double b = (AUGUR_VEC_GETD(nu, i) - mean_i) * augur_sin(theta);
      AUGUR_VEC_SETD(prop_theta, i, a + b + mean_i);
    }

    double prop_ll = objfn(aux, prop);
    if (prop_ll > log_y) {
      // Maintain invariant that curr == prop
      augur_vec_cpy(curr_theta, prop_theta);
      break;
    }

    // Shrink bracket
    if (theta < 0) {
      theta_min = theta;
    }
    else {
      theta_max = theta;
    }
    theta = augur_uniform_sample(aux.rng, theta_min, theta_max);
  }
}

/**
 * Reflective Slice Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_refl_slice
     ( AugurMemLoc_t loc
     , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
     , AugurBlk_t* theta0, AugurBlk_t* thetaProp
     , AugurBlk_t* grad, AugurBlk_t* p0
     , double simlen, double eps
     , void (*computeGrad)(AugurAux_t, AugurMod_t)
     , double (*objFn)(AugurAux_t, AugurMod_t)
     ) {
  // Initialize momentum
  h_augur_blk_randn(aux.rng, p0);

  // Compute slice level
  double z = objFn(aux, curr) - gsl_ran_exponential(h_rng, 1.0);
  int cond = 0;
  double stepsize = eps;
  uint_t simsteps = augur_max(1, round(simlen / stepsize));

  for (int i = 0 ; i < simsteps; i++) {
    // Take scaled-step in direction of momentum
    h_augur_blk_scale_plus(loc, thetaProp, thetaProp, stepsize, p0);

    cond = objFn(aux, prop) < z;
    if (cond) {
      // Reflect (outside version) if we leave slice

      // Compute grad at outside point
      h_augur_blk_zero(loc, grad);
      computeGrad(aux, prop);

      // Compute reflection and update direction
      double angle1 = h_augur_blk_dot(loc, p0, grad);
      double angle2 = h_augur_blk_dot(loc, grad, grad);
      double scale = angle1 / angle2;
      h_augur_blk_scale_plus(loc, p0, p0, -2.0 * scale, grad);

    }
  }

  if (!cond) {
    // Copy if we are in slice
    h_augur_blk_cpy(loc, theta0, thetaProp);
  }
}

/* NUTS */

typedef enum NutsDir {
  NUTS_MINUS = 0,
  NUTS_PLUS = 1,
} NutsDir_t;

typedef struct NutsRet {
  int n;
  int s;
} NutsRet_t;

typedef struct NutsRT {
  AugurBlk_t* work;
  AugurBlkStk_t* theta_prop_stk;
  AugurBlkStk_t* theta_stk;
  AugurBlkStk_t* mom_stk;
} NutsRT_t;

typedef struct NutsInfo {
  AugurAux_t aux;
  AugurMod_t mod;
  AugurBlk_t* grad;
  double (*modObjFn)(AugurAux_t, AugurMod_t);
  void (*computeGrad)(AugurAux_t, AugurMod_t);
  double stepsize;
  double logu;
} NutsInfo_t;

int h_augur_nuts_update_s(AugurMemLoc_t loc, int s, AugurBlk_t* work, AugurBlk_t* theta_plus, AugurBlk_t* mom_plus, AugurBlk_t* theta_minus, AugurBlk_t* mom_minus) {
  if (s == 1) {
    h_augur_blk_minus(loc, work, theta_plus, theta_minus);
    int ut1 = augur_indicator(h_augur_blk_dot(loc, work, mom_plus) >= 0.0);
    int ut2 = augur_indicator(h_augur_blk_dot(loc, work, mom_minus) >= 0.0);
    if (ut1 && ut2) {
      return 1;
    }
  }
  return 0;
}

/**
 * info.mod.theta = theta (gradient is always evaluated at latest value of theta)
 *
 * Invariants:
 * Suppose dir = PLUS, then:
 *
 * theta: latest value of theta^+
 * mom: latest value of mom^+
 */
NutsRet_t h_augur_nuts_build_tree
      ( AugurMemLoc_t loc, NutsInfo_t* info , NutsRT_t* rt
      , AugurBlk_t* shape
      , AugurBlk_t* theta, AugurBlk_t* mom, int j, NutsDir_t dir
      , int tab) {
  if (j == 0) {
    // Simulate (updates theta, mom)
    h_augur_mcmc_leapfrog1(loc, info->aux, info->mod,
			   info->grad, theta, mom,
			   info->computeGrad,
			   info->stepsize);

    // Update return value
    augur_blk_stk_push(loc, rt->theta_prop_stk, shape, theta);

    // Check stopping condition
    NutsRet_t ret;
    double ll = info->modObjFn(info->aux, info->mod) - 0.5 * h_augur_blk_dot(loc, mom, mom);
    ret.n = augur_indicator(info->logu <= ll);
    ret.s = augur_indicator(info->logu < MCMC.deltaMax + ll);

    return ret;
  }
  else {
    // Build left subtree
    NutsRet_t left = h_augur_nuts_build_tree(loc, info, rt, shape, theta, mom, j - 1, dir, tab+2);

    // Build right subtree (if we should keep going)
    if (left.s == 1) {
      // Save "left"-most
      if (j == 1) {
	augur_blk_stk_push(loc, rt->theta_stk, shape, theta);
	augur_blk_stk_push(loc, rt->mom_stk, shape, mom);
      }

      NutsRet_t right = h_augur_nuts_build_tree(loc, info, rt, shape, theta, mom, j - 1, dir, tab+2);

      // Propogate proposal with probability proportional to size of subtrees
      AugurBlk_t* prop1 = augur_blk_stk_pop(rt->theta_prop_stk);
      AugurBlk_t* prop2 = augur_blk_stk_pop(rt->theta_prop_stk);
      double bias = ((double) right.n) / (left.n + right.n);
      if (h_augur_bernoulli_sample(h_rng, bias) == 0) {
	augur_blk_stk_push(loc, rt->theta_prop_stk, shape, prop2);
      }
      else {
	augur_blk_stk_push(loc, rt->theta_prop_stk, shape, prop1);
      }
      augur_blk_stk_dump(loc, rt->theta_prop_stk);

      // Propogate tree size
      left.n += right.n;

      // Check u-turn
      AugurBlk_t* theta_minus; AugurBlk_t* mom_minus;
      AugurBlk_t* theta_plus; AugurBlk_t* mom_plus;
      switch (dir) {
      case NUTS_MINUS: {
	theta_plus = augur_blk_stk_peek(rt->theta_stk);
	mom_plus = augur_blk_stk_peek(rt->mom_stk);
	theta_minus = theta;
	mom_minus = mom;
	break;
      }
      case NUTS_PLUS: {
	theta_minus = augur_blk_stk_peek(rt->theta_stk);
	mom_minus = augur_blk_stk_peek(rt->mom_stk);
	theta_plus = theta;
	mom_plus = mom;
	break;
      }
      }
      left.s = h_augur_nuts_update_s(loc, right.s, rt->work, theta_plus, mom_plus, theta_minus, mom_minus);

      // Pop "left"-most
      if (j > 1) {
	augur_blk_stk_pop(rt->theta_stk);
	augur_blk_stk_pop(rt->mom_stk);
      }
    }

    return left;
  }
}

/**
 * No-U-Turn Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_nuts
      ( AugurMemLoc_t loc, AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurBlk_t* theta0, AugurBlk_t* theta_prop
      , AugurBlk_t* grad, AugurBlk_t* work
      , AugurBlk_t* theta_minus, AugurBlk_t* mom_minus
      , AugurBlk_t* theta_plus, AugurBlk_t* mom_plus
      , AugurBlk_t* shape
      , double stepsize
      , void (*computeGrad)(AugurAux_t, AugurMod_t)
      , double (*modObjFn)(AugurAux_t, AugurMod_t)
      ) {
  // Initialize simulation values
  h_augur_blk_cpy(loc, theta_minus, theta0);
  h_augur_blk_randn(aux.rng, mom_minus);
  h_augur_blk_cpy(loc, theta_plus, theta0);
  h_augur_blk_cpy(loc, mom_plus, mom_minus);

  // Initialize info
  NutsInfo_t info;
  info.aux = aux;
  info.mod = prop;
  info.grad = grad;
  info.modObjFn = modObjFn;
  info.computeGrad = computeGrad;
  info.logu = modObjFn(aux, curr) - 0.5 * h_augur_blk_dot(loc, mom_minus, mom_minus) - gsl_ran_exponential(h_rng, 1.0);

  // Initialize auxilliary nuts runtime values
  NutsRT_t rt;
  rt.work = work;
  AugurBlkStk_t theta_prop_stk = augur_blk_stk_stk_alloc();
  AugurBlkStk_t theta_stk = augur_blk_stk_stk_alloc();
  AugurBlkStk_t mom_stk = augur_blk_stk_stk_alloc();
  rt.theta_prop_stk = &theta_prop_stk;
  rt.theta_stk = &theta_stk;
  rt.mom_stk = &mom_stk;

  int n = 1;
  int j = 0;
  int s = 1;

  while (TRUE) {
    NutsRet_t nuts_ret;
    augur_blk_stk_reset(rt.theta_stk);
    augur_blk_stk_reset(rt.mom_stk);
    if (h_augur_bernoulli_sample(h_rng, 0.5)) {
      info.stepsize = -stepsize;
      h_augur_blk_cpy(loc, theta_prop, theta_minus);
      nuts_ret = h_augur_nuts_build_tree(loc, &info, &rt, shape, theta_prop, mom_minus, j, NUTS_MINUS, 0);
      h_augur_blk_cpy(loc, theta_minus, theta_prop);
    }
    else {
      info.stepsize = stepsize;
      h_augur_blk_cpy(loc, theta_prop, theta_plus);
      nuts_ret = h_augur_nuts_build_tree(loc, &info, &rt, shape, theta_prop, mom_plus, j, NUTS_PLUS, 0);
      h_augur_blk_cpy(loc, theta_plus, theta_prop);
    }

    AugurBlk_t* theta_prop_p = augur_blk_stk_pop(rt.theta_prop_stk);
    Bool_t f_cpy = FALSE;
    if (nuts_ret.s == 1) {
      double bias = augur_min(1.0, ((double) nuts_ret.n) / n);
      if (h_augur_bernoulli_sample(h_rng, bias) == 1) {
	// Update proposal
	h_augur_blk_cpy(loc, theta_prop, theta_prop_p);
	f_cpy = TRUE;
      }
    }

    n += nuts_ret.n;
    s = h_augur_nuts_update_s(loc, nuts_ret.s, rt.work, theta_plus, mom_plus, theta_minus, mom_minus);
    j += 1;

    if (s != 1) {
      // Maintain invariant
      if (f_cpy) {
	h_augur_blk_cpy(loc, theta0, theta_prop);
      }
      else {
	h_augur_blk_cpy(loc, theta_prop, theta0);
      }
      break;
    }
  }
}

#endif


text:
/*
 * applyForceToolFrame.h
 *
 *  Created on: Sep 11, 2015
 *      Author: mas
 */
#ifndef APPLY_FORCE_TOOLFRAME_H_
#define APPLY_FORCE_TOOLFRAME_H_
#include <cmath>
#include <barrett/units.h>
#include <barrett/systems.h>
#include <barrett/products/product_manager.h>
#include <libconfig.h++>
#include <barrett/detail/ca_macro.h>
#include <barrett/cdlbt/calgrav.h>
#include <barrett/systems/abstract/system.h>
#include <barrett/systems/abstract/single_io.h>
#include <barrett/systems/kinematics_base.h>
#include <barrett/math/kinematics.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <cstdlib>  // For std::atexit()
#include <barrett/os.h>  // For btsleep()
#include <barrett/math.h>  // For barrett::math::saturate()
#include <Eigen/Dense>
#include <Eigen/Core>
#include <barrett/standard_main_function.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#define BARRETT_SMF_VALIDATE_ARGS
#include <barrett/standard_main_function.h>

using namespace barrett;
using namespace systems;
using systems::connect;
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

template<size_t DOF>
class applyForceToolFrame : public systems::System
{
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

// IO  (inputs)
public:
Input<cp_type> CpInput;                 // cart pos. input
Input<Eigen::Quaterniond> OrnInput;     // orientation input (quaternion)
Input<cf_type> FtInput;                 // cart force input (in tool frame)
Input<ct_type> TtInput;                // cart torque input (in tool frame)

// IO  (outputs)
public:
Output<cf_type> CFOutput;    // output Cartesian forces (in base frame)
Output<ct_type> CTOutput;    // output Cartesian torque (in base frame)

protected:
typename Output<cf_type>::Value* cfOutputValue;
typename Output<ct_type>::Value* ctOutputValue;

public:
cf_type computedF;          // can be used to display later in the program
ct_type computedT;          // can be used to display later in the program

public:
applyForceToolFrame(
      const std::string& sysName = "applyForceToolFrame") :
      System(sysName), CpInput(this), OrnInput(this), FtInput(this) , TtInput(this) ,
      CFOutput(this, &cfOutputValue), CTOutput(this, &ctOutputValue)
{
}
virtual ~applyForceToolFrame()
{
this->mandatoryCleanUp();
}
protected:
    cf_type cf;
    ct_type ct;

    cf_type ForceTool;
    ct_type TorqueTool;
    cp_type Xcurr;
    Eigen::Quaterniond OrnCurr;

virtual void operate()
{

  Xcurr      = CpInput.getValue();       // current Cart. Pose
  OrnCurr    = OrnInput.getValue();      // current tool Orientation (quaternion)
  ForceTool  = FtInput.getValue();       // desired tool force
  TorqueTool = TtInput.getValue();       // desired tool torque

  OrnCurr.x() = - OrnCurr.x();           // wam quaternion imaginary part in negative
  OrnCurr.y() = - OrnCurr.y();           // wam quaternion imaginary part in negative
  OrnCurr.z() = - OrnCurr.z();           // wam quaternion imaginary part in negative
//****************************************
//  gsl_matrix* RotMat = gsl_matrix_alloc (3,3);
//  gsl_vector* Ft = gsl_vector_alloc(3);   // Force  in tool frame
//  gsl_vector* Tt = gsl_vector_alloc(3);   // Torque in tool frame
//
//  gsl_vector* Fb = gsl_vector_alloc(3);   // Force  in base frame
//  gsl_vector* Tb = gsl_vector_alloc(3);   // Torque in base frame
//
//  // RotMat = [  1 - 2*( OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z() ) ,       2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z())   ,       2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ;
//  //                 2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z())   ,   1 - 2*(OrnCurr.x()^2 + OrnCurr.z()^2)     ,       2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ;
//  //                 2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y())   ,       2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z())   ,   1 - 2*(OrnCurr.x()^2 + OrnCurr.y()^2)     ] ;
//
// gsl_matrix_set(RotMat, 0 , 0 , 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 0 , 1 ,     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 0 , 2 ,     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) );
//
// gsl_matrix_set(RotMat, 1 , 0 ,     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 1 , 1 , 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 1 , 2 ,     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x())  );
//
// gsl_matrix_set(RotMat, 2 , 0 ,     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y())  );
// gsl_matrix_set(RotMat, 2 , 1 ,     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z())  );
// gsl_matrix_set(RotMat, 2 , 2 , 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y())  );
//
//
//
// for (int i = 0; i<3 ; i++)
//        {
//           gsl_vector_set (Ft , i , ForceTool[i] ) ;
//           gsl_vector_set (Tt , i , TorqueTool[i] ) ;
//        }
//
// gsl_blas_dgemv(CblasNoTrans, 1.0, RotMat, Ft, 0.0, Fb);    // Fb = 1.0 * RotMat * Ft + 0.0
// gsl_blas_dgemv(CblasNoTrans, 1.0, RotMat, Tt, 0.0, Tb);    // Tb = 1.0 * RotMat * Tt + 0.0
//
// for (int i = 0; i<3 ; i++)
//       {
//           cf[i] = gsl_vector_get ( Fb , i );
//           ct[i] = gsl_vector_get ( Tb , i );
//       }
//****************************************

  cf[0] = ( 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) ) * ForceTool[0] +
          (     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) ) * ForceTool[1] +
          (     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ) * ForceTool[2]   ;

  cf[1] = (     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) ) * ForceTool[0] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) ) * ForceTool[1] +
          (     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ) * ForceTool[2]   ;

  cf[2] = (     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y()) ) * ForceTool[0] +
          (     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z()) ) * ForceTool[1] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y()) ) * ForceTool[2]   ;

//****************************************
  ct[0] = ( 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) ) * TorqueTool[0] +
          (     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) ) * TorqueTool[1] +
          (     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ) * TorqueTool[2]   ;

  ct[1] = (     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) ) * TorqueTool[0] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) ) * TorqueTool[1] +
          (     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ) * TorqueTool[2]   ;

  ct[2] = (     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y()) ) * TorqueTool[0] +
          (     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z()) ) * TorqueTool[1] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y()) ) * TorqueTool[2]   ;

//****************************************

  computedF =  cf;
  computedT =  ct;

  cfOutputValue->setData(&cf);
  ctOutputValue->setData(&ct);
}

private:
DISALLOW_COPY_AND_ASSIGN(applyForceToolFrame);
};

#endif /*APPLY_FORCE_TOOLFRAME_H_*/


text:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_core.h>
#include <gbpCosmo_NFW_etc.h>
#include <gsl/gsl_sf_expint.h>

// FFT of NFW profile from White '01
double rho_NFW_fft(double k, double M_vir, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double r_s;
    double z_k;
    double g_c;
    double rho_o;
    double x1;
    double x2;
    double Ci1;
    double Ci2;
    double Si1;
    double Si2;
    double r_val;
    set_NFW_params(M_vir, z, mode, cosmo, &c_vir, &R_vir);
    r_s   = R_vir / c_vir;
    z_k   = k * r_s;
    g_c   = 1. / (log(1. + c_vir) - c_vir / (1. + c_vir));
    rho_o = M_vir * g_c / (FOUR_PI * r_s * r_s * r_s);
    x1    = 1. + c_vir;
    x2    = c_vir * z_k;
    Ci1   = gsl_sf_Ci((1. + c_vir) * z_k);
    Si1   = gsl_sf_Si((1. + c_vir) * z_k);
    Ci2   = gsl_sf_Ci(z_k);
    Si2   = gsl_sf_Si(z_k);
    r_val = (FOUR_PI * rho_o * r_s * r_s * r_s / M_vir) * (cos(z_k) * (Ci1 - Ci2) + sin(z_k) * (Si1 - Si2) - sin(c_vir * z_k) / (z_k * (1. + c_vir)));
    return (r_val);
}

max_stars_repo_name: gbpoole/gbpCode

max_issues_repo_name: gbpoole/gbpCode

max_forks_repo_name: gbpoole/gbpCode


text:
#include <stdio.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

int
main(void)
{
  double data[5] = {17.2, 18.1, 16.5, 18.3, 12.6};
  double median, upperq, lowerq;

  printf ("Original dataset:  %g, %g, %g, %g, %g\n",
         data[0], data[1], data[2], data[3], data[4]);

  gsl_sort (data, 1, 5);

  printf ("Sorted dataset: %g, %g, %g, %g, %g\n",
         data[0], data[1], data[2], data[3], data[4]);

  median
    = gsl_stats_median_from_sorted_data (data,
                                         1, 5);

  upperq
    = gsl_stats_quantile_from_sorted_data (data,
                                           1, 5,
                                           0.75);
  lowerq
    = gsl_stats_quantile_from_sorted_data (data,
                                           1, 5,
                                           0.25);

  printf ("The median is %g\n", median);
  printf ("The upper quartile is %g\n", upperq);
  printf ("The lower quartile is %g\n", lowerq);
  return 0;
}

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
#pragma once

#define UPCA_ARCH_H

#include <memory>
#include <vector>

#include <stdint.h>

#include <gsl/gsl>

namespace upca {
namespace arch {

namespace detail {
template <typename Reason> struct null_resolver {
  struct null_type {
    friend std::ostream &operator<<(std::ostream &os, const null_type &) {
      return os;
    }
  };
  static null_type resolve(const std::string &) {
    throw std::runtime_error(Reason::reason);
  }
};

template <typename TIMESTAMP, typename REASON> struct basic_pmu {
  using resolver_type = upca::arch::detail::null_resolver<REASON>;
  template <typename T> basic_pmu(const T &) {}

  uint64_t timestamp_begin() { return TIMESTAMP::timestamp(); }
  uint64_t timestamp_end() { return TIMESTAMP::timestamp(); }

  gsl::span<uint64_t>::index_type start(gsl::span<uint64_t>) { return 0; }
  gsl::span<uint64_t>::index_type stop(gsl::span<uint64_t>) { return 0; }
};

} // namespace detail

template <typename ARCH> class arch_common_base final {
private:
  ptrdiff_t offset_ = 0;
  const ptrdiff_t slice_;
  ARCH arch_;

  template <typename T> friend class resolver;

public:
  using resolver_type = typename ARCH::resolver_type;

  template <typename C>
  arch_common_base(const C &pmcs, const unsigned external_pmcs = 0)
      : slice_(gsl::narrow<ptrdiff_t>(pmcs.size() + external_pmcs)),
        arch_(pmcs) {}

  gsl::span<uint64_t>::index_type start(gsl::span<uint64_t> output) {
    const auto count = arch_.start(output.subspan(1, slice_ - 1));
    output[0] = arch_.timestamp_begin();
    return count + 1;
  }

  gsl::span<uint64_t>::index_type stop(gsl::span<uint64_t> output) {
    output[0] = arch_.timestamp_end() - output[0];
    const auto count = arch_.stop(output.subspan(1, slice_ - 1));
    return count + 1;
  }
};

} // namespace arch
} // namespace upca

#ifdef __aarch64__

#  include "aarch64.h"

using pmu = upca::arch::arch_common_base<upca::arch::aarch64::pmu>;

#elif defined(__ARM_ARCH_7A__)

#  include "aarch32.h"

using pmu = upca::arch::arch_common_base<upca::arch::aarch32::pmu>;

#elif defined(__sparc)

#  include "sparc.h"

using pmu = upca::arch::arch_common_base<upca::arch::sparc::pmu>;

#elif defined(__x86_64__)
#  include "x86_64.h"

/* MCK can be either:
 * - mck_rawmsr for raw MSR access via rdmsr/wrmsr mcK syscalls
 * - mck_mckmsr for PMU programmed via mcK pmc_* syscalls
 *
 * LINUX can be any of:
 * - linux_perf for using perf
 * - linux_jevents for using jevents
 * - linux_rawmsr for raw MSR access using /dev/cpu/N/msr
 */

#ifdef JEVENTS_FOUND
using pmu = upca::arch::arch_common_base<upca::arch::x86_64::x86_linux_mckernel<
    upca::arch::x86_64::mck_mckmsr, upca::arch::x86_64::linux_rawmsr>>;
#else
using pmu = upca::arch::arch_common_base<upca::arch::x86_64::x86_64_pmu>;
#endif

#elif defined(__bgq__)

#  include "bgq.h"

using pmu = upca::arch::arch_common_base<upca::arch::bgq::pmu>;

#elif defined(__ppc__) || defined(_ARCH_PPC) || defined(__PPC__)

#  include "ppc.h"

using pmu = upca::arch::arch_common_base<upca::arch::ppc::pmu>;

#else

#error "Unkown/Unsupported architecture"

#endif

max_stars_repo_name: hannesweisbach/ucpa

max_issues_repo_name: hannesweisbach/ucpa

max_forks_repo_name: hannesweisbach/ucpa