/*
build:
export CFLAGS
gcc $CFLAGS -Wall -Wextra -o top-w-friction top-w-friction.c -lm -lgsl -lgslcblas

gcc $CFLAGS -Wall -Wextra -o top-w-friction top-w-friction.c -lm -lgsl -lcblas -latlas

gcc $CFLAGS -Wall -Wextra -o top-w-friction top-w-friction.c -lm -lgsl -lptcblas -latlas

usage: 20 parameters

top-w-friction m g radius lambda-t lambda-a L3 muk psi' theta' phi' psi theta phi x1' x2' x1 x2 t0 tmax tinc > output.csv

top-w-friction 0.008 9.81 0.02 0.0000008 0.0000016 0.02 0.01 \
 6.28318530717958647692 0 0 0 0.17453292519943295769 0 0.0 0.09 0.0 0.0 0.0 8.0 0.001 > top.csv

*/

#define _GNU_SOURCE
#define HAVE_INLINE
#define GSL_RANGE_CHECK_OFF
#define GSL_DISABLE_DEPRECATED

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>

// global structure for GSL function parameters

typedef struct parameters {
double m;
double g;
double rad;
double lambdat;
double lambdaa;
double L3;
double muk;
gsl_matrix *Msym;
gsl_vector *Fsym;
gsl_vector *ydot;
gsl_permutation *lupermute;
} parameters;

/* ------------------------------------------------ */
/* --------- GSL function for ODE system ---------- */
/* ------------------------------------------------ */

int ode_eqn (double t, const double y[], double dydt[], void *params)
{ 
// cast void pointer to type parameters
double m = ((parameters *)params)->m;
double g = ((parameters *)params)->g;
double rad = ((parameters *)params)->rad;
double lambdat = ((parameters *)params)->lambdat;
double lambdaa = ((parameters *)params)->lambdaa;
double L3 = ((parameters *)params)->L3;
double muk = ((parameters *)params)->muk;
gsl_matrix *M = ((parameters *)params)->Msym;
gsl_vector *F = ((parameters *)params)->Fsym;
gsl_vector *ydot = ((parameters *)params)->ydot;
gsl_permutation *perm = ((parameters *)params)->lupermute;

static double one=1.0, two=2.0;
int i, s;
//pre-determine some repeated values
double sinps;
double sinth;
double sinph;
double cosps;
double costh;
double cosph;

sincos(y[3], &sinps, &cosps);
sincos(y[4], &sinth, &costh);
sincos(y[5], &sinph, &cosph);

// define functions
// M * ydot = F
// the mass matrix M
gsl_matrix_set_zero(M);
gsl_matrix_set(M, 0, 0, -lambdat*sinth*(pow(sinph,two)*two-one));
gsl_matrix_set(M, 0, 1, -two*lambdat*cosph*sinph-L3*cosph*(L3*m*sinph*pow(sinth,two)+L3*m*muk*sinth*(cosph*cosps-costh*sinph*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1])-L3*m*muk*sinth*(cosph*sinps+cosps*costh*sinph)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1]))-L3*sinph*(L3*m*cosph*pow(sinth,two)-L3*m*muk*sinth*(cosps*sinph+cosph*costh*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1])+L3*m*muk*sinth*(sinph*sinps-cosph*cosps*costh)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])));
gsl_matrix_set(M, 1, 0, -two*lambdat*cosph*sinph*sinth);
gsl_matrix_set(M, 1, 1, -lambdat*pow(cosph,two)+lambdat*pow(sinph,two)-L3*cosph*(L3*m*cosph*pow(sinth,two)-L3*m*muk*sinth*(cosps*sinph+cosph*costh*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1])+L3*m*muk*sinth*(sinph*sinps-cosph*cosps*costh)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1]))+L3*sinph*(L3*m*sinph*pow(sinth,two)+L3*m*muk*sinth*(cosph*cosps-costh*sinph*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1])-L3*m*muk*sinth*(cosph*sinps+cosps*costh*sinph)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])));
gsl_matrix_set(M, 2, 0, costh);
gsl_matrix_set(M, 2, 2, one);
gsl_matrix_set(M, 3, 3, one);
gsl_matrix_set(M, 4, 4, one);
gsl_matrix_set(M, 5, 5, one);
gsl_matrix_set(M, 6, 1, L3*m*muk*sinth*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]));
gsl_matrix_set(M, 6, 6, m);
gsl_matrix_set(M, 7, 1, -L3*m*muk*sinth*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1]));
gsl_matrix_set(M, 7, 7, m);
gsl_matrix_set(M, 8, 8, one);
gsl_matrix_set(M, 9, 9, one);

