/*
 * j1939.c
 *
 *  Created on: May 3, 2024
 *      Author: colorbass
 */

#include "main.h"
#include "j1939.h"
#include "charger_gbt.h"
#include "string.h"
#include "can.h"

extern GBT_BCL_t GBT_ReqPower;
extern GBT_BCL_t GBT_CurrPower;

j_receive_t j_rx;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef RxHeader;
	uint8_t RxData[8] = {0,};

    if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK)
    {
    	if((RxHeader.ExtId & 0x00FFFF) == ((J_ID_SE << 8) | J_ID_EV)){  // SA, DA match
    		switch ((RxHeader.ExtId>>8) & 0x00FF00){

    		case 0xEC00: //PGN Connection Management Message
    			if(RxData[0] == 16){ //Request to Send
					/* Set the RTS values */
    				j_rx.size = RxData[1] | (RxData[2]<<8);
    				j_rx.packet = 1;
    				j_rx.packets = RxData[3];
    				j_rx.step = 2; //TODO
    				j_rx.step_cts_remain = j_rx.step;
    				j_rx.PGN = (RxData[7] << 16) | (RxData[6] << 8) | RxData[5];
    				if(j_rx.size<256) { //TODO: valid check
    					J_SendCTS(j_rx);
    					j_rx.state = 1;
    				}
    			}
    			if(RxData[0] == 255){ //Connection Abort
    				j_rx.state = 0;
    			}

    			//if(RxData[0] == 32){}//Broadcast Announce Message
    			/*
    			 * 1CEC56F4  10 31 00 07 07 00 02 00
    			 * 1CECF456  11 02 01 FF FF 00 02 00
    			 * 1CEB56F4  01 01 01 00 03 46 05 40
    			 * 1CEC56F4  FF FF FF FF FF 00 00 00
    			 */

    			break;

    		case 0xEB00: //PGN Data Message
    			if(j_rx.state != 1) break;
    			if((RxData[0]>0) && (RxData[0]<35)){ //Array limit check
    				if(j_rx.packet == RxData[0]){	//step check
    					memcpy (&j_rx.data[(RxData[0]-1)*7], &RxData[1],7);
    					j_rx.packet++;
						if(j_rx.packet > j_rx.packets){
							//End of transmission
							J_SendACK(j_rx);

							j_rx.state = 2;
						}else{
							if(j_rx.step_cts_remain > 0) j_rx.step_cts_remain--;
							if(j_rx.step_cts_remain == 0){
								J_SendCTS(j_rx);
								j_rx.step_cts_remain = 2;
							}
						}
    				}
    			}
    			break;

    		case 0x1E00: //PGN BEM (ERROR)
			//Error force stop
    			GBT_ForceStop();
    			break;

    		case 0x1900: //PGN BST (STOP)
    			//Normal stop
    			GBT_Stop(GBT_CST_BMS_ACTIVELY_SUSPENDS);

    			break;

    		default:
    			if(j_rx.state == 0){//TODO protections
					//Short packet
					j_rx.size = RxHeader.DLC;
					j_rx.packet = 1;
					j_rx.packets = 1;
					j_rx.step = 1;
					j_rx.step_cts_remain = 0;
					j_rx.PGN = (RxHeader.ExtId>>8) & 0x00FF00;
					j_rx.state = 2;
					memcpy (j_rx.data, RxData, j_rx.size);
    			}
    		}
    	}
    }
}

void GBT_CAN_ReInit(){
	HAL_CAN_Stop(&hcan1);
	MX_CAN1_Init();
	HAL_CAN_Start(&hcan1);
	HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
	GBT_CAN_FilterInit();
}

void J_SendPacket(uint32_t PGN, uint8_t pri, uint8_t DLC, uint8_t *data){

        CAN_TxHeaderTypeDef tx_header;
        uint32_t tx_mailbox;

        tx_header.ExtId = (pri << 26) | (PGN << 8) | (J_ID_EV << 8) | J_ID_SE;
        tx_header.RTR = CAN_RTR_DATA;
        tx_header.IDE = CAN_ID_EXT;
        tx_header.DLC = DLC;

        HAL_CAN_AddTxMessage(&hcan1, &tx_header, data, &tx_mailbox);
        //HAL_Delay(2);

}

//void J_SendPacketLong(){
//	//TODO (no need)
//}

// J1939 sequence Clear To Send packet
void J_SendCTS(j_receive_t rx){

	//if(rx.packets <= rx.packet) return; TODO
    uint8_t data[8];
	data[0] = 17; //CONTROL_BYTE_TP_CM_CTS
	data[1] = rx.step;//total_number_of_packages_transmitted
	if (rx.step > (rx.packets - rx.packet+1)) data[1] = rx.packets - rx.packet+1;
	data[2] = rx.packet;//next_packet_number_transmitted
	data[3] = 0xFF; /* Reserved */
	data[4] = 0xFF;
	data[5] = rx.PGN;
	data[6] = rx.PGN >> 8;
	data[7] = rx.PGN >> 16;

	J_SendPacket(0x00EC00, 7, 8, data);
}

// J1939 sequence ACK packet
void J_SendACK(j_receive_t rx){//uint32_t PGN, uint8_t step, uint8_t packet){

    uint8_t data[8];
	data[0] = 19; //CONTROL_BYTE_TP_CM_ACK
	data[1] = j_rx.size;
	data[2] = j_rx.size>>8;
	data[3] = j_rx.packets;
	data[4] = 0xFF;//TODO
	data[5] = rx.PGN;
	data[6] = rx.PGN >> 8;
	data[7] = rx.PGN >> 16;

	J_SendPacket(0x00EC00, 7, 8, data);
}

void GBT_CAN_FilterInit(){
	  CAN_FilterTypeDef  sFilterConfig;

	  sFilterConfig.FilterBank = 0;
	  sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
	  sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
	  sFilterConfig.FilterIdHigh = 0x0000;
	  sFilterConfig.FilterIdLow = 0x0000;
	  sFilterConfig.FilterMaskIdHigh = 0x0000;
	  sFilterConfig.FilterMaskIdLow = 0x0000;
	  sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
	  sFilterConfig.FilterActivation = ENABLE;
	  //sFilterConfig.SlaveStartFilterBank = 14;
	  if(HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
	  {
	    Error_Handler();
	  }

}