//  'Simple Soft Serial' from Robyn2 
//  Adapted to 'ValentineOne Soft Serial' by JamoV13
//
//  AS IS WHERE IS!
//
//  Interfaces an Arduino to a ValentineOne radar.
//
//  This is adapted to listen only (sorry) for a ValentineOne radar infDisplay packet.
//  The ValentineOne will need to use the ESP protocol.
//  Retro mode is not supported at all.
//
//  The file V1SoftSerial _vX.ino (that you are reading here) needs the file 
//	v1ss_vX.ino to be in the same directory in order to be able to reference it.
//
//  The program should work with either a Mega, Nano, Uno, Pro Mini (16MHz)
//  and probably others at 16MHz with the same chip etc.
//
//	Basically, the program listens to Pin3 at (in theory) 57600 baud 8 bits, 1 stop bit, no parity
//  However, the Valentine seems to be (in practice) at a baud rate of ~71500  -  I'm not sure why.
//
//  To do this, two interupts are used:
//		- One to flag (test for) a failing voltage (start bit)
//		- Two to flag (sample at) regular intervals so that we can test each bit at the baud rate
//
//	It sends what it hears on Pin 3 back to the Serial port.
//
//	Connection list (UNO):
//
//  PIN	#	ArduinoUno use			to		Remote device	
//
//  PIN 0	Serial port	(Rx)		<->		BlueTooth (Tx)
//  PIN 1	Serial port	(Tx)		<->		BlueTooth (Rx)
//  PIN 2	pinV1Mute			<->		ValentineOne Mute wire (Yellow) (5V)
//  PIN 3	v1ss				<->		ValentineOne Data wire (Black) (5V)
//  PIN 4	pinButtonMute			<->		Handle bar button (green) (5V)
//  PIN 5	pinButtonMute			<->		Handle bar button (green) (5V)
//

//====Global constants====

const int pinV1Mute = 2;			// connected to the Mute wire of the V1
						// Pin 3 used by v1ss
const int pinButtonMute = 4;		        // Mute push button pin
const int pinLedL = 5;				// L band LED pin
const int pinLedKa = 6;				// Ka band LED pin
const int pinLedK = 7;				// K band LED pin
const int pinLedX = 8;				// X band LED pin
const int pinLedGreen = 9;			// Green LED pin
const int pinLedBlue = 10;			// Blue LED pin
const int pinLedRed = 11;			// Red LED pin

//====Global variables====

//----General----
bool validPacket = false;			// Checksum test result for incoming packets
int buttonMuteState_prev = LOW;		// the previous reading from the input pin
int buttonMuteState;				// the current reading from the input pin
int redVal, blueVal, greenVal;		// the Red Green & Blue LED intensities
long buttonMuteDebounceStart = 0;	// the last time the output pin was toggled


//----Packet contents of infDisplay----
byte Sof = 0;	// 0xAA	Start of frame 
byte Dx = 0;	// 0xD8	Receiver ID
byte Ex = 0;	// 0xEA	Sender ID
byte PID = 0;	// 0x31	Packet ID
byte PLn = 0;	// 0x09	Packet length (bytes)
byte BC1 = 0;	// 0x5B	Bogey Counter 1 display
byte BC2 = 0;	// 0x5B	Bogey Counter 2 display
byte Ss = 0;	// 0x1F	Signal strength bar
byte BA1 = 0;	// 0x38	Band and Arrow display 1
byte BA2 = 0;	// 0x28	Band and Arrow display 2 
byte Ax0 = 0;	// 0x0C	Aux0	
byte Ax1 = 0;	// 0x00 Aux1 Not used 
byte Ax2 = 0;	// 0x00 Aux2 Not used
byte Cs = 0;	// 0xE7	Checksum
byte Eof = 0;	// 0xAB	End of frame

//Band Detection States and Directions
bool L, K, Ka, X, front, side, rear = 0;


//====Intitial Setup===========

void setup() 
{
	//----Initialise pins----

	// handle button for muting the v1
	pinMode(pinButtonMute, INPUT);

	// MUTE wire to v1, also controls retro/esp mode
	pinMode(pinV1Mute, OUTPUT);
	// turn the v1Display ON by making the voltage LOW
	digitalWrite(pinV1Mute, LOW); 

	// LED to show the L band detections to the rider
	pinMode(pinLedL, OUTPUT);
	// set initial LED state
	digitalWrite(pinLedL, LOW);
	
	// LED to show the Ka band detections to the rider
	pinMode(pinLedKa, OUTPUT);
	// set initial LED state
	digitalWrite(pinLedKa, LOW);
	
	// LED to show the K band detections to the rider
	pinMode(pinLedK, OUTPUT);
	// set initial LED state
	digitalWrite(pinLedK, LOW);
	
	// LED to show the X band detections to the rider
	pinMode(pinLedX, OUTPUT);
	// set initial LED state
	digitalWrite(pinLedX, LOW);


	//----Start serial port----

	// Start hardware serial
	Serial.begin(9600);
	Serial.println("V1SoftSerial");

	// Start V1SoftSerial
	v1ssBegin();

}


