#include <adolc.h>
#include <adolc_sparse.h>
#include <iostream>
#include <vector>

#include <iostream>
#include <fstream>
#include <iterator>
#include <random>
#include <cassert>
#include <algorithm>
#include <vector>
#include <fstream>

//using namespace SimTK;
using namespace std;

/// number of inputs/outputs of function F
constexpr int n_in = 1;
constexpr int n_out = 1;


constexpr int NX = 2; // states
constexpr int NR = 2; //


static const int sp_x[] = { NX, 1, 0, NX, 0, 1};
static const int sp_tau[] = { NR, 1, 0, NR, 0, 1};
static const int sp_x_null[] = { NX, 1, 0, 0 };
static const int sp_tau_null[] = { NR, 1, 0, 0 };


/// Total number of input/output nonzeros
const int n = NX;	// number of independent variables (inputs)
							//const int m = NR + NMus + NMus + NMus + NMus; // number of dependent variables (outputs)
const int m = NR; // number of dependent variables (outputs)


static int is_initialised = false;
static int is_initialised_Fadolc = false;

template<typename T>
T value(const adouble& e) {
	return e;
}

template<>
double value(const adouble& e) {
	return e.getValue();
}

adouble  step5(adouble x) {
	adouble out = 10 * pow(x, 3) - 15 * pow(x, 4) + 6 * pow(x, 5);
	return out;
}
adouble stribeck(adouble us, adouble ud,
	adouble uv, adouble vrel) {
	adouble mu_wet = uv*vrel;
	adouble mu_dry;

	adouble mu_dry3 = ud;
	adouble mu_dry2 = us - (us - ud)*step5((vrel - 1) / 2);
	adouble mu_dry1 = us*step5(vrel);

	adouble mu_dry_aux = mu_dry2 + ((1 + tanh(1e7 * (vrel - 3))) / 2)*(mu_dry3 - mu_dry2);
	mu_dry = mu_dry1 + ((1 + tanh(1e7 * (vrel - 1))) / 2)*(mu_dry_aux - mu_dry1);
	adouble mufriction = mu_wet + mu_dry;

	return mufriction;
}


void calcForce(adouble Indentation, adouble IndentationVel, adouble* ft, adouble* fn) {

	adouble radiusSphere_heel = 0.02;
	adouble radiusSphere_front = 0.02;
	adouble stiffness = 1.2654e+06;
	adouble dissipation = 2.0;
	adouble us = 0.8;
	adouble ud = 0.8;
	adouble uv = 0.5;
	adouble vt = 0.1;
	std::vector<adouble> normal(3, 0);
	normal[1] = 1;
	std::vector<adouble> vtangent(3, 0);
	adouble offset = 0.0;
	adouble RadiusSphere = 0.02;

	adouble E = stiffness*pow(0.5, 1.5);
	adouble k = (4.0 / 3.0)*sqrt(RadiusSphere)*E;
	adouble c = dissipation;

	double eps = 1e-16; //1e-8
	double bd = 1500; //bd=1000 //bd=1500
	double bv = 3.5; //bv=0.5;
					 //double bd = 1; //bd=1000
					 //double bv = 1; //bv=0.5;
	double bn = 1;
	double baux = 2000; // 2000

	adouble fp = pow((pow(Indentation, 2) + eps), 3.0 / 4.0);
	adouble fv = (1.0 + (3.0 / 2.0)*c*IndentationVel);

	//Remove negative parts
	adouble fp_nonneg = fp*(1.0 / 2.0 + (1.0 / 2.0)*tanh((1e7)*Indentation));
	adouble fv_nonneg = fv*(1.0 / 2.0 + (1.0 / 2.0)*tanh((1e7)*(IndentationVel + (2.0 / (3.0*c)))));

	//Decreasing exponential when there is no contact
	adouble auxfun_p = (exp((Indentation - 0.01) / 0.1))*bn*(1.0 / 2.0 - (1.0 / 2.0)*tanh(bd*(Indentation - 0.01)));

	//Smooth curves
	fn[0] = k*fp_nonneg*fv_nonneg*(1.0 / 2.0 + (1.0 / 2.0)*tanh(bd*Indentation))*(1.0 / 2.0 + (1.0 / 2.0)*tanh(bv*(IndentationVel + (2.0 / (3.0*c)))));
	//Real fn = k*fp*fv*(1.0 / 2.0 + (1.0 / 2.0)*NTraits<Real>::tanh(bd*Indentation))*(1.0 / 2.0 + (1.0 / 2.0)*NTraits<Real>::tanh(bv*(IndentationVel + (2.0 / (3.0*c)))));

	//Add auxiliar function
	fn[0] = auxfun_p + ((1 + tanh(baux * (Indentation - 0.002))) / 2)*(fn[0] - auxfun_p);
	vector<adouble> force(3, 0);
	force[1] = fn[0];


	//Vec3 vtangent2 = v - vnormal*normal;
	adouble aux = pow(vtangent[0], 2) + pow(vtangent[1], 2) + pow(vtangent[2], 2) + eps;
	adouble vslip = pow(aux, 0.5);
	adouble vrel = vslip / vt;

	adouble mufriction = stribeck(us, ud, uv*vt, vrel);
	ft[0] = fn[0]*mufriction;

	//std::cout << "Indentation=" << Indentation << std::endl;
	//std::cout << " ft= " << ft[0] << " fn=" << fn[0] << std::endl;
}


