#include <v_lib.h>

#error Gradient descent is BROKEN

#define	MAX_LAYERS				4
#define MAX_NODES_ON_LAYER		64
#define	I_DIM					3

u32_t	num_layers	= 3;
u32_t	num_nodes[MAX_LAYERS] = {I_DIM,5,1};
u32_t	nweights = 0; /* will be calculated later */

/*
 * Initial state: center of the unit volume in weight space
 */
vec_t	wts_all[MAX_LAYERS * MAX_NODES_ON_LAYER * MAX_NODES_ON_LAYER] = 
{
	0.5,0.5,0.5,
	
	0.5,0.5,0.5,	0.5,0.5,0.5,	0.5,0.5,0.5,	0.5,0.5,0.5,	0.5,0.5,0.5,
	
	0.5,0.5,0.5,0.5,0.5
};

/*
 * Next best point in weight space
 */
vec_t	wts_better[MAX_LAYERS * MAX_NODES_ON_LAYER * MAX_NODES_ON_LAYER];

/*
 * Pointers to weight subarrays of every node in every layer
 */
vec_p	i_w[MAX_LAYERS][MAX_NODES_ON_LAYER];

void iw_setup(void)
{
	u32_t i,j;
	vec_t *ptr = wts_all;
	/*
	 * Input layer: 1 weight per node
	 */
	for(j=0;j<num_nodes[0];j++)
	{
		i_w[0][j] = ptr;
		ptr++;
	}
	/*
	 * N'th layer: num_nodes[N-1] weights per node
	 */
	for(i=1;i<num_layers;i++)
	{
		for(j=0;j<num_nodes[i];j++)
		{
			i_w[i][j] = ptr;
			ptr += num_nodes[i-1];
		}
	}
	nweights = ptr - wts_all;
}

#define	RHO	4

vec_t activate(vec_t input)
{
	return 1/(1+exp(-input / RHO));
}

vec_t propagate(vec_t *inputs)
{
	u32_t i,j,k;
	/*
	 * Cumulative network output
	 */
	vec_t netout[MAX_NODES_ON_LAYER];
	vec_t netout_prev[MAX_NODES_ON_LAYER];
	/*
	 * Input layer
	 */
	for(j=0;j<num_nodes[0];j++)
		netout_prev[j] = activate(i_w[0][j][0] * inputs[j]);
	/*
	 * N'th layer
	 */
	for(i=1;i<num_layers;i++)
	{
		for(j=0;j<num_nodes[i];j++)
		{
			netout[j] = 0;
			for(k=0;k<num_nodes[i-1];k++)
				netout[j] += i_w[i][j][k] * netout_prev[k];
			netout[j] = activate(netout[j]);
		}
		memCopy(netout_prev,netout,num_nodes[i] * sizeof(vec_t));
	}
	return netout_prev[0];
}

vec_t bestz;
u32_t deep;

void z_bestpoint(vec_t *inputs,vec_t desired,vec_t zstep)
{
	int j;
	u32_t i;
	vec_t errf, errf_min;
	vec_t curz = propagate(inputs);
	bestz = curz;
	deep = 0;
	errf_min = v_abs(bestz - desired);
	memCopy(wts_better,wts_all,nweights * sizeof(vec_t));
	for(i=0;i<nweights;i++)
	{
		for(j=-1;j<=2;j+=3)
		{
			wts_all[i] += zstep * j;
			if(wts_all[i] < 0 || wts_all[i]>1)	/* clip to unit volume */
				continue;
			curz = propagate(inputs);
			errf = v_abs(curz - desired);
			if(errf < errf_min)
			{
				bestz = curz;
				memCopy(wts_better,wts_all,nweights * sizeof(vec_t));
				errf_min = errf;
				deep = i;
			}
		}
		wts_all[i] -= zstep; /* return wts_all to previuos state */
	}
}

vboolean z_same()
{
	u32_t i;
	for(i=0;i<nweights;i++)
		if(!IsZero(wts_all[i]-wts_better[i]))
			return false;
	return true;
}

#define MAX_ITER		(1024 * 64)
		
void backprop(vec_t *inputs,vec_t desired)
{
	u32_t num_iter, sq, max_sq = 5;
	vec_t zstep = 0.0005;
	vec_t bestz_prev = v_abs(propagate(inputs) - desired);
	for(sq=0;sq<max_sq;sq++)
	{
		for(num_iter=0;num_iter < MAX_ITER;num_iter++)
		{
			z_bestpoint(inputs,desired,zstep);
			if(z_same())
				break;
			memCopy(wts_all,wts_better,nweights * sizeof(vec_t));
		}
		printf("backprop(): sq %u; %u iter (deep %u,nw %u); from %f to %f (desired %f)\n",
			sq,num_iter,deep,nweights,bestz_prev,bestz,desired);
		bestz_prev = bestz;
		zstep *= .5;
	}
}