//====Main loop==============

void loop() 
{
	//clean up serial port
	Serial.flush();

	// Check for button press
	void checkMuteButton();


	//----Read a packet----

	byte *frame = readFrame();

	// If the first byte is the SOF byte
	if (frame[0] == 0xAA)
	{
		// Divide the frame into global variables
		assignFrame(frame);

		// Did the packet have correct data
		validPacket = verifyCS();
	}


	//----Do stuff with valid packet----
			
	if(validPacket)
	{
		// print all the infDisplay packet variables to the serial port
		// infDisplayValuesToSerial();

		// print the detections to the serial port
		detectionsToSerial();

		// apply new detection data to L LED
		detectionsToLed();
	}
}


//====Functions==============

//Check for a button press and debounce it
void checkMuteButton()
{
	// the debounce time, increase if the outputs flicker etc
	const long debounceDelay = 50;		

	// check to see if the button is pressed
	int reading = digitalRead(pinButtonMute);

	// If the button changed state (due to noise or pressing) then:
	if (reading != buttonMuteState_prev) 
	{
		// reset the debouncing timer
		buttonMuteDebounceStart = millis();
	}
	
	// check if the button has been held longer than our debouce time
	if ((millis() - buttonMuteDebounceStart) > debounceDelay) 
	{
		// if the button state has changed:
		if (reading != buttonMuteState_prev) 
		{
			buttonMuteState = reading;
		}
	}

	// save the reading.  Next time through the loop,
	// it'll be the lastButtonState:
	buttonMuteState_prev = reading;
}

// send the detection states to the LED
void detectionsToLed()
{
	// If any band is detected then:
	if (L == HIGH) 
	{
		// Light up the LED
        digitalWrite(pinLedL, HIGH);
    }
	else
	{
		// Turn on the LED if nothing detected
		digitalWrite(pinLedL, LOW);
	}

		// If any band is detected then:
	if (Ka == HIGH) 
	{
		// Light up the LED
        digitalWrite(pinLedKa, HIGH);
    }
	else
	{
		// Turn on the LED if nothing detected
		digitalWrite(pinLedKa, LOW);
	}

		// If any band is detected then:
	if (K == HIGH) 
	{
		// Light up the LED
        digitalWrite(pinLedK, HIGH);
    }
	else
	{
		// Turn on the LED if nothing detected
		digitalWrite(pinLedK, LOW);
	}

		// If any band is detected then:
	if (X == HIGH) 
	{
		// Light up the LED
        digitalWrite(pinLedX, HIGH);
    }
	else
	{
		// Turn on the LED if nothing detected
		digitalWrite(pinLedX, LOW);
	}
}

//----Packet functions----
// Divide up the frame into individual values
void assignFrame(byte *frame)
{
	//int frameLen = sizeof(frame);		// number of bytes in frame
	//Serial.println(frameLen);

	Sof = frame[0];		// 0xAA	Start of frame 
	Dx = frame[1];		// 0xD8	Receiver ID
	Ex = frame[2];		// 0xEA	Sender ID
	PID = frame[3];		// 0x31	Packet ID
	PLn = frame[4];		// 0x09	Packet length (bytes)

	if (PID == 0x31)
	{
		//Payload bytes - infDisplay
		BC1 = frame[5];		// 0x5B	Bogey Counter 1 display
		BC2 = frame[6];		// 0x5B	Bogey Counter 2 display
		Ss = frame[7];		// 0x1F	Signal strength bar
		BA1 = frame[8];		// 0x38	Band and Arrow display 1
		BA2 = frame[9];		// 0x38	Band and Arrow display 2 
		Ax0 = frame[10];	// 0x0C	Aux0	
		Ax1 = frame[11];	// 0x00 Aux1 Not used 
		Ax2 = frame[12];	// 0x00 Aux2 Not used
	}

	Cs = frame[PLn + 4];	// 0x22	Checksum
	Eof = frame[PLn + 5];		// 0xAB	End of frame

}

// Print the detected bands and directions to serial port
int checkForDetections()
{
	int result = false;

	L = testbyte(BA1, 1);
	Ka = testbyte(BA1, 2);
	K = testbyte(BA1, 4);
	X = testbyte(BA1, 8);
	//	bool res = testBit(BA1, 16);
	front = testbyte(BA1, 32);
	side = testbyte(BA1, 64);
	rear = testbyte(BA1, 128);

	//fake Ka band detection
	//Ka = true;

	//If anything is detected
	if(BA1 != 0x00)
	{
		// flag as a detection present
		result = true;
	}

	return result;
}

