#include "PBABitSet.h"

int PBABitSet::read(unsigned int index, bool &returnValue) {
	if (index > getLastIndex())
		return -1;

	int i_bitmask = index / 16;
	int i_bit = index % 16;

	//https://stackoverflow.com/questions/2249731/how-do-i-get-bit-by-bit-data-from-an-integer-value-in-c
	returnValue = (bitmask[i_bitmask] & (1 << i_bit)) >> i_bit;

	return 1;
}

int PBABitSet::read(unsigned int index) {
	if (index > getLastIndex())
		return -1;

	int i_bitmask = index / 16;
	int i_bit = index % 16;

	//https://stackoverflow.com/questions/2249731/how-do-i-get-bit-by-bit-data-from-an-integer-value-in-c
	return (bitmask[i_bitmask] & (1 << i_bit)) >> i_bit;
}

int PBABitSet::store(bool value) {

	switch (current_index_bit) {
	case -1:
		bitmask[current_index_bitmask] += value * 1;
		break;
	case 0:
		bitmask[current_index_bitmask] += value * 2;
		break;
	case 1:
		bitmask[current_index_bitmask] += value * 4;
		break;
	case 2:
		bitmask[current_index_bitmask] += value * 8;
		break;
	case 3:
		bitmask[current_index_bitmask] += value * 16;
		break;
	case 4:
		bitmask[current_index_bitmask] += value * 32;
		break;
	case 5:
		bitmask[current_index_bitmask] += value * 64;
		break;
	case 6:
		bitmask[current_index_bitmask] += value * 128;
		break;
	case 7:
		bitmask[current_index_bitmask] += value * 256;
		break;
	case 8:
		bitmask[current_index_bitmask] += value * 512;
		break;
	case 9:
		bitmask[current_index_bitmask] += value * 1024;
		break;
	case 10:
		bitmask[current_index_bitmask] += value * 2048;
		break;
	case 11:
		bitmask[current_index_bitmask] += value * 4096;
		break;
	case 12:
		bitmask[current_index_bitmask] += value * 8192;
		break;
	case 13:
		bitmask[current_index_bitmask] += value * 16384;
		break;
	case 14:
		bitmask[current_index_bitmask] += value * 32768;
		break;
	case 15:
		bitmask.push_back(0);
		current_index_bitmask++;
		bitmask[current_index_bitmask] += value * 1;
		break;

	default:
		return -1;
	}

	if (current_index_bit < 15 && current_index_bit >= -1)
		current_index_bit++;
	else if (current_index_bit == 15)
		current_index_bit = 0;
	else
		return -1;

	return 1;
}

//index of last bit stored, n >= 0
unsigned int PBABitSet::getLastIndex() {
	return 16 * current_index_bitmask + current_index_bit;
}

//number of bits stored, n >= 1
unsigned int PBABitSet::getSize() {
	return  getLastIndex() + 1;
}

void PBABitSet::storeChase(double &angle) {
	angle = std::fmod(angle, 360);
	if (-30 <= angle && angle <= 30) 
	{
		//1 - 001
		store(1);
		store(0);
		store(0);
		angle = 0;
	}
	else if (30 < angle && angle <= 90) 
	{
		//2 - 010
		store(0);
		store(1);
		store(0);
		angle = 60;
	}
	else if (90 < angle && angle <= 150)
	{
		//3 - 011
		store(1);
		store(1);
		store(0);
		angle = 120;
	}
	else if (150 < angle && angle <= 180 || -150 > angle && angle >= -180)
	{
		//4 - 100
		store(0);
		store(0);
		store(1);
		angle = 180;
	}
	else if ( -90 > angle && angle >= -150)
	{
		//5 - 101
		store(1);
		store(0);
		store(1);
		angle = -120;
	}
	else if (-30 > angle && angle >= -90)
	{
		//6 - 110
		store(0);
		store(1);
		store(1);
		angle = -60;
	}
}

