/*
 * mazda_can.c
 *
 *  Created on: Mar 17, 2022
 *      Author: v0stap
 */

/*
 * app.c
 *
 *  Created on: Jul 25, 2020
 *      Author: v0stap
 */

//int AVIsendCANAddress = 0x2C0;
//int DPIsendCANAddress1 = 0x2C2;
//int DPIsendCANAddress2 = 0x2C4;
//int DPOrxCANAddress1 = 0x2D0;
//int DPOrxCANAddress2 = 0x2D2;
//int KeepAliveCANAddress = 0x2C6;
#include "main.h"
void Haltech_App_Init(void) {
	// configure TIM2 to 1uS / tick timer
	// setup CCR interrupt happen after sturtup delay is over

}

void Haltech_App(uint32_t id) {
	if (CAN_RX_Rdy) {
		var.can_timeout = 0;
		CAN_Read_Msg(&CAN_RX_Msg);
		Haltech_Write_Digital(&CAN_RX_Msg);
	}
	if (var.can_timeout > 2) {

	}
	if (CAN_TX_Rdy) {
		if (var.Send_Data_IRQ) {

			if (var.Send_Data_IRQ == 4)
				Haltech_Send_Keap_Alive(id + 6);
			else if (var.Send_Data_IRQ == 3)
				Haltech_Send_Analog(id);
			else if (var.Send_Data_IRQ == 2)
				Haltech_Send_Digital1(id + 2);
			else if (var.Send_Data_IRQ == 1)
				Haltech_Send_Digital2(id + 4);
			var.Send_Data_IRQ--;
		}
	}
}

void Haltech_Send_Analog(uint32_t id) {

	Get_Analog_Var();

	CAN_TX_Msg.id = id;
	CAN_TX_Msg.frame = DATA_FRAME;
	CAN_TX_Msg.format = EXTD_FORMAT;
	CAN_TX_Msg.data[0] = (uint8_t) var.avi[0];
	CAN_TX_Msg.data[1] = (uint8_t) (var.avi[0] >> 8);
	CAN_TX_Msg.data[2] = (uint8_t) var.avi[1];
	CAN_TX_Msg.data[3] = (uint8_t) (var.avi[1] >> 8);
	CAN_TX_Msg.data[4] = (uint8_t) var.avi[2];
	CAN_TX_Msg.data[5] = (uint8_t) (var.avi[2] >> 8);
	CAN_TX_Msg.data[6] = (uint8_t) var.avi[3];
	CAN_TX_Msg.data[7] = (uint8_t) (var.avi[3] >> 8);
	CAN_TX_Msg.lenght = 8;
	CAN_Send_Msg(&CAN_TX_Msg);
}

void Haltech_Send_Digital1(uint32_t id) {
	if (var.tim1_timeout > 2) {
		NVIC_DisableIRQ(TIM1_CC_IRQn);
				if (GPIO_READ_PIN(GPIOB, GPIO_PIN_15)) {
					var.dvi_dt[0] = 0xff00;
					var.dvi_fr[0] = 0;
				} else {
					var.dvi_dt[0] = 0;
					var.dvi_fr[0] = 0;
				}
				NVIC_EnableIRQ(TIM1_CC_IRQn);
	}
	if (var.tim2_timeout > 2) {
		NVIC_DisableIRQ(TIM2_IRQn);
		if (GPIO_READ_PIN(GPIOB, GPIO_PIN_3)) {
			var.dvi_dt[1] = 0xff00;
			var.dvi_fr[1] = 0;
		} else {
			var.dvi_dt[1] = 0;
			var.dvi_fr[1] = 0;
		}
		NVIC_EnableIRQ(TIM2_IRQn);
	}
	CAN_TX_Msg.id = id;
	CAN_TX_Msg.frame = DATA_FRAME;
	CAN_TX_Msg.format = EXTD_FORMAT;
	CAN_TX_Msg.data[0] = (uint8_t) (var.dvi_dt[0]);
	CAN_TX_Msg.data[1] = (uint8_t) (var.dvi_dt[0] >> 8);
	CAN_TX_Msg.data[2] = (uint8_t) (var.dvi_fr[0]);
	CAN_TX_Msg.data[3] = (uint8_t) (var.dvi_fr[0] >> 8);
	CAN_TX_Msg.data[4] = (uint8_t) (var.dvi_dt[1]);
	CAN_TX_Msg.data[5] = (uint8_t) (var.dvi_dt[1] >> 8);
	CAN_TX_Msg.data[6] = (uint8_t) (var.dvi_fr[1]);
	CAN_TX_Msg.data[7] = (uint8_t) (var.dvi_fr[1] >> 8);
	CAN_TX_Msg.lenght = 8;
	CAN_Send_Msg(&CAN_TX_Msg);
}
void Haltech_Send_Digital2(uint32_t id) {
	if (var.tim3_timeout > 2) {
		var.dvi_dt[2] = 0xff00;
		var.dvi_fr[2] = 0;
	}
	if (var.tim15_timeout > 2) {
		NVIC_DisableIRQ(TIM15_IRQn);
				if (GPIO_READ_PIN(GPIOB, GPIO_PIN_13)) {
					var.dvi_dt[3] = 0xff00;
					var.dvi_fr[3] = 0;
				} else {
					var.dvi_dt[3] = 0;
					var.dvi_fr[3] = 0;
				}
				NVIC_EnableIRQ(TIM15_IRQn);
	}
	CAN_TX_Msg.id = id;
	CAN_TX_Msg.frame = DATA_FRAME;
	CAN_TX_Msg.format = EXTD_FORMAT;
	CAN_TX_Msg.data[0] = (uint8_t) (var.dvi_dt[2]);
	CAN_TX_Msg.data[1] = (uint8_t) (var.dvi_dt[2] >> 8);
	CAN_TX_Msg.data[2] = (uint8_t) (var.dvi_fr[2]);
	CAN_TX_Msg.data[3] = (uint8_t) (var.dvi_fr[2] >> 8);
	CAN_TX_Msg.data[4] = (uint8_t) (var.dvi_dt[3]);
	CAN_TX_Msg.data[5] = (uint8_t) (var.dvi_dt[3] >> 8);
	CAN_TX_Msg.data[6] = (uint8_t) (var.dvi_fr[3]);
	CAN_TX_Msg.data[7] = (uint8_t) (var.dvi_fr[3] >> 8);
	CAN_TX_Msg.lenght = 8;
	CAN_Send_Msg(&CAN_TX_Msg);
}

