/*
FT_2018_STW_CAN.cpp
*/

#include "Arduino.h"
#include "DueTimer.h"
#include "due_can.h"
#include "FT_2018e_STW_CAN.h"
#include "FT18e_STW_INIT.h"

CAN_FRAME can_0_msg;
//can_1_msg.id = 0x110;
int can_0_temp_data = 0;
int leds[] = {led1,led2,led3,led4,led5,led6,led7,led8,led9,led10,led11,led12,led13,led14,led15,led16};


void Init_Can_0(){
	Can0.begin(1000000);					// set CAN0 baud to 1kbit/s and don`t use enable pin!
	Can0.setNumTXBoxes(1);					// reserves mailbox 0 for tx only 8 mailboxes are available (the other 7 mailboxes are for rx)
	Can0.watchFor(0x502);					// set CAN RX filter for ID 0x502 and reserves mailbox 1 for rx
	Can0.watchFor(0x504);
	Can0.watchFor(0x500);
	Can0.watchFor(0x773);					// set CAN RX filter for ID 0x773 and reserves mailbox 3 for rx
	Can0.watchFor(0x775);
//	Can0.watchFor(0x777);					// set CAN RX filter for ID 0x777 and reserves mailbox 5 for rx
	Can0.watchFor(0x779);					// set CAN RX filter for ID 0x779 and reserves mailbox 6 for rx
	Can0.watchFor(0x77A);
	Can0.setGeneralCallback(Receive_Can_0);
	Timer3.attachInterrupt(Send_0x110);			// set send interrupt
	Timer3.start(10000); 						// Calls every 10ms
}

void Send_0x110(){
	read_buttons();
	read_rotary();
	can_0_msg.id = 0x110;
	can_0_msg.fid = 0;
	can_0_msg.rtr = 0;
	can_0_msg.priority = 0;
	can_0_msg.length = 2;
	can_0_msg.extended = 0;
	can_0_temp_data = 0;
	can_0_temp_data |= Stw_data.Stw_shift_up & 0b00000001;
	can_0_temp_data |= Stw_data.Stw_shift_down << 1 & 0b00000010;
	can_0_temp_data |= Stw_data.Stw_neutral << 2 & 0b00000100;
	can_0_temp_data |= Stw_data.Stw_auto_shift << 3 & 0b00001000;
	can_0_temp_data |= Stw_data.buttonStateEnc1 << 5 & 0b00100000; //pitlane
	can_0_msg.data.byte[0] = can_0_temp_data;
	can_0_msg.data.byte[1] = Stw_data.trc & 0b00001111;
	can_0_msg.data.byte[2] = Stw_data.mode & 0b00000111;
	if ((Stw_data.Stw_auto_shift << 3 & 0b00001000)){
		if(Vehicle_data.g_auto){
			Vehicle_data.g_auto = false;
		}else{
			Vehicle_data.g_auto = true;
		}
	}
	Can0.sendFrame(can_0_msg);
}

void Receive_Can_0(CAN_FRAME *temp_message){
	switch (temp_message->id) {
		//g_auto
		case 0x502:{ // eDrossel error bit
			Vehicle_data.e_thro			= (temp_message->data.byte[0] & 0x80) | (temp_message->data.byte[0] & 0x40) | (temp_message->data.byte[0] & 0x20) | (temp_message->data.byte[0] & 0x10); // bit 4-7
			
			if(temp_message->data.byte[0] & 0x80){
				Stw_data.error_type = 1;//"pc_error";
			} 
			if(temp_message->data.byte[0] & 0x40){
				Stw_data.error_type = 2;//"bse_error";
			} 
			if(temp_message->data.byte[0] & 0x20){
				Stw_data.error_type = 3;//"aps_error";
			} 
			if(temp_message->data.byte[0] & 0x10){
				Stw_data.error_type = 4;//"etb_error";
			}
			//can_1_temp_data |= g_etb_e << 4;
			//can_1_temp_data |= g_aps_e << 5;
			//can_1_temp_data |= g_bse_e << 6;
			//can_1_temp_data |= g_pc_e << 7;
			break;
		}
		case 0x504:{ //autoshift+gear
			//Vehicle_data.g_auto					= (temp_message->data.byte[1]) >> 4;
			Vehicle_data.gear					= (temp_message->data.byte[1]) >> 5;
			break;
		}
		case 0x773:{ // rpm
			Vehicle_data.revol					= (temp_message->data.byte[4] | temp_message->data.byte[3] << 8);
			break;
		}
		case 0x779:{ // battery voltage
			Vehicle_data.u_batt					= temp_message->data.byte[6];
			break;
		}
		/*case 0x77A: // revolution limit bit
			Vehicle_data.rev_lim				= (temp_message->data.byte[3] & 0x20) >> 4;
			switch(temp_message->data.byte[0]) {
				case 0x02: // temp. intercooler
					Vehicle_data.t_air			= temp_message->data.byte[7];
					break;
				case 0x05: // temp. water
					Vehicle_data.t_mot			= temp_message->data.byte[4];
					break;
				case 0x04: // temp. oil
					Vehicle_data.t_oil			= temp_message->data.byte[5];
				case 0x01: {
					Vehicle_data.p_wat 			= temp_message->data.byte[6];
					Vehicle_data.p_fuel			= temp_message->data.byte[7];
					Vehicle_data.p_oil 			= temp_message->data.byte[5];
					break;
				}
			}
			break;*/
		case 0x77A:{//temp und p
			//g_ms4_idle_b			= (temp_message->data.byte[2] & 0b10000000) >> 7;
			//g_ms4_engine_status		= (temp_message->data.byte[3] & 0b01000000) >> 6;
			//g_ms4_ignoff_b			= (temp_message->data.byte[3] & 0b10000000) >> 7;			
			
			if ( temp_message->data.byte[0] == 1){
				Vehicle_data.p_oil			= temp_message->data.byte[5];
				Vehicle_data.p_fuel			= temp_message->data.byte[7];
			}
			else if ( temp_message->data.byte[0] == 2){
				Vehicle_data.t_air			= temp_message->data.byte[7];
			}
			else if ( temp_message->data.byte[0] == 4){
				Vehicle_data.t_oil			= temp_message->data.byte[5];
			}
			else if ( temp_message->data.byte[0] == 5){
				Vehicle_data.t_mot			= temp_message->data.byte[4];
			}
			break;
		}
		case 0x775:{//speed
			Vehicle_data.speed_fl			= 2*(temp_message->data.byte[2]);
			Vehicle_data.speed_fr			= 2*(temp_message->data.byte[3]);
			Vehicle_data.speed 				= (Vehicle_data.speed_fl+Vehicle_data.speed_fr)/2;
			break;
		}
		/*case 0x777:{//m4_gear
			Vehicle_data.gear				= temp_message->data.byte[0];
			break;
		}*/
		case 0x500:{
				Vehicle_data.p_brake_front	= temp_message->data.byte[1];
				Vehicle_data.p_brake_rear 	= temp_message->data.byte[2];
			break;
		}
	}
}