void PBABitSet::storeJump(unsigned short jx, unsigned short jy, xDirection xDir, yDirection yDir) {

	unsigned short temp = 0;


	if (jx > 0)
	{
		store(0);
		store(0);
		store(0);

		//direction on x-axis
		if (xDir == left)
			store(1);//left
		else if(xDir == right)
			store(0);//right

		//number of subsequent bits required for jx
		// = 3 * (decimal(bit_i bit_j) + 1)
		if (jx <= 8)
		{
			temp = 3;
			//3
			store(0);
			store(0);
		}
		else if (jx <= 64) //2^(6-3) + 2^6
		{
			temp = 6;
			//6
			store(1);
			store(0);
		}
		else if (jx <= 512) //2^(9-3) + 2^9
		{
			temp = 9;
			//9
			store(0);
			store(1);
		}
		else if (jx <= 4096 && xDir == right)//2^(12-3) + 2^12, right direction
		{
			temp = 12;
			//12
			store(1);
			store(1);
		}
		else
		{
			//store above elseif + rest of the information shall be stored on a NEW jump instance
			_ASSERT(0);
		}

		//store jx on temp bits
		
		for (unsigned short i = 0; i < temp; i++)
		{
			//remember this -1 in decoding part
			bool bit = ((jx - 1) & (1 << i)) >> i;
			store(bit);
		}
		
	}

	temp = 0;

	//jy
	if (jy > 0)
	{
		store(1);
		store(1);
		store(1);

		//direction on y-axis
		if (yDir == down)
			store(1);//down
		else if (yDir == up)
			store(0);//up

		//number of subsequent bits required for jy
		if (jy <= 8)
		{
			temp = 3;
			//3
			if(yDir == down)
			{
				store(0);
				store(0);
			}
			else
			{
				store(1);
				store(0);
			}
			
		}
		else if (jy <= 64)
		{
			temp = 6;
			//6
			if(yDir == down)
			{
				store(1);
				store(0);
			}
			else
			{
				store(0);
				store(1);
			}
			
		}
		else if (jy <= 512)
		{
			temp = 9;
			//9
			if(yDir == down)
			{
				store(0);
				store(1);
			}
			else
			{
				store(1);
				store(1);
			}
			
		}
		else if (jy <= 4096 && yDir == down)
		{
			temp = 12;
			//12
			store(1);
			store(1);
		}
		else
		{
			//rest of the information shall be stored on a NEW jump instance
			_ASSERT(0);
		}

		
		//store jy on temp bits
		
		for (unsigned short i = 0; i < temp; i++)
		{
			//remember this -1 in decoding part
			bool bit = ((jy - 1) & (1 << i)) >> i;
			store(bit);
		}
		
	}
}

void PBABitSet::storeTimestamp(time_t &ct, time_t nt, double temporalError) {
	//the whitepaper stated dt = (nt - ct) / 1000
	// this however made no sense to me, as temporalError was supposed to be measured in seconds
	// because the type time_t also represents the amount of seconds elapsed since 00:00 hours, Jan 1, 1970 UTC
	long dt = (nt - ct);

	if (dt <= temporalError)
		return;
	else if(temporalError < dt && dt < 2*temporalError)
	{
		//000111
		store(0);
		store(0);
		store(0);
		store(1);
		store(1);
		store(1);

		ct = ct + 2 * temporalError;
	}
	else if (dt > 2 * temporalError)
	{
		store(1);
		store(1);
		store(1);
		store(0);
		store(0);
		store(0);
		
		int multiple = dt / temporalError - 3;

		bool bit = (multiple) & (1 << 0) >> 0;
		store(bit);
		bit = (multiple) & (1 << 1) >> 1;
		store(bit);
		bit = (multiple) & (1 << 2) >> 2;
		store(bit);

		ct = (multiple + 3) * temporalError + ct;
	}
}