// Print all packet values to serial port
/*void infDisplayValuesToSerial()
{
	Serial.println("");
	Serial.println("All Packet Values Received: ");

	Serial.println(" ");
	Serial.print("SOF ");
	Serial.println(Sof, HEX);		// 0xAA	Start of frame 
	Serial.print("Dest ");
	Serial.println(Dx, HEX);		// 0xAA	0Receiver ID
	Serial.print("From ");
	Serial.println(Ex, HEX);		// 0xAA	Sender ID
	Serial.print("PID ");
	Serial.println(PID, HEX);		// 0xAA	Packet ID
	Serial.print("PLn ");
	Serial.println(PLn, HEX);		// 0x09	Packet length (bytes)

	if (PID == 0x31)
	{
		//Payload bytes - infDisplay
		Serial.print("BC1 ");
		Serial.println(BC1, HEX);	// 0x5B	Bogey Counter 1 display
		Serial.print("BC2 ");
		Serial.println(BC2, HEX);	// 0x5B	Bogey Counter 2 display
		Serial.print("SS ");
		Serial.println(Ss, HEX);	// 0x1F	Signal strength bar
		Serial.print("BA1 ");
		Serial.println(BA1, HEX);	// 0x38	Band and Arrow display 1
		Serial.print("BA2 ");
		Serial.println(BA2, HEX);	// 0x28	Band and Arrow display 2
		Serial.print("Ax0 ");
		Serial.println(Ax0, HEX);	// 0x0C	Aux0	
		Serial.print("Ax1 ");
		Serial.println(Ax1, HEX);	// 0x00 Aux1 Not used 
		Serial.print("Ax2 ");
		Serial.println(Ax2, HEX);	// 0x00 Aux2 Not used
	}

	Serial.print("CS ");
	Serial.println(Cs, HEX);	// 0xE7	Checksum
	Serial.print("EOF ");
	Serial.println(Eof, HEX);
	Serial.println(" ");
}*/

// Print the detected bands and directions to serial port
void detectionsToSerial()
{
	int detectionsPresent = checkForDetections();

	//Serial.print("Are detections present? ");
	//Serial.println(detectionsPresent);

	//if(detectionsPresent)
	//{
		
		Serial.print(" Bands: ");
		if (L) { Serial.println("L "); }
		if (Ka) { Serial.println("Ka "); }
		if (K) { Serial.println("K "); }
		if (X) { Serial.println("X "); }
		Serial.println("");
		//Serial.print("to the ");
		//if (front) { Serial.println("FRONT"); }
		//else if (side) { Serial.println("SIDE"); }
		//else if (rear) { Serial.println("REAR"); }
		//Serial.println("");
	//}
}

//Read in a frame from 0xAA to 0xAB
byte* readFrame()
{
	bool endOfFrame = false;
	bool recordBytes = false;

	int i = 0;
	int j = 0;
	const int arrayLength = 120;

	byte bb = 0xFF;
	byte byteArray[arrayLength];


	while(i < arrayLength && endOfFrame == false)
	{
		//read in a byte (8-bits unsigned)
		while (bb == 0xFF) { bb = receiveFrame();}

		//show the byte
		//Serial.println(bb, HEX);

		if(bb == 0xAA)
		{
			//Store the byte
			byteArray[i] = bb;
			
			recordBytes = true;

			//show the byte
			//Serial.println(byteArray[i], HEX);

			//Get ready for next byte
			i++;
		}
		else if(recordBytes == true)
		{

			//Store the byte
			byteArray[i] = bb;

			//show the byte
			//Serial.println(byteArray[i], HEX);

			//Get ready for next byte
			i++;
		}		

		if(bb == 0xAB)
		{
			bb = 0xFF;
			recordBytes = false;
			endOfFrame = true;
		}

		bb = 0xFF;
	}

	return byteArray;
}

// Recieve packets from V1
byte receiveFrame() 
{
	byte bb = 0xFF;

	//init to null char
	if (v1ssAvailable() > 0) 
	{
	    bb = v1ssRead();
		//Serial.println(bb, HEX);
	}

	return bb;
}

//Mask the byte and test if non-zero
bool testbyte(byte testByte, byte testBit)
{
	bool result = false;
	
	byte maskedByte = testByte & testBit;

	if (maskedByte != 0)
	{
		result = true;
	}

	return result;
}

// compare the packet to checksum
bool verifyCS()
{
	bool result = false;

	byte newCS = 0x00;

	//CS = SOF + DI + OI + PI + PL + PD1 + PD2 +� + PDPL 1

	newCS = Sof + Dx;
	newCS = newCS + Ex;
	newCS = newCS + PID;
	newCS = newCS + PLn;
	newCS = newCS + BC1; 
	newCS = newCS + BC2; 
	newCS = newCS + Ss;
	newCS = newCS + BA1;
	newCS = newCS + BA2;
	newCS = newCS + Ax0;
	newCS = newCS + Ax1;
	newCS = newCS + Ax2;

	//Serial.print("newCS: ");
	//Serial.print(newCS, HEX);
	//Serial.print("\tCS: ");
	//Serial.println(Cs, HEX);

	if (Cs == newCS && Cs != 0)
	{
		result = true;
	}

	return result;
}

//========END=========