#include <stdio.h>
#include <math.h>

/* if sqrt(abs(x1-x2)^2+abs(y1-y2)^2) < MAXDIST, then we are in range */
#define MAXDIST 4.0

struct node;
struct packet;
struct route;

enum packettypes {
	packetPing,
	packetPong,
	packetRouteRequest,
	packetNoRoutes,
	packetKnownRoute
};

struct route {
	int to;
	int via;
	int ttl;
};

struct packet {
	int id;
	int fromid;
	int toid;
	int lastvia;
	int ttl;
	int len;
	union {
		int type;
		struct {
			int type;
			struct route route;
		} knownroute;
	} u;
};

struct node {
	int id;
	int x, y;
	int routes;
	struct route route[128];
};

int nodes = 0;
struct node node[128];	

void introduce(int x, int y);
void move(int id, int x, int y);
int  range(int x1, int y1, int x2, int y2);
void broadcast(int id, struct packet p);
void gotpacket(int id, struct packet p);
void rescan(int id);

void main()
{
	printf(">>> Route sim: starting\n");
	while(1)
	{
		printf("cmd ");
		int cmd = getchar();
		switch(cmd)
		{
			case 'i': case 'I':
				{
				int x,y;
				printf("  x ");
				scanf("%d", &x);
				printf("  y ");
				scanf("%d", &y);
				introduce(x,y);
				printf(">>> Introduced new node: ID %d\n", nodes-1);
				getchar();
				break;
				}
			case 'm': case 'M':
				{
				int id,x,y;
				printf(" id ");
				scanf("%d", &id);
				printf("  x ");
				scanf("%d", &x);
				printf("  y ");
				scanf("%d", &y);
				move(id,x,y);
				getchar();
				break;
				}
			case 't': case 'T':
				{
				struct packet p;
				int id;
				printf(" id ");
				scanf("%d",&id);
				p.fromid = id;
				p.toid = -1;
				p.lastvia = id;
				p.ttl = 0;
				p.len = 0;
				broadcast(id, p);
				getchar();
				break;
				}
			case 'p': case 'P':
				{
				struct packet p;
				int fid, tid;
				printf(" id ");
				scanf("%d",&fid);
				printf("tid ");
				scanf("%d",&tid);
				printf("ttl ");
				scanf("%d",&p.ttl);
				p.fromid = fid;
				p.toid = tid;
				p.lastvia = fid;
				p.len = 0;
				broadcast(fid, p);
				getchar();
				break;
				}
			case 'r': case 'R':
				{
				int cid;
				
				for (cid=0; cid < nodes; cid++)
				{
					printf(">>> Node %d rescanning horizon\n", cid);
					rescan(cid);
				}
				
				getchar();
				break;
				}
			case 's': case 'S':
				{
				int id, r;
				printf(" id ");
				scanf("%d",&id);
				
				printf("%d routes for node %d\n", node[id].routes, id);
				printf("to\tvia\tttl\n");
				printf("--------------------------------------------\n");
				for (r = 0; r < node[id].routes; r++)
					printf("%d\t%d\t%d\n", node[id].route[r].to, node[id].route[r].via, node[id].route[r].ttl);
				printf("--------------------------------------------\n");
				}
				
				getchar();
				break;
			case 'q': case 'Q':
				printf(">>> Bye\n");
				exit(1);
			case '\r':
			case '\n':
				// oh ffs
				continue;
			default:
				printf(">>> Unknown command %c\n", cmd);
				while (getchar() != '\n');
		};
	}
}

void introduce(int x, int y)
{
	node[nodes].id = nodes;
	node[nodes].x = x;
	node[nodes].y = y;
	node[nodes].routes = 0;
	nodes++;
}

void move(int id, int x, int y)
{
	node[id].x = x;
	node[id].y = y;
}

int range(int x1, int y1, int x2, int y2)
{
	/* sqrt(abs(x1-x2)^2+abs(y1-y2)^2) */
	float x1f = x1, y1f = y1, x2f = x2, y2f = y2;
	float f = sqrt(fabs(x1-x2)*fabs(x1-x2)+fabs(y1-y2)*fabs(y1-y2));
	return f < MAXDIST;
}

void broadcast(int id, struct packet p)
{
	int cid;
	if (id > nodes)
	{
		printf("Er, we can't send a packet from a node that is nonexistant.\n");
		return;
	}
	for (cid = 0; cid < nodes; cid++)
	{
		if (range(node[cid].x, node[cid].y, node[id].x, node[id].y))
			gotpacket(cid, p);
	}
}

