#include "network.h"
#include <vector>
#include <math.h>

// determine if a loop has data based on lane config data
bool DataAvail(char flag, int num) {
	int mag, fel;

	// find mask value
	if (num == 1)
		mag = 0x01;
	else if (num == 2)
		mag = 0x02;
	else if (num == 3)
		mag = 0x04;
	else if (num == 4)
		mag = 0x08;
	else if (num == 5)
		mag = 0x10;
	else if (num == 6)
		mag = 0x20;
	else if (num == 7)
		mag = 0x40;
	else if (num == 8)
		mag = 0x80;
	
	fel = flag & mag;
	fel = fel >> (num - 1);
	
	if (fel == 1)
		return true;
	else 
		return false;
}

// Initializes the LDS_LOOP dataPack with all necessary static data and default dynamic data
void init_loop_dataPack(LDS_LOOP *loop)
{
	int j;
	// Allocate memory for dataPack
	loop->dataPack = (unsigned char *) calloc(sizeof(unsigned char), loop->length);

	// dataPack 5-8: lane config
	char d5 = 0, d6 = 0, d7 = 0, d8 = 0;		
	for (j = 0; j < loop->num; j++)
	{
		for (int k = 0; k < 8; k++)
		{
			if (strcmp(loop->loop_loc[j], dp5[k]) == 0)
				d5 += pow(2, k);
			if (strcmp(loop->loop_loc[j], dp6[k]) == 0)
				d6 += pow(2, k);
			if (strcmp(loop->loop_loc[j], dp7[k]) == 0)
				d7 += pow(2, k);
			if (strcmp(loop->loop_loc[j], dp8[k]) == 0)
				d8 += pow(2, k);
		}
	}
	loop->dataPack[5-1] = d5;
	loop->dataPack[6-1] = d6;
	loop->dataPack[7-1] = d7;
	loop->dataPack[8-1] = d8;

	// dataPack 1: Drop number, i.e. station address
	loop->dataPack[1-1] = loop->drop;

	// dataPack2 (2 bytes per loop)
	loop->dataPack[2-1] = loop->num * 2 + Fixed_Byte_To_Checksum;

	// dataPacket 3 (lowbyte: # of mainline loops, highbyte: # of opposite loops)
	int low = 0, high = 0;
	for (j = 1; j <= 6; j++)
	{
		low += DataAvail(loop->dataPack[5-1], j);
		high += DataAvail(loop->dataPack[6-1], j);
	}
	high = high << 4;
	loop->dataPack[3-1] = high | low;

	// dataPack4 (Miscl. flags: samples are: 80, A0, E0, 00)
	loop->dataPack[4-1] = 0xA0;

	// dataPack 9: initialized as 00 (meaning no metering); need to be updated every 30 sec
	loop->dataPack[9-1] = 0;

	// datadataPack 10-13: lane malfunction? Assuming all functional
	loop->dataPack[10-1] = 0;
	loop->dataPack[11-1] = 0;
	loop->dataPack[12-1] = 0;
	loop->dataPack[13-1] = 0;

	// dataPack 14-22: ramp metering data
	// BYTE 16 and 22 need to be updated every 30 sec
	bool found = false;
	for (j = 0; j < loop->num; j++)
	{
		if (strcmp(loop->loop_loc[j], "DEMAND") == 0)
		{
			found = true;
			break;
		}
	}
	if (found)
	{
		// BYTE 14: mostly 07, some are 05, 03, 00
		loop->dataPack[14-1] = 0x07;
		// mostly 06(TOD table 1); some are 0B (No metering) or 05(traffic responsive)
		loop->dataPack[15-1] = 0x06;
		// most 00, some are 01 (queue override) or 80(Meter ON sign)
		loop->dataPack[16-1] = 0x00;
		// Field Manual Rate
		loop->dataPack[17-1] = 0xFF;
		// TOC Manual Rate
		loop->dataPack[18-1] = 0xFF;
		// PSO Manual Rate
		loop->dataPack[19-1] = 0xFF;
		// CORM Rate
		loop->dataPack[20-1] = 0xFF;
		// Local Responsive Rate. DON'T UNDERSTAND YET
		loop->dataPack[21-1] = 0x00;
		// TOD Rate: need to query RAMP plugin! 
		loop->dataPack[22-1] = 0x00;
	}
	// LDS: NO Metering
	else
	{
		loop->dataPack[14-1] = 0x00;
		loop->dataPack[15-1] = 0x0B;
		loop->dataPack[16-1] = 0x00;			
		loop->dataPack[17-1] = 0xFF;
		loop->dataPack[18-1] = 0xFF;
		loop->dataPack[19-1] = 0xFF;
		loop->dataPack[20-1] = 0xFF;
		loop->dataPack[21-1] = 0x00;
		loop->dataPack[22-1] = 0x00;
	}

	// dataPack 23-24: sum of mainline/Oppsite traffic data; need to be updated every 30 sec
	loop->MlTotVol = 0;
	loop->OppTotVol = 0;
	loop->dataPack[23-1] = loop->MlTotVol;	
	loop->dataPack[24-1] = loop->OppTotVol;	
	
	// dataPack 25-26: BYTE 25 is fixed, i.e. 03; BYTE 26 is either 0xA2 or 0x84
	loop->dataPack[25-1] = 0x03;
	loop->dataPack[26-1] = 0x84;

	printf("lds=%d (%d), p1=%2X, p2=%2X, p3=%2X, p5=%2X, p6=%X, p7=%X, p8=%2X\n", 
		loop->lds, loop->length, loop->dataPack[1-1], loop->dataPack[2-1], 
		loop->dataPack[3-1], loop->dataPack[5-1], 
		loop->dataPack[6-1], loop->dataPack[7-1], loop->dataPack[8-1]);
}

