/*
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()
{
	Serial.begin(9600);
	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(CAN_CELL_STATS_ID);
	Can0.watchFor(CAN_BATTERY_STATS_ID);
	Can0.watchFor(CAN_COOLING_STATS_ID);
	Can0.watchFor(CAN_INVERTER_STATS_ID);
	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.button_ll << 0;
	can_0_temp_data |= Stw_data.button_lr << 1;
	can_0_temp_data |= Stw_data.button_rl << 2;
	can_0_temp_data |= Stw_data.button_rr << 3;
	can_0_msg.data.byte[0] = can_0_temp_data;
	can_0_msg.data.byte[1] = Stw_data.mode;
	Can0.sendFrame(can_0_msg);
}

void Receive_Can_0(CAN_FRAME *temp_message)
{
	switch (temp_message->id)
	{
	case CAN_CELL_STATS_ID:
		process_cell_stats(temp_message);
		break;
	case CAN_BATTERY_STATS_ID:
		process_battery_stats(temp_message);
		break;
	case CAN_COOLING_STATS_ID:
		process_cooling_stats(temp_message);
		break;
	case CAN_INVERTER_STATS_ID:
		process_inverter_stats(temp_message);
		break;
	default:
		// TODO: How to handle this in the car?
		Serial.print("ERROR: Unknown CAN ID: ");
		Serial.println(temp_message->id);
	}
}

void process_cell_stats(CAN_FRAME *frame)
{
	CellStats *data = (CellStats *)&frame->data;
	Vehicle_data.t_cell_max = data->max_cell_temp;
	Vehicle_data.u_cell_min = data->min_cell_voltage;
}

void process_battery_stats(CAN_FRAME *frame)
{
	BatteryStats *data = (BatteryStats *)&frame->data;
	Vehicle_data.u_batt = data->pre_air_voltage;
}

void process_cooling_stats(CAN_FRAME *frame)
{
	CoolingStats *data = (CoolingStats *)&frame->data;
	Vehicle_data.p_wat = data->water_pressure;
	Vehicle_data.t_wat = data->water_temp;
	Vehicle_data.t_mot_l = data->motor_l_temp;
	Vehicle_data.t_mot_r = data->motor_r_temp;
}

void process_inverter_stats(CAN_FRAME *frame)
{
	InverterStats *data = (InverterStats *)&frame->data;
	uint8_t status = data->status;
	Vehicle_data.inverter.ready = status & CAN_INVERTER_STATS_READY;
	Vehicle_data.inverter.derating = status & CAN_INVERTER_STATS_DERATING;
	Vehicle_data.inverter.warning = status & CAN_INVERTER_STATS_WARNING;
	Vehicle_data.inverter.error = status & CAN_INVERTER_STATS_ERROR;
	Vehicle_data.inverter.on = status & CAN_INVERTER_STATS_ON;
	Vehicle_data.inverter.precharge = status & CAN_INVERTER_STATS_PRECHARGE;
	Vehicle_data.inverter.ams_emerg = status & CAN_INVERTER_STATS_AMS_EMERG;
	Vehicle_data.inverter.ts_active = status & CAN_INVERTER_STATS_TS_ACTIVE;
	Vehicle_data.t_inv = data->temp;
	Vehicle_data.revol = data->velocity;
	Vehicle_data.wheel_speed = data->wheel_speed;
}

void update_LED()
{
	bool t_mot = (Vehicle_data.t_mot_l > LED_THRESH_T_MOT) || (Vehicle_data.t_mot_r > LED_THRESH_T_MOT);
	bool t_inv = Vehicle_data.t_inv > LED_THRESH_T_INV;
	bool t_bat = Vehicle_data.t_cell_max > LED_THRESH_T_BAT;

	bool precharge_active = !Vehicle_data.inverter.precharge;
	bool derating = Vehicle_data.inverter.derating;
	bool u_batt = Vehicle_data.u_cell_min < LED_THRESH_U_BATT;

	digitalWrite(led11, t_mot); // rot,		links, oben
	digitalWrite(led12, t_inv); // rot,		links, mitte
	digitalWrite(led13, t_bat); // rot,		links, unten

	digitalWrite(led14, precharge_active); // rot,		rechts, oben
	digitalWrite(led15, derating);		   // rot		rechts, mitte
	digitalWrite(led16, u_batt);		   // blau		rechts, unten

	bool rev_lim = Vehicle_data.rev_lim;

	int16_t rev = Vehicle_data.revol;
	digitalWrite(led1, RPM_THRESH_1 <= rev);
	digitalWrite(led2, RPM_THRESH_2 <= rev);
	digitalWrite(led3, RPM_THRESH_3 <= rev);
	digitalWrite(led4, RPM_THRESH_4 <= rev);
	digitalWrite(led5, RPM_THRESH_5 <= rev);
	digitalWrite(led6, RPM_THRESH_6 <= rev);
	digitalWrite(led7, RPM_THRESH_7 <= rev);
	digitalWrite(led8, RPM_THRESH_8 <= rev);
	digitalWrite(led9, RPM_THRESH_9 <= rev);
	digitalWrite(led10, RPM_THRESH_10 <= rev);
}