void gotpacket(int id, struct packet p)
{
	//printf("%d: Got a packet initially from %d, through %d to %d. TTL %d.\n", id, p.fromid, p.lastvia, p.toid, p.ttl);
	if (p.toid == id)
	{
		if (p.u.type == packetPing)
		{
			//printf("%d: Ping! Turning around and ponging!\n", id );
			p.toid = p.fromid;
			p.u.type = packetPong;
			p.lastvia = id;
			p.fromid = id;
			broadcast(id, p);
			return;
		}
		if (p.u.type == packetPong)
		{
			if (p.id == -1)
			{
				int r, found;
				
				printf("%d: Horizon scan results: Found %d\n", id, p.fromid);
				printf("%d: Adding %d to route table\n", id, p.fromid);
				
				found = 0;
				for (r = 0; r < node[id].routes; r++)
				{
					if (node[id].route[r].to == p.fromid)
					{
						found = 1;
						node[id].route[r].via = id;
						node[id].route[r].ttl = 0;
						break;
					}
				}
				if (!found)
				{
					r = node[id].routes;
					node[id].route[r].to = p.fromid;
					node[id].route[r].via = id;
					node[id].route[r].ttl = 0;
					node[id].routes++;
				}
				
				printf("%d: Asking %d about known peers\n", id, p.fromid);
				p.toid = p.fromid;
				p.u.type = packetRouteRequest;
				p.ttl = 0;
				p.lastvia = id;
				p.fromid = id;
				broadcast(id, p);
				return;
			}
			printf("%d: Pong from %d with remaining TTL %d\n", id, p.fromid, p.ttl);
		}
		if (p.u.type == packetRouteRequest)
		{
			int r, or;
			
			printf("%d: Got a route request from %d\n", id, p.fromid);
			
			if (p.fromid == id)
			{
				printf("%d: Ignoring route request from myself\n");
				return;
			}
			
			if (node[id].routes == 0)
			{
				printf("%d: No routes to send to %d\n", id, p.fromid);
				p.toid = p.fromid;
				p.ttl = 0;
				p.u.type = packetNoRoutes;
				p.lastvia = id;
				p.fromid = id;
				broadcast(id, p);
				return;
			}
			
			for (r = 0; r < node[id].routes; r++)
			{
				struct packet np = p;
				
				np.toid = p.fromid;
				np.ttl = 0;
				np.lastvia = id;
				np.fromid = id;
				
				np.u.type = packetKnownRoute;
				np.u.knownroute.route = node[id].route[r];
				
				printf("%d: Telling %d about my route to %d\n", id, np.toid, node[id].route[r].to);
				
				broadcast(id, np);
			}
		}
		if (p.u.type == packetKnownRoute)
		{
			int r, found;
			
			if (p.fromid == id)
			{
				printf("%d: Ignoring route from myself\n", id);
				return;
			}
				
			if (p.u.knownroute.route.to == id)
			{
				printf("%d: Ignoring route to myself\n", id);
				return;
			}
			
			printf("%d: Got a route to %d through %d\n", id, p.u.knownroute.route.to, p.fromid);
			p.u.knownroute.route.ttl++;
			
			found = 0;
			for (r = 0; r < node[id].routes; r++)
			{
				if (node[id].route[r].to == p.u.knownroute.route.to)
				{
					found = 1;
					if (node[id].route[r].ttl > p.u.knownroute.route.ttl)
					{
						node[id].route[r].via = p.fromid;
						node[id].route[r].ttl = p.u.knownroute.route.ttl;
					}
					break;
				}
			}
			if (!found)
			{
				r = node[id].routes;
				node[id].route[r].to = p.u.knownroute.route.to;
				node[id].route[r].via = p.fromid;
				node[id].route[r].ttl = p.u.knownroute.route.ttl;
				node[id].routes++;
			}
			return;
		}
		return;
	}
	if (p.toid == -1)
	{
		if (p.u.type == packetPing)
		{
			//printf("%d: Broadcast ping! Turning around and ponging!\n", id );
			p.toid = p.fromid;
			p.u.type = packetPong;
			p.lastvia = id;
			p.fromid = id;
			broadcast(id, p);
			return;
		}
	}
	if (p.ttl == 0)
	{
		//printf("%d: TTL is zero - processing ends here.\n");
		return;
	}
	p.ttl--;
	p.lastvia = id;
	broadcast(id, p);	// This is very inefficient, and can/will create a broadcast flood, but it works momentarily.
}

void rescan(int id)
{
	struct packet p;
	
	p.fromid = id;
	p.toid = -1;
	p.id = -1;
	p.u.type = packetPing;
	p.lastvia = id;
	p.ttl = 0;
	broadcast(id, p);
}