template<typename T>
int F_generic(T** arg, T** res) {

	// Read inputs
	std::vector<T> x(arg[0], arg[0] + NX);
	/*std::vector<T> u(arg[1], arg[1] + NU);
	std::vector<T> p(arg[2], arg[2] + NP);*/

	if (!is_initialised) {
		/*model_setup();*/
		is_initialised = true;
	}

	adouble Indentation;
	adouble IndentationVel;

	Indentation = x[0];
	IndentationVel = x[1];
	adouble fn[1];
	adouble ft[1];
	calcForce(Indentation, IndentationVel, ft, fn);

	if (res[0]) {
		res[0][0] = value<T>(ft[0]);
		res[0][1] = value<T>(fn[0]);
	}

	return 0;

}

#ifdef __cplusplus
extern "C" {
#endif

int F_adolc(double** arg, double* res) {
	static adouble x[NX];
	static adouble tau[NR];
	
	adouble* adouble_arg[n_in] = { x};
	adouble* adouble_res[n_out] = { tau };


	int out=trace_on(1, 1);
	for (int i = 0; i < NX; ++i) x[i] <<= arg[0][i];
	
	F_generic<adouble>(adouble_arg, adouble_res);

	for (int i = 0; i < NR; ++i) adouble_res[0][i] >>= res[i];
	
	trace_off();

	std::cout << "res in F_adolc=" << res[0] << " " << res[1] << std::endl;
	std::cout << "out trace=" << out << std::endl;

	return 0;
}

int F(double** arg, double** res) {
	F_generic<double>(arg, res);
	if (!is_initialised_Fadolc) {
		F_adolc(arg, *res);
		is_initialised_Fadolc = true;
	}
	return 0;
}

void main(){

	double **arg;
	double **res;
	arg = new double*[1];
	arg[0] = new double[2];
	arg[0][0] = 0.00;
	arg[0][1] = 0.00;
	res = new double*[1];
	res[0] = new double[2];
	double** J;
	J = new double*[2];
	J[0] = new double[2];
	J[1] = new double[2];
	std::cout << "F_ADOLC initialized=" << is_initialised_Fadolc << std::endl;

	double x[n]; // input  (independent variables)

	////// No error
	for (int i = 0; i<NX; ++i) x[i] = arg[0][i];
	F(arg,res);
	std::cout << "\nres[0,0]="<< res[0][0] << " " << res[0][1] << std::endl;
	
	int out = jacobian(1, m, n, x, J);
	std::cout << "out jacobian= " << out << std::endl;
	std::cout << "Jacobian=" << std::endl;
	std::cout << J[0][0] << " " << J[0][1] << std::endl;
	std::cout << J[1][0] << " " << J[1][1] << std::endl;
	std::cout << "F_ADOLC initialized=" << is_initialised_Fadolc << std::endl;

	////// No error
	arg[0][0] = 0.100;
	for (int i = 0; i<NX; ++i) x[i] = arg[0][i];
	F(arg, res);
	std::cout << "\nres[0.1,0]=" << res[0][0] << " " << res[0][1] << std::endl;

	out = jacobian(1, m, n, x, J);
	std::cout << "out jacobian= " << out << std::endl;
	std::cout << "Jacobian=" << std::endl;
	std::cout << J[0][0] << " " << J[0][1] << std::endl;
	std::cout << J[1][0] << " " << J[1][1] << std::endl;
	std::cout << "F_ADOLC initialized=" << is_initialised_Fadolc << std::endl;

	////// 	Error when F_adolc is not evaluated
	arg[0][0] = 1.00;
	F(arg, res);
	std::cout << " \n res[1.00,0]=" << res[0][0] << " " << res[0][1] << std::endl;

	for (int i = 0; i<NX; ++i) x[i] = arg[0][i];
	std::cout << "x=" << x[0] << " " << x[1] << std::endl;
	out = jacobian(1, m, n, x, J);
	std::cout << "out jacobian=" << out << std::endl;
	std::cout << "Jacobian=" << std::endl;
	std::cout << J[0][0] << " " << J[0][1] << std::endl;
	std::cout << J[1][0] << " " << J[1][1] << std::endl;
	std::cout << "F_ADOLC initialized=" << is_initialised_Fadolc << std::endl;

	////// No error if evaluating F_adolc
	arg[0][0] = 1.00;
	F(arg, res);
	std::cout << " \n res[1,0]=" << res[0][0] << " " << res[0][1] << std::endl;

	for (int i = 0; i<NX; ++i) x[i] = arg[0][i];
	F_adolc(arg, *res);
	out = jacobian(1, m, n, x, J);
	std::cout << "out jacobian=" << out << std::endl;
	std::cout << "Jacobian=" << std::endl;
	std::cout << J[0][0] << " " << J[0][1] << std::endl;
	std::cout << J[1][0] << " " << J[1][1] << std::endl;
	std::cout << "F_ADOLC initialized=" << is_initialised_Fadolc << std::endl;

}

#ifdef __cplusplus
} /* extern "C" */
#endif