#define NUM_PATTERNS	8

vec_t tset[NUM_PATTERNS][I_DIM] = {
	{1,1,1},
	{-1,1,1},
	{1,-1,1},
	{-1,-1,1},
	{1,1,-1},
	{-1,1,-1},
	{1,-1,-1},
	{-1,-1,-1}
};

vec_t ts_desired[NUM_PATTERNS] = {.7,.7,.2,.9,.2,.4,.8,.1};

vec_t errf_multi()
{
	u32_t i;
	vec_t dz, esum = 0;
	for(i=0;i<NUM_PATTERNS;i++)
	{
		dz = propagate(tset[i]) - ts_desired[i];
		esum += v_abs(dz);
	}
	return esum;
}

vec_t errf_min;

void z_bestpoint_multi(vec_t zstep)
{
	int j;
	u32_t i;
	vec_t errf;
	deep = 0;
	errf_min = errf_multi();
	memCopy(wts_better,wts_all,nweights * sizeof(vec_t));
	for(i=0;i<nweights;i++)
	{
		for(j=-1;j<=2;j+=3)
		{
			wts_all[i] += zstep * j;
			if(wts_all[i] < 0 || wts_all[i]>1)	/* clip to unit volume */
				continue;
			errf = errf_multi();
			if(errf < errf_min)
			{
				memCopy(wts_better,wts_all,nweights * sizeof(vec_t));
				errf_min = errf;
				deep = i;
			}
		}
		wts_all[i] -= zstep; /* return wts_all to previous state */
	}
}

/*

void next_point(u32_t np,u32_t nw,vec_t *pt_ret)
{
	int i, r, r1 = np;
	for(i=0;i<nw;i++)
	{
		r = 
	}
	
void try_all()
{

}

*/

void backprop_multi()
{
	u32_t num_iter, sq, max_sq = 5;
	vec_t zstep = 0.005;
	vec_t errf_prev = errf_multi();
	for(sq=0;sq<max_sq;sq++)
	{
		for(num_iter=0;num_iter < MAX_ITER;num_iter++)
		{
			z_bestpoint_multi(zstep);
			if(z_same())
				break;
			memCopy(wts_all,wts_better,nweights * sizeof(vec_t));
		}
		printf("backprop(): sq %u; %u iter (deep %u,nw %u); erf from %f to %f\n",
			sq,num_iter,deep,nweights,errf_prev,errf_min);
		zstep *= .5;
	}
}


#define	NPAT	3
vec3_t patterns[NPAT] = {{1,0,0},{0,1,0},{0,0,1}};
vec_t desired[NPAT] = {.2,.8,.3};

int rnddev;
void rnd_setup()
{
	rnddev=open("/dev/random",0);
}

int v_rand(int num)
{
	unsigned short w;
	read(rnddev,&w,2);
	return w % num;
}

int main()
{
	int i, niter = 12, pat;
	
	iw_setup();
	rnd_setup();
	
	printf("main(): before training:\n");
	for(pat=0;pat<NPAT;pat++)
		printf("pattern %u: result %f (desired %f)\n",pat,propagate(patterns[pat]),desired[pat]);
	fflush(stdout);
	
	for(i=0;i<niter;i++)
	{
		pat = v_rand(NPAT);
		printf("running backprop() for pattern %u on iteration %u ...\n",pat,i);
		fflush(stdout);
		backprop(patterns[pat],desired[pat]);
	}
		
	printf("main(): after training:\n");
	for(pat=0;pat<NPAT;pat++)
		printf("pattern %u: result %f (desired %f)\n",pat,propagate(patterns[pat]),desired[pat]);

	return 0;
}


#if 0

int main()
{
	u32_t i;

	iw_setup();

	for(i=0;i<NUM_PATTERNS;i++)
		printf("main(): before training: %f, desired %f\n",propagate(tset[i]),ts_desired[i]);

	backprop_multi();

	for(i=0;i<NUM_PATTERNS;i++)
		printf("main(): before training: %f, desired %f\n",propagate(tset[i]),ts_desired[i]);
	
	return 0;
}

int main()
{
	int i,j,k;
	iw_setup();
	printf("nweights %u\n",nweights);
	for(i=0;i<nweights;i++)
		wts_all[i] = 1.0 * i;
	for(i=1;i<num_layers;i++)
	{
		printf("layer %u:\n",i);
		for(j=0;j<num_nodes[i];j++)
		{
			printf("node %u\n",j);
			for(k=0;k<num_nodes[i-1];k++)
				printf("%g\t",i_w[i][j][k]);
			puts("");
		}
		puts("");
	}
	return 0;
}

#endif