std::vector<GPSPoint> PBABitSet::decode(double spatialError) {
	std::vector<GPSPoint> trajectory;

	//First lat,lon GPSpoint stored outside bitset. Store it into bitset if you have time.
	trajectory.push_back(startPoint);


	bool xJumpAvailable = false;
	GPSPoint xEndPoint = GPSPoint();

	unsigned short jx = 0;
	unsigned short jy = 0;
	
	unsigned int lastIndex = getLastIndex();
	for (unsigned int i = 0; i <= lastIndex; i+=3)
	{
		unsigned short bit123 = 0;
		
		bit123 = (bit123 << 1) | read(i + 2);
		bit123 = (bit123 << 1) | read(i + 1);
		bit123 = (bit123 << 1) | read(i);

		if (bit123 == 0 || bit123 == 7)
		{
			

			//jump / timestamp
			i += 3;

			unsigned short bit456 = 0;
			bit456 = (bit456 << 1) | read(i + 2);
			bit456 = (bit456 << 1) | read(i + 1);
			bit456 = (bit456 << 1) | read(i);

			
			if (bit123 == 0) //000, jump X or timestamp
			{
				if (bit456 == 7)
				{
					// 000 111 timestamp
				}
				else
				{
					//jump x
					xDirection xDir = left;
					if((bit456 & 1) == 0)
						xDir = right;

					
					unsigned short nBits = 0;
					
					if(xDir == left)
						nBits = ((bit456 >> 1) + 1) * 3;
					else if(xDir == right)
						nBits = ((bit456 >> 1) + 1) * 3;

					
					i += nBits;
					for (unsigned short j = nBits; j > 0; j--)
					{
						jx = (jx << 1) | read(i+j-1);
					}
					

					//-1 from encoding taken into account
					jx++;
					

	

					double base = horizontalLength(jx, spatialError);
					double deg = -90;
					if (xDir == right)
						deg = 90;

					//x-jump endpoint
					GPSPoint pt = latlon_radial_distance(trajectory.back(), deg, base);

					
					bool safeToContinue = (i + 3 + 2) <= getLastIndex();

					unsigned short nextbit123 = 0;
					if (safeToContinue)
					{
						//check if x-jump is followed by y-jump
						nextbit123 = (nextbit123 << 1) | read(i + 3 + 2);
						nextbit123 = (nextbit123 << 1) | read(i + 3 + 1);
						nextbit123 = (nextbit123 << 1) | read(i + 3);
					}

					if (nextbit123 == 7) //wait for y jump, then calculate y-jump with x-jump endpoint as basepoint
					{
						xJumpAvailable = true;
						xEndPoint = pt;
					}
					else //y-jump does not follow. we can store x-jump endpoint
					{
						
						trajectory.push_back(pt);
						jx = 0;
					}
				}
			}
			else if (bit123 == 7) //111, jump Y or timestamp
			{
				if (bit456 == 0)
				{
					// 111 000 timestamp
				}
				else
				{
					//jump y
					yDirection yDir = down;
					if ((bit456 & 1) == 0)
						yDir = up;

					unsigned short nBits = 0;
					
					if(yDir == down)
						nBits = ((bit456 >> 1) + 1) * 3;
					else if(yDir == up)
						nBits = (bit456 >> 1) * 3;

					
					i += nBits;
					for (unsigned short j = nBits; j > 0; j--)
					{
						jy = (jy << 1) | read(i + j - 1);
					}
					

					//-1 from encoding taken into account
					jy++;
					
					double height = verticalLength(jy, spatialError, jx);
					double deg = 0;
					if (yDir == up)
						deg = 180;

					//y-jump endpoint
					GPSPoint pt = GPSPoint();

					if (xJumpAvailable)
					{
						pt = latlon_radial_distance(xEndPoint, deg, height);
						xJumpAvailable = false;
						xEndPoint = GPSPoint();

						jx = 0;
						jy = 0;
					}
					else
					{
						pt = latlon_radial_distance(trajectory.back(), deg, height);

						jy = 0;
					}

					trajectory.push_back(pt);
				}
			}


		}
		else if (0 < bit123 < 7) //chase
		{
			double deg = 0;

			
			switch (bit123) {

				case 1: // -30, 30
				{
					deg = 0;
					break;
				}
				case 2: // 30, 90
				{
					deg = 60;
					break;
				}
				case 3: // 90, 150
				{
					deg = 120;
					break;
				}
				case 4: // 150, 180 || -180, -150 
				{
					deg = 180;
					break;
				}
				case 5: // -150, -90
				{
					deg = -120;
					break;
				}
				case 6: // -90, -30
				{
					deg = -60;
					break;
				}
			}
			
			double d = spatialError * 2;
			GPSPoint pt = latlon_radial_distance(trajectory.back(), deg, d);

			trajectory.push_back(pt);
		}
	}
	return trajectory;
}