// M * ydot = F
// 
gsl_vector_set(F, 0, -sinph*(L3*(m*cosph*sinth*(g-L3*costh*pow(y[1],two))+m*muk*(sinph*sinps-cosph*cosps*costh)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])-m*muk*(cosps*sinph+cosph*costh*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]))+lambdaa*(y[2]+costh*y[0])*(sinph*y[1]-cosph*sinth*y[0]))-L3*cosph*(m*sinph*sinth*(g-L3*costh*pow(y[1],two))-m*muk*(cosph*sinps+cosps*costh*sinph)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])+m*muk*(cosph*cosps-costh*sinph*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]))+lambdat*costh*y[0]*y[1]*two+lambdaa*pow(cosph,two)*y[2]*y[1]+lambdaa*pow(cosph,two)*costh*y[0]*y[1]-lambdat*pow(cosph,two)*costh*y[0]*y[1]*4.0+lambdaa*cosph*costh*sinph*sinth*pow(y[0],two)-lambdat*cosph*costh*sinph*sinth*pow(y[0],two)*two+lambdaa*cosph*sinph*sinth*y[2]*y[0]);
gsl_vector_set(F, 1, -cosph*(L3*(m*cosph*sinth*(g-L3*costh*pow(y[1],two))+m*muk*(sinph*sinps-cosph*cosps*costh)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])-m*muk*(cosps*sinph+cosph*costh*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]))+lambdaa*(y[2]+costh*y[0])*(sinph*y[1]-cosph*sinth*y[0]))-sinph*(lambdaa*cosph*y[2]*y[1]+lambdaa*cosph*costh*y[0]*y[1]-lambdat*cosph*costh*y[0]*y[1]*two+lambdaa*costh*sinph*sinth*pow(y[0],two)-lambdat*costh*sinph*sinth*pow(y[0],two)+lambdaa*sinph*sinth*y[2]*y[0])+L3*sinph*(m*sinph*sinth*(g-L3*costh*pow(y[1],two))-m*muk*(cosph*sinps+cosps*costh*sinph)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1])+m*muk*(cosph*cosps-costh*sinph*sinps)*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]))+lambdat*cosph*costh*y[0]*(sinph*y[1]*two-cosph*sinth*y[0]));
gsl_vector_set(F, 2, sinth*y[0]*y[1]);
gsl_vector_set(F, 3, y[0]);
gsl_vector_set(F, 4, y[1]);
gsl_vector_set(F, 5, y[2]);
gsl_vector_set(F, 6, m*muk*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1]));
gsl_vector_set(F, 7, -m*muk*one/sqrt(pow(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1],two)+pow(-y[6]+L3*cosps*sinth*y[0]+L3*costh*sinps*y[1],two))*(g-L3*costh*pow(y[1],two))*(y[7]-L3*sinps*sinth*y[0]+L3*cosps*costh*y[1]));
gsl_vector_set(F, 8, y[6]);
gsl_vector_set(F, 9, y[7]);

// solve for ydot

gsl_linalg_LU_decomp (M, perm, &s);
gsl_linalg_LU_solve (M, perm, F, ydot);

for(i=0; i<=9; i++)
{ dydt[i]=gsl_vector_get(ydot, i); }

return GSL_SUCCESS; 
}
/* ------------------------------------------------ */
/* ------ end of GSL function for ODE system ------ */
/* ------------------------------------------------ */


/* -------------------------------------------------------- */
/*                  main()                                  */
/* -------------------------------------------------------- */

