#include <v_lib.h>

vec_t rk4delta(vec_t x0,vec_t y0,vec_t h,vec_t (*right_side)(vec_t x,vec_t y))
{
	static vec_t one_by_three = (vec_t)1.0 / (vec_t)3.0;
	static vec_t one_by_six = (vec_t)1.0 / (vec_t)6.0;
	vec_t k1, k2, k3, k4, halfstep = 0.5*h;
	k1 = h * right_side(x0,y0);
	x0 += halfstep;
	k2 = h * right_side(x0,y0 + k1 * 0.5);
	k3 = h * right_side(x0,y0 + k2 * 0.5);
	x0 += halfstep;
	k4 = h * right_side(x0,y0 + k3);
	return (k1 + k4) * one_by_six + (k2 + k3) * one_by_three;
}

void rk4solve(vec_t x0,vec_t y0,vec_t xn,u32_t n,vec_t (*right_side)(vec_t x,vec_t y),vec_t ax_ret[],vec_t ay_ret[])
{
	u32_t i;
	vec_t step = (xn-x0) / n;
	ax_ret[0]=x0;
	ay_ret[0]=y0;
	for(i=1;i<=n;i++)
	{
		y0 += rk4delta(x0,y0,step,right_side);
		x0 += step;
		ax_ret[i]=x0;
		ay_ret[i]=y0;
	}
}

/*
 * Helper routines
 */
void dxf_line2(vhandle_t dxf,vec_t x1,vec_t y1,vec_t x2,vec_t y2)
{
	vec3_t p1, p2;
	vec3Set(p1,x1,y1,0);
	vec3Set(p2,x2,y2,0);
	dxfLine(dxf,p1,p2);
}

void output_arrays(vhandle_t dxf,vec_t ax[],vec_t ay[],int n)
{
	int i;
	vec3_t p1,p2;
	for(i=0;i<n;i++)
	{
		vec3Set(p1,ax[i],ay[i],0);
		vec3Set(p2,ax[i+1],ay[i+1],0);
		dxfLine(dxf,p1,p2);
	}
}

/*
 * Specific task:
 */	
vec_t ax[1024 * 128],ay[1024 * 128];
vec_t right_side(vec_t x,vec_t y)
{
	return - 2 * x * y; /* y = c * exp(-x^2) */
}

#define NUM_CURVES	4
vec2_t	ic[NUM_CURVES] = {
	{0,-4},
	{0,-2},
	{0,2},
	{0,4}
};

#define N			200
#define MAX_X		4

int main(void)
{
	vhandle_t out;
	int curve;
	out=dxfOpenWrite("out.dxf");
	for(curve=0;curve<NUM_CURVES;curve++)
	{
		rk4solve(ic[curve][0],ic[curve][1],MAX_X,N,right_side,ax,ay);
		output_arrays(out,ax,ay,N);
		rk4solve(ic[curve][0],ic[curve][1],-MAX_X,N,right_side,ax,ay);
		output_arrays(out,ax,ay,N);
	}
	dxf_line2(out,0,-5,0,5);
	dxf_line2(out,-MAX_X-1,0,MAX_X+1,0);
	
	dxfClose(out);
	return 0;
}
