#include <v_lib.h>

#define MAX_PARTICLES (1024)

u32_t	num_particles = 6;

u8_t	connected[MAX_PARTICLES][MAX_PARTICLES] = \
	{  /*0 1 2 3 4 5*/
		{0,0,0,0,0,1},/*0*/
		{0,0,0,1,0,0},/*1*/
		{0,0,0,0,0,0},/*2*/
		{0,0,0,0,0,0},/*3*/
		{0,0,0,0,0,1},/*4*/
		{0,0,0,0,0,0} /*5*/
	};


typedef struct iset_s
{
	u32_t num_elems;
	vboolean in[MAX_PARTICLES];
	u32_t list[MAX_PARTICLES]; /* delayed */
} iset_t, *iset_p;

void isetClear(iset_p set)
{
	memZero(set, sizeof(iset_t));
}
void isetAdd(iset_p set,u32_t elem)
{
	if(set->in[elem])
		return;
	set->in[elem] = true;
	set->num_elems++;
}

void isetDel(iset_p set,u32_t elem)
{
	if(!set->in[elem])
		return;
	set->in[elem] = false;
	set->num_elems--;
}

vboolean isetGet(iset_p set,u32_t *ret_val)
{
	u32_t i;
	if(!set->num_elems)
		return false;
	for(i = 0;false == set->in[i];i++);
	*ret_val = i;
	return true;
}
void isetUpdateList(iset_p set)
{
	u32_t i, count;
	for(i = count = 0;count < set->num_elems;i++)
	{
		if(set->in[i])
			set->list[count++] = i;
	}
}

void isetPrintList(iset_p set)
{
	u32_t i;
	if(!set || !set->num_elems)
	{
		conPuts("empty\n");
		return;
	}
	conPrintf("num_elems;%u;elems", set->num_elems);
	for(i = 0;i < set->num_elems;i++)
		conPrintf(";%u", set->list[i]);
	conPuts("\n");
}

#define	IS_CONNECTED(connmatrix,i,j)	(((j) > (i)) ? (connmatrix)[i][j] : (connmatrix)[j][i])

iset_t	isolated;
u32_t	num_clusters;
iset_t	clusters[MAX_PARTICLES];

void clustersClear(void)
{
	u32_t i;
	num_clusters = 0;
	isetClear(&isolated);
	memZero(&clusters, sizeof(clusters));
	for(i = 0;i < num_particles;i++)
		isetAdd(&isolated, i);
	isetUpdateList(&isolated);
}

V_LOCAL void addParticleToCluster_r(iset_p dst,iset_p src,u32_t particle_no)
{
	iset_t src_copy;
	u32_t i, next_no;
	isetAdd(dst, particle_no);
	isetDel(src, particle_no);
	isetUpdateList(src);
	/* make a copy of src */
	memCopy(&src_copy, src, sizeof(iset_t));
	/* find all particles which are connected to next_no */
	for(i = 0;i < src_copy.num_elems;i++)
	{
		next_no = src_copy.list[i];
		if(IS_CONNECTED(connected,particle_no,next_no))
		{
			/* add particle to current cluster */
			addParticleToCluster_r(dst,src,next_no);
		}
	}
}

void clustersFill()
{
	u32_t next_no;
	for(num_clusters = 0;isetGet(&isolated,&next_no);num_clusters++)
	{
		iset_p cluster = &clusters[num_clusters];
		addParticleToCluster_r(cluster,&isolated,next_no);
		isetUpdateList(cluster);
	}
}

void clustersDump(vhandle_t out)
{
	u32_t i, j;
	filePrintf(out,"num_clusters %u\n",num_clusters);
	for(i = 0;i < num_clusters;i++)
	{
		filePrintf(out,"%u;%u", i, clusters[i].num_elems);
		isetUpdateList(&clusters[i]);
		for(j = 0;j < clusters[i].num_elems;j++)
			filePrintf(out,";%u", clusters[i].list[j]);
		filePrintf(out,"\n");
	}
}

void connmatDumpGraph(vhandle_t out, vboolean print_isolated)
{
	u32_t i, j, num_conn;
	filePrintf(out, "graph connmat {\n\tlayout=fdp\n");
	filePrintf(out, "\tnode [shape=ellipse]\n");
	if(print_isolated)
	{
		for(i = 0;i < num_particles;i++)
			filePrintf(out, "\t%u [label=\"P%04u\"]\n", i, i);
	}
	else
	{
		num_conn = 0;
		for(i = 0;i < num_particles;i++)
		{
			for(j = 0;j < num_particles;j++)
			{
				if(IS_CONNECTED(connected,i,j))
				{
					filePrintf(out, "\t%u [label=\"P%04u\"]\n", i, i);
					break;
				}
			}
		}
	}
	for(i = 0;i < num_particles - 1;i++)
	{
		for(j = i;j < num_particles;j++)
		{
			if(connected[i][j])
				filePrintf(out, "\t%u -- %u\n", i, j);
		}
	}
	filePrintf(out, "}\n");
}

int main()
{
	vhandle_t out;
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);
	out = fileOpenWrite("connmat.gv");
	connmatDumpGraph(out, false);
	fileClose(out);
	clustersClear();
	clustersFill();
	out = fileOpenWrite("clusters.csv");
	clustersDump(out);
	fileClose(out);
	return 0;
}
