using Plots
#using SpectialFunctions


#-------------------- définition du modèle --------------------------

function potential(x)
	return (x[1])^2 + (x[2])^2;
end

function forces(x,xp)
	xp[1] = 2*x[1];
	xp[2] = 2*x[2];
end

function epsilon(x,p)
	se = p[1];
	type = p[3];
	if type == 1					# eps(x) = scaleps
		return se;
	elseif type == 2				# eps(x) = scaleps * |grad U|
		(dim,)=size(x);
		fo = zeros(dim,1);
		forces(x,fo);
		return se*sqrt((fo'*fo)[1]);
	else						# eps(x) = scaleps / |grad U|
		(dim,)=size(x);
		fo = zeros(dim,1);
		forces(x,fo);
		return se/sqrt((fo'*fo)[1]);
	end
end

function ratebound(x,v,p)
	M = 2;				#borne sur la hessienne
	scaleps = p[1];
	sigma = p[2];
	type = p[3];
	(dim,)=size(x);
	fo = zeros(dim,1);
	forces(x,fo);
	if type == 1
		a = max(0,(v'*fo)[1]) + sigma*sqrt((fo'*fo)[1])/(sqrt(2*pi)*scaleps);
		b = M*((v'*v)[1]+sigma*sqrt((v'*v)[1])/(sqrt(2*pi)*scaleps));
		c = 0;
	elseif type == 2
		a = max(0,(v'*fo)[1]) + sigma/(sqrt(2*pi)*scaleps);
		b = M*(v'*v)[1];
		c = 0;
	else
		a = max(0,(v'*fo)[1]) + sigma*((fo'*fo)[1])/(sqrt(2*pi)*scaleps);
		b = M*((v'*v)[1]+2*sigma*sqrt((v'*v)[1])/(sqrt(2*pi)*scaleps));
		c = M^2*((v'*v)[1])*sigma/(sqrt(2*pi)*scaleps);	
	end
	return(a,b,c);
end

#-------------------- fonctions auxiliaires fixes --------------------------

function erf(x)
	err = 0.0001;
	if x>3
		return 1;
	elseif x<-3
		return -1;
	else 
		n = 0;
		y = x;
		a = -(x^3)/3;
		while abs(a) > 2*err
			n = n+1;
			y = y + a;
			a = -(x^2)*a*(2*n+1)/((2*n+3)*(n+1));
		end
		return 2*y/sqrt(pi);
	end
end

function frate(m)
	m * (erf(m/sqrt(2))+1)/2 + exp(-m^2/2)/sqrt(2*pi);
end

function proposal(a,b,c)
	h = -log.(rand(3,1));
	v1 = h[1]/a;
	v2 = sqrt(2*h[2]/b);
	v3 = cbrt(3*h[3]/c);
	return min(v1,v2,v3);
end

function sampleVelocity(m)
	reject = true;
	prop = 0;
  	if m > 0
		while(reject)
			alea = rand(4,1);
			if (alea[1] < m/(m +1/sqrt(2*pi)))
				prop = sqrt(-2*log.(alea[2])) * cos(2*pi*alea[3]);
			else
				prop = sqrt(-2*log.(alea[2]));
			end
			proba = max(0,m+prop)/(m+max(0,prop));
			if (alea[4] < proba)
				reject = false;
			end
		end
	elseif m > -10				#trop long sinon (de toutes façons, G converge vers -m quand m->-infty)
		while(reject)
			alea = rand(2,1);
			prop = sqrt(m^2 -2*log.(alea[1]));
			proba = (m + prop)/prop;
			if (alea[2] < proba) 
				reject = false;
			end
		end
	else
		alea = rand(1,1);
		prop = sqrt(m^2 -2*log.(alea[1]));
	end
	return prop;
end

#-------------------- transition --------------------------

function transition(x,v,i,p)
	scaleps = p[1];
	sigma = p[2];
	(dim,) = size(x);
	(a,b,c) = ratebound(x[:,i],v[:,i],p);
	s = proposal(a,b,c);
	x[:,i+1] = x[:,i] + s*v[:,i];
	fo = zeros(dim,1);
	forces(x[:,i+1],fo);
	nfo = sqrt(fo'*fo)[1];
	if nfo == 0
		v[:,i+1] = v[:,i];
	else
		eps = epsilon(x[:,i+1],p);
		m = eps * (fo'*v[:,i])[1] / (nfo * sigma) ;
		jumprate = sigma*nfo/eps*frate(m);
		bound = a + b*s + c*s^2;
		u = rand(1,1);
		if (u[1] < jumprate/bound)
			v[:,i+1] = v[:,i] - 2*eps*sigma*(m+sampleVelocity(m))/(nfo*(1+eps^2))*fo;
		else
			v[:,i+1] = v[:,i];
		end
	end
	return 0;
end


function verlet(x,v,i,dt)
	z = x[:,i] + dt*v[:,i]/2;
	fo = zeros(dim,1);
	forces(z,fo);
	v[:,i+1] = v[:,i] - dt*fo;
	x[:,i+1] = z + dt*v[:,i]/2;
	return 0;	
end

#-------------------- Main --------------------------

function Jump(njump,scaleps)
	dim = 2;
	sigma = 1;
	type = 1;
	p=[scaleps,sigma,type];
	x = zeros(dim,njump);
	x[1,1]=1;
	v = zeros(dim,njump);
	v[:,1]=ones(dim,1);
	for i=1:(njump-1)
		transition(x,v,i,p);
	end
	return plot(x[1,:],x[2,:],leg=false,lw=1,xlabel="x1",xlims=(-1.5,1.5),ylabel="x2",ylims=(-1,1),title=string("eps = ",string(scaleps),", n = ",string(njump)));
end

function Jumpp(njump,scaleps)
	png(string("jump",string(scaleps),"-",string(njump)));
end

function Hamil(niter,deltat)
	dim = 2;
	x = zeros(dim,niter);
	x[1,1]=1;
	v = zeros(dim,niter);
	v[:,1]=ones(dim,1);
	for i=1:(niter-1)
		verlet(x,v,i,deltat);
	end
	return plot(x[1,:],x[2,:],leg=false,lw=1,xlabel="x1",xlims=(-1.5,1.5),ylabel="x2",ylims=(-1,1),title=string("Verlet, delta = ",string(deltat),", n = ",string(niter)));
end

function Hamilp(niter,deltat)
	png(string("hamil",string(deltat),"-",string(niter)));
end