int main (int argc, char **argv)
{ 

// initial values for system in t0, y[]

double t, tinc, ti, t0, tmax, y[10];
// vars/parameters for GSL ODE solver
double hstart, eps_abs, eps_rel, a_y, a_dydt, scale_abs[10]; 
// euler angles to coord. vectors per rotations.berkeley.edu
double e1x, e1y, e1z, e2x, e2y, e2z, e3x, e3y, e3z; 
// center-of-mass position and position/velocity of contact point
double x3, x3dot, xP1, xP2, xP3, vPx, vPy, vP;
// angular velocity components
double omega1, omega2, omega3;
// Total Energy, rotational energy, kinetic energy of C.M., potential energy
double ET, ER, EK, EP;
int status;

// global structure params contains ODE system parameters, coefficients
parameters params;

// gsl_odeiv2_system sys is defined below
gsl_odeiv2_driver *drv;

// read initial conditions, time steps, and parameters
// from command line
params.m=atof(argv[1]);
params.g=atof(argv[2]);
params.rad=atof(argv[3]);
params.lambdat=atof(argv[4]);
params.lambdaa=atof(argv[5]);
params.L3=atof(argv[6]);
params.muk=atof(argv[7]);

// state vector y
// psi', theta', phi', psi, theta, phi, x1', x2', x1, x2

//initial conditions
y[0]=atof(argv[8]); // psi' precession rate
y[1]=atof(argv[9]); // theta' nutation rate
y[2]=atof(argv[10]); // phi' spin rate
y[3]=atof(argv[11]); // psi 
y[4]=atof(argv[12]); // theta
y[5]=atof(argv[13]); // phi
y[6]=atof(argv[14]); // x1'
y[7]=atof(argv[15]); // x2'
y[8]=atof(argv[16]); // x1
y[9]=atof(argv[17]); // x2

// time steps
t0=atof(argv[18]);
tmax=atof(argv[19]);
tinc=atof(argv[20]);

/* ---------- set up GSL solvers ------------ */
gsl_matrix *Msym = gsl_matrix_alloc(10, 10);
gsl_vector *Fsym = gsl_vector_alloc(10);
gsl_vector *ydot = gsl_vector_alloc(10);
gsl_permutation *lupermute = gsl_permutation_alloc(10);

params.Msym = Msym;
params.Fsym = Fsym;
params.ydot = ydot;
params.lupermute = lupermute;
gsl_odeiv2_system sys = { ode_eqn, NULL, 10, &params };

hstart=1e-6;
eps_abs=1e-10;
eps_rel=1e-10;
a_y=1.0;
a_dydt=0.0;

// these weight values allow fine tuning of the convergence but
// we leave them at 1.0 here
scale_abs[0]=1.0;
scale_abs[1]=1.0;
scale_abs[2]=1.0;
scale_abs[3]=1.0;
scale_abs[4]=1.0;
scale_abs[5]=1.0;
scale_abs[6]=1.0;
scale_abs[7]=1.0;
scale_abs[8]=1.0;
scale_abs[9]=1.0;

// ode solver driver
drv = gsl_odeiv2_driver_alloc_scaled_new(&sys, gsl_odeiv2_step_rk8pd, hstart, eps_abs, eps_rel, a_y, a_dydt, scale_abs);

/* --- end set up GSL solvers --- */

// output header
fprintf(stdout, "#%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n", \
		 "t", "Dpsi", "Dtheta", "Dphi", "psi", "theta", "phi", "Dx1", "Dx2", "Dx3", "x1", "x2", "x3", "xP1", "xP2", "xP3", "vP", \
		 "e1x", "e1y", "e1z", "e2x", "e2y", "e2z", "e3x", "e3y", "e3z", "omega1", "omega2", "omega3", "ET", "ER", "EK");

// using the given initial conditions, calculate all coords and vectors
// center of mass vertical
x3=params.L3*cos(y[4]);
x3dot = -params.L3*sin(y[4])*y[1];

// convert 3-1-3 Euler angles
// e1 = inverse(R313).transpose(E1)
e1x=cos(y[5])*cos(y[3])-cos(y[4])*sin(y[5])*sin(y[3]);
e1y=cos(y[5])*sin(y[3])+cos(y[3])*cos(y[4])*sin(y[5]);
e1z=sin(y[5])*sin(y[4]);

// e2 = inverse(R313).transpose(E2)
e2x=-cos(y[3])*sin(y[5])-cos(y[5])*cos(y[4])*sin(y[3]);
e2y=cos(y[5])*cos(y[3])*cos(y[4])-sin(y[5])*sin(y[3]);
e2z=cos(y[5])*sin(y[4]);

// e3 = inverse(R313).transpose(E3)
e3x=sin(y[3])*sin(y[4]);
e3y=-cos(y[3])*sin(y[4]);
e3z=cos(y[4]);

omega1=y[0]*sin(y[4])*sin(y[5]) + y[1]*cos(y[5]);
omega2=y[0]*sin(y[4])*cos(y[5]) - y[1]*sin(y[5]);
omega3=y[0]*cos(y[4]) + y[2];

// calc energy
EK = 0.5*params.m*(y[6]*y[6] + y[7]*y[7] + x3dot*x3dot);
ER = 0.5*(params.lambdat*omega1*omega1 + params.lambdat*omega2*omega2 + params.lambdaa*omega3*omega3) + params.m*params.g*x3; // last term is potential energy
EP = params.m*params.g*x3;
ET = EK + ER + EP;

// contact point, P
xP1=y[8] - params.L3*e3x;
xP2=y[9] - params.L3*e3y;
xP3=x3 - params.L3*e3z;

// contact point velocity, vP
vPx = y[6] - params.L3*cos(y[3])*sin(y[4])*y[0] - params.L3*sin(y[3])*cos(y[4])*y[1];
vPy = y[7] - params.L3*sin(y[3])*sin(y[4])*y[0] + params.L3*cos(y[3])*cos(y[4])*y[1];
vP = sqrt(vPx*vPx + vPy*vPy);

// output initial conditions 
fprintf(stdout, "%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,", \
		 t0, y[0], y[1], y[2], y[3], y[4], y[5], y[6], y[7], x3dot, y[8], y[9]);

// output initial computed values 
fprintf(stdout, "%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e\n", \
		 x3, xP1, xP2, xP3, vP, e1x, e1y, e1z, e2x, e2y, e2z, e3x, e3y, e3z, omega1, omega2, omega3, ET, ER, EK);

// set up times
t=t0; ti=t0+tinc;

/* ------------------------------ */
/* simulation loop from t to tmax */
/* ------------------------------ */

do {

status = gsl_odeiv2_driver_apply(drv, &t, ti, y);
if (status != GSL_SUCCESS)
{
fprintf(stderr, "Error, return value=%d %s\n", status, gsl_strerror(status));
break;
}
 
// using the simulated values at ti, calculate all coords and vectors
// center of mass vertical
x3=params.L3*cos(y[4]);
x3dot = -params.L3*sin(y[4])*y[1];

// convert 3-1-3 Euler angles
e1x=cos(y[5])*cos(y[3])-cos(y[4])*sin(y[5])*sin(y[3]);
e1y=cos(y[5])*sin(y[3])+cos(y[3])*cos(y[4])*sin(y[5]);
e1z=sin(y[5])*sin(y[4]);

e2x=-cos(y[3])*sin(y[5])-cos(y[5])*cos(y[4])*sin(y[3]);
e2y=cos(y[5])*cos(y[3])*cos(y[4])-sin(y[5])*sin(y[3]);
e2z=cos(y[5])*sin(y[4]);

e3x=sin(y[3])*sin(y[4]);
e3y=-cos(y[3])*sin(y[4]);
e3z=cos(y[4]);

omega1=y[0]*sin(y[4])*sin(y[5]) + y[1]*cos(y[5]);
omega2=y[0]*sin(y[4])*cos(y[5]) - y[1]*sin(y[5]);
omega3=y[0]*cos(y[4]) + y[2];

omega1=y[0]*sin(y[4])*sin(y[5]) + y[1]*cos(y[5]);
omega2=y[0]*sin(y[4])*cos(y[5]) - y[1]*sin(y[5]);
omega3=y[0]*cos(y[4]) + y[2];

// calc energy
EK = 0.5*params.m*(y[6]*y[6] + y[7]*y[7] + x3dot*x3dot);
ER = 0.5*(params.lambdat*omega1*omega1 + params.lambdat*omega2*omega2 + params.lambdaa*omega3*omega3);
EP = params.m*params.g*x3;
ET = EK + ER + EP;

// contact point, P
xP1=y[8] - params.L3*e3x;
xP2=y[9] - params.L3*e3y;
xP3=x3 - params.L3*e3z;

// contact point velocity, vP
vPx = y[6] - params.L3*cos(y[3])*sin(y[4])*y[0] - params.L3*sin(y[3])*cos(y[4])*y[1];
vPy = y[7] - params.L3*sin(y[3])*sin(y[4])*y[0] + params.L3*cos(y[3])*cos(y[4])*y[1];
vP = sqrt(vPx*vPx + vPy*vPy);

// Output returned values at ti
fprintf(stdout, "%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,", \
		 t, y[0], y[1], y[2], y[3], y[4], y[5], y[6], y[7], x3dot, y[8], y[9]);

// output computed values 
fprintf(stdout, "%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e,%.5e\n", \
		 x3, xP1, xP2, xP3, vP, e1x, e1y, e1z, e2x, e2y, e2z, e3x, e3y, e3z, omega1, omega2, omega3, ET, ER, EK);

ti=t+tinc;
}
while(ti<=tmax);
// end main simulation loop

gsl_odeiv2_driver_free(drv);
gsl_matrix_free(Msym);
gsl_vector_free(Fsym);
gsl_vector_free(ydot);
gsl_permutation_free(lupermute);
return 0;
}