void update_LED(){
//Copyright Michael Dietzel
//m.dietzel@fasttube.de
//Edit Michael Witt 05-2015
//m.witt@fasttube.de

//EDIT BAHA ZARROUKI 05-2107
//z.baha@fasttube.de

// alle Werte als Hex-Werte angegeben
	bool t_oil = (Vehicle_data.t_oil - 40) >= 0x96;	// 150°C temp.oil
	bool t_air = (Vehicle_data.t_air - 40) >= 0x3C;	// 60°C temp.llk
	bool t_mot = ((Vehicle_data.t_mot - 40) >= 0x69) and ((Vehicle_data.t_mot - 40)!=0xC8);	// 105°C temp.water und !=200
	
	bool g_auto = Vehicle_data.g_auto;
	bool u_batt = Vehicle_data.u_batt <= 0xB1;	// 12.5V batt.spann.
	bool e_dros = Vehicle_data.e_thro;			// error-bit
	
	bool rev_lim = Vehicle_data.rev_lim;
	
	uint16_t rev = Vehicle_data.revol;
	
	/*if(Vehicle_data.rev_lim){
		for (int j = 0; j < 10; j++){
			digitalWrite(leds[j], HIGH);
			//analogWrite(leds[j], STW_data.br);	//nur eine der zwei zeilen
		}
		delay(100);
		for (int j = 0; j < 10; j++){
			digitalWrite(leds[j], LOW);
		}
		delay(100);
	}else{*/
	/*uint8_t helligkeit = 20;
	if(RPM_THRES_1 <= rev){
		analogWrite(led1, helligkeit);
	}else{
		analogWrite(led1, 0);
	}
	if(RPM_THRES_2 <= rev){
		analogWrite(led2, helligkeit);
	}else{
		analogWrite(led2, 0);
	}
	if(RPM_THRES_3 <= rev){
		analogWrite(led3, helligkeit);
	}else{
		analogWrite(led3, 0);
	}
	if(RPM_THRES_4 <= rev){
		analogWrite(led4, helligkeit);
	}else{
		analogWrite(led4, 0);
	}
	if(RPM_THRES_5 <= rev){
		analogWrite(led5, helligkeit);
	}else{
		analogWrite(led5, 0);
	}
	if(RPM_THRES_6 <= rev){
		analogWrite(led6, helligkeit);
	}else{
		analogWrite(led6, 0);
	}
	if(RPM_THRES_7 <= rev){
		analogWrite(led7, helligkeit);
	}else{
		analogWrite(led7, 0);
	}	
	if(RPM_THRES_8 <= rev){
		analogWrite(led8, helligkeit);
	}else{
		analogWrite(led8, 0);
	}
	if(RPM_THRES_9 <= rev){
		analogWrite(led9, helligkeit);
	}else{
		analogWrite(led9, 0);
	}
	if(RPM_THRES_10 <= rev){
		analogWrite(led10, helligkeit);
	}else{
		analogWrite(led10, 0);
	}*/
	digitalWrite(led1, RPM_THRES_1 <= rev);
	digitalWrite(led2, RPM_THRES_2 <= rev);
	digitalWrite(led3, RPM_THRES_3 <= rev);
	digitalWrite(led4, RPM_THRES_4 <= rev);
	digitalWrite(led5, RPM_THRES_5 <= rev);
	digitalWrite(led6, RPM_THRES_6 <= rev);
	digitalWrite(led7, RPM_THRES_7 <= rev);
	digitalWrite(led8, RPM_THRES_8 <= rev);
	digitalWrite(led9, RPM_THRES_9 <= rev);
	digitalWrite(led10, RPM_THRES_10 <= rev);

	digitalWrite(led11, t_mot);		// rot,		links, oben
	digitalWrite(led12, t_air);		// rot,		links, mitte
	digitalWrite(led13, t_oil);		// rot,		links, unten
	
	digitalWrite(led14, e_dros);	// rot,		rechts, oben
	digitalWrite(led15, u_batt);	// rot		rechts, mitte
	digitalWrite(led16, g_auto);	// blau		rechts, unten
	/*if(Vehicle_data.g_auto){
		digitalWrite(led16, HIGH);  
	}else{
		digitalWrite(led16, LOW);  
	}*/
}