std::vector<GPSPointEx> PBABitSet::decodeEx(double spatialError, double temporalError) {
	std::vector<GPSPointEx> trajectory;

	//First lat,lon GPSpoint stored outside bitset. Store it into bitset if you have time.
	time_t ct = startTime;
	trajectory.push_back(GPSPointEx(startPoint, ct));


	bool xJumpAvailable = false;
	GPSPoint xEndPoint = GPSPoint();

	unsigned int lastIndex = getLastIndex();
	for (unsigned int i = 0; i <= lastIndex; i += 3)
	{
		unsigned short bit123 = 0;

		bit123 = (bit123 << 1) | read(i + 2);
		bit123 = (bit123 << 1) | read(i + 1);
		bit123 = (bit123 << 1) | read(i);

		if (bit123 == 0 || bit123 == 7)
		{
			unsigned short jx = 0;
			unsigned short jy = 0;

			//jump / timestamp
			i += 3;

			unsigned short bit456 = 0;
			bit456 = (bit456 << 1) | read(i + 2);
			bit456 = (bit456 << 1) | read(i + 1);
			bit456 = (bit456 << 1) | read(i);


			if (bit123 == 0) //000, jump X or timestamp
			{
				if (bit456 == 7)
				{
					// 000 111 timestamp
					ct += temporalError * 2;
				}
				else
				{
					//jump x
					xDirection xDir = left;
					if ((bit456 & 1) == 0) // or read(i);
						xDir = right;

					//luetaan liikaa bittejä atm
					unsigned short nBits = 0;

					if (xDir == left)
						nBits = ((bit456 >> 1) + 1) * 3;
					else if (xDir == right)
						nBits = ((bit456 >> 1) + 1) * 3;

					i += nBits;
					for (unsigned short j = nBits; j > 0; j--)
					{
						jx = (jx << 1) | read(i + j - 1);
					}
					
					//-1 from encoding taken into account
					jx++;


					double base = horizontalLength(jx, spatialError);
					double deg = -90;
					if (xDir == right)
						deg = 90;

					//x-jump endpoint
					GPSPoint pt = latlon_radial_distance(trajectory.back(), deg, base);


					bool safeToContinue = (i + 3 + 2) <= getLastIndex();

					unsigned short nextbit123 = 0;
					if (safeToContinue)
					{
						//check if x-jump is followed by y-jump
						nextbit123 = (nextbit123 << 1) | read(i + 3 + 2);
						nextbit123 = (nextbit123 << 1) | read(i + 3 + 1);
						nextbit123 = (nextbit123 << 1) | read(i + 3);
					}

					if (nextbit123 == 7) //wait for y jump, then calculate y-jump with x-jump endpoint as basepoint
					{
						xJumpAvailable = true;
						xEndPoint = pt;
					}
					else //y-jump does not follow. we can store x-jump endpoint
					{
						trajectory.push_back(GPSPointEx(pt, ct));
					}
				}
			}
			else if (bit123 == 7) //111, jump Y or timestamp
			{
				if (bit456 == 0)
				{
					// 111 000 timestamp
					i += 3;
					int multiple = 0;
					multiple = (multiple << 1) | read(i + 3 + 2);
					multiple = (multiple << 1) | read(i + 3 + 1);
					multiple = (multiple << 1) | read(i + 3);

					ct += (multiple + 3) * temporalError;
				}
				else
				{
					//jump y
					yDirection yDir = down;
					if ((bit456 & 1) == 0) //or read(i);
						yDir = up;

					unsigned short nBits = 0;

					if (yDir == down)
						nBits = ((bit456 >> 1) + 1) * 3;
					else if (yDir == up)
						nBits = (bit456 >> 1) * 3;


					i += nBits;
					for (unsigned short j = nBits; j > 0; j--)
					{
						jy = (jy << 1) | read(i + j - 1);
					}
					

					//-1 from encoding taken into account
					jy++;
					
					double height = verticalLength(jy, spatialError, jx);
					double deg = 0;
					if (yDir == up)
						deg = 180;

					//y-jump endpoint
					GPSPoint pt = GPSPoint();

					if (xJumpAvailable)
					{
						pt = latlon_radial_distance(xEndPoint, deg, height);
						xJumpAvailable = false;
						xEndPoint = GPSPoint();
					}
					else
						pt = latlon_radial_distance(trajectory.back(), deg, height);

					trajectory.push_back(GPSPointEx(pt, ct));
				}
			}


		}
		else if (0 < bit123 < 7) //chase
		{
			double deg = 0;


			switch (bit123) {

			case 1: // -30, 30
			{
				deg = 0;
				break;
			}
			case 2: // 30, 90
			{
				deg = 60;
				break;
			}
			case 3: // 90, 150
			{
				deg = 120;
				break;
			}
			case 4: // 150, 180 || -180, -150 
			{
				deg = 180;
				break;
			}
			case 5: // -150, -90
			{
				deg = -120;
				break;
			}
			case 6: // -90, -30
			{
				deg = -60;
				break;
			}
			}

			double d = spatialError * 2;
			GPSPoint pt = latlon_radial_distance(trajectory.back(), deg, d);

			trajectory.push_back(GPSPointEx(pt, ct));
		}
	}
	return trajectory;
}