byte value = 0x00;
byte lastvalue = 0xFF;

bool sendMessage = false;

const unsigned long eventInterval = 1000;
unsigned long previousTime = 0;

const unsigned long indexSampler = 400;
unsigned long leftIndex = 0;
unsigned long rightIndex = 0;
unsigned long bothIndex = 0;

bool leftIndexSet = false;
bool rightIndexSet = false;
bool bothIndexSet = false;

/* L.TURN INDICATOR 7, R.TURN INDICATOR 8, HAZARD 9, HIGHBEAM 10, FRONTFOG 11, REARFOG 12 */
const int Pins[6] =   {7,    8,    9,    10,   11,   12};
const int Values[6] = {0x20, 0x40, 0x60, 0x05, 0x08, 0x10};

void setup() {
	Serial.begin(9600, SERIAL_8E1);

	for (int i=0; i < 6; i++) {
		pinMode(Pins[i], INPUT);
	}
}

void loop() {
	unsigned long currentTime = millis();
	value = 0x00;

	for (int i=0; i < 6; i++) {
		if (digitalRead(Pins[i]) == HIGH) {
			if(i == 0) { leftIndex = millis(); leftIndexSet = true; }
			if(i == 1) { rightIndex = millis(); rightIndexSet = true; }
			//if(i == 2) { bothIndex = millis(); bothIndexSet = true; }

			value+=Values[i];

			sendMessage = true;
		} else {
			if(i == 0) { leftIndexSet = false; }
			if(i == 1) { rightIndexSet = false; }
			//if(i == 2) { bothIndexSet = false; }
			continue;
		}
	}

	if (currentTime - leftIndex <= indexSampler && leftIndexSet == false) { value += Values[0]; }
	if (currentTime - rightIndex <= indexSampler && rightIndexSet == false) { value += Values[1]; }
	//if (currentTime - bothIndex <= indexSampler && bothIndexSet == false) { value += Values[2]; }

	if(sendMessage == true && value != lastvalue) {
		sendMessage = false;
		lastvalue = value;

		// TURN INDICATOR FIX
		if(value >= 0xA0) { value = value-0x40; }
		if(value >= 0x80) { value = value-0x20; }

		byte message[] = {0xD0, 0x08, 0xBF, 0x5B, value, 0x00, 0x00, 0x00, 0x00, 0x58, 0x00};
		sendKbus(message);
	}

	if (currentTime - previousTime >= eventInterval) {
		previousTime = currentTime;
		lastvalue = 0xFF;
		sendMessage = true;
	}
}

void sendKbus(byte *data) {
	int end_i = data[1]+2 ;
	data[end_i-1] = iso_checksum(data, end_i-1);
	Serial.write(data, end_i+1);
}

byte iso_checksum(byte *data, byte len) {
	byte crc=0;

	for(byte i=0; i < len; i++) {
		crc=crc^data[i];
	}
	return crc;
}