void Haltech_Send_Keap_Alive(uint32_t id) {
	CAN_TX_Msg.id = id;
	CAN_TX_Msg.frame = DATA_FRAME;
	CAN_TX_Msg.format = EXTD_FORMAT;
	CAN_TX_Msg.data[0] = 0X10;
	CAN_TX_Msg.data[1] = 0x09;
	CAN_TX_Msg.data[2] = 0x0a;
	CAN_TX_Msg.data[3] = 0x00;
	CAN_TX_Msg.data[4] = 0x00;
	CAN_TX_Msg.lenght = 5;

	CAN_Send_Msg(&CAN_TX_Msg);
}

void Haltech_Write_Digital(can_msg_typedef *section) {
	uint16_t temp_dt = 0, temp_fr = 0;
	if (section->id == 0x2D0) {
		//printf("\n Digital1");
		temp_fr = section->data[0] + (section->data[1] << 8);
		temp_dt = section->data[2] + (section->data[3] << 8);
		if (temp_fr != var.dvo_fr[0]) {
			var.dvo_fr[0] = temp_fr;
			TIM14->PSC = var.dvo_fr[0];
		}
		if (temp_dt != var.dvo_dt[0]) {
			if (temp_dt < 1000) {
				var.dvo_dt[0] = temp_dt;
				TIM14->CCR1 = var.dvo_dt[0];
			} else {
				var.dvo_dt[0] = 1000;
				TIM14->CCR1 = var.dvo_dt[0];
			}
		}
		temp_fr = section->data[4] + (section->data[5] << 8);
		temp_dt = section->data[6] + (section->data[7] << 8);
		if (temp_fr != var.dvo_fr[1]) {
			var.dvo_fr[1] = temp_fr;
			TIM16->PSC = var.dvo_fr[1];
		}
		if (temp_dt != var.dvo_dt[1]) {
			if (temp_dt < 1000) {
				var.dvo_dt[1] = temp_dt;
				TIM16->CCR1 = var.dvo_dt[1];
			} else {
				var.dvo_dt[1] = 1000;
				TIM16->CCR1 = var.dvo_dt[1];
			}
		}
	} else if (section->id == 0x2D2) {
		temp_fr = section->data[0] + (section->data[1] << 8);
		temp_dt = section->data[2] + (section->data[3] << 8);
		if (temp_fr != var.dvo_fr[2]) {
			var.dvo_fr[2] = temp_fr;
			TIM17->PSC = var.dvo_fr[2];
		}
		if (temp_dt != var.dvo_dt[2]) {
			if (temp_dt < 1000) {
				var.dvo_dt[2] = temp_dt;
				TIM17->CCR1 = var.dvo_dt[2];
			} else {
				var.dvo_dt[2] = 1000;
				TIM17->CCR1 = var.dvo_dt[2];
			}
		}
		temp_fr = section->data[4] + (section->data[5] << 8);
		temp_dt = section->data[6] + (section->data[7] << 8);
		if (temp_fr != var.dvo_fr[3]) {
			var.dvo_fr[3] = temp_fr;
			TIM7->PSC = var.dvo_fr[3];
		}
		if (temp_dt != var.dvo_dt[3]) {
			if (temp_dt < 1000) {
				var.dvo_dt[3] = temp_dt;

				//todo
			} else {
				var.dvo_dt[3] = 1000;
				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
			}
		}
	} else {
		//TODO Eror
		printf("\n Digital Error");
	}
}