// TEST DATA
// lds_id	line 	drop sch lineinfo	system_key	sch_seq glo_seq		count	freeway	Dir	ca_pm	lds_name		
// 1203103	50	13	13	13	1123005873	24148	1357650		19	55	S	6.88	MACARTHU1 
FEP_LINE_LDS * load_lines(int *size, const char * fName)
{
	FEP_LINE_LDS *lines = (FEP_LINE_LDS *) calloc(sizeof(FEP_LINE_LDS), 1);
	*size = 1;
	lines[0].lineNum = 50;
	lines[0].lds.push_back(1203103);
	lines[0].ldsIndex.push_back(0);
	lines[0].ldsNum = 1;
	lines[0].count = 19;
	lines[0].schedule = 13;
	lines[0].globalSeq = 1357650;
	lines[0].schedleSeq = 24148;
	return lines;	
}

// TEST DATA
//FWY	Dir     POSTMI	LDS_ID	VDS_ID	LOOP_ID LOC    LANE LOOP_LOC      	PARAMICS_NAME  PARAMICS_LANE    
//55	S	6.88	1203103	1203104	1203105	QU	1	QUEUE      	55s6.88ora   	0

//55	S	6.88	1203103	1203104	1203106	DM	2	DEMAND     	55s6.88ora   	0

//55	S	6.88	1203103	1203104	1203107	PA	3	PASSAGE    	55s6.88ora   	0

//55	S	6.88	1203103	1203108	1203109	HV	1	SD_1       	?            	0

//55	S	6.88	1203103	1203110	1203111	ML	1	ML_1       	55s6.88ml    	4

//55	S	6.88	1203103	1203110	1203112	ML	2	ML_2       	55s6.88ml    	3

//55	S	6.88	1203103	1203110	1203113	ML	3	ML_3       	55s6.88ml    	2

//55	S	6.88	1203103	1203110	1203114	ML	4	ML_4       	55s6.88ml    	1
LDS_LOOP * load_lds(const char * fName)
{
	/* Read loop meta data */
	LDS_LOOP *lds_map = (LDS_LOOP *) calloc(sizeof(LDS_LOOP), 1);
	lds_map[0].lds = 1203103;
	lds_map[0].line_num = 50;
	lds_map[0].drop = 13;
	lds_map[0].num = 1;

	// Loop ids
	long *loopIDs = (long *) calloc(sizeof(long), lds_map[0].num);
	loopIDs[0] = 1203113;
	lds_map[0].loopID = loopIDs;
	
	// Loop locations
	char *loc = "ML_3";
	lds_map[0].loop_loc = &loc;

	// Init Loop dataPack
	lds_map[0].length = lds_map[0].num * 2 + CONTROL_DATA_LEN;
 	init_loop_dataPack(lds_map);

	lds_map[0].pos = 0;

	lds_map[0].MlTotVol = 150;
	lds_map[0].OppTotVol = 150;

	return lds_map;
}