/*
 * 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"
#include "edcan.h"
#include "debug.h"
#include <string.h>

extern GBT_BCL_t GBT_ReqPower;
extern GBT_BCL_t GBT_CurrPower;

j_receive_t j_rx;

typedef struct{
	uint8_t data[256];
	uint32_t PGN;
	uint16_t size;
	uint8_t packets;
	uint8_t next_packet;
	uint8_t step;
	uint8_t state; // 0=idle, 1=wait CTS, 2=wait ACK
	uint32_t tick;
}j_transmit_t;

static j_transmit_t j_tx;

#define J1939_BUFFER_SIZE 64U

typedef struct {
	j1939_rx_frame_t buffer[J1939_BUFFER_SIZE];
	uint16_t head;
	uint16_t tail;
	uint16_t count;
	uint32_t overflow;
} j1939_rx_ring_t;

typedef struct {
	j1939_tx_frame_t buffer[J1939_BUFFER_SIZE];
	uint16_t head;
	uint16_t tail;
	uint16_t count;
	uint8_t busy;
	uint32_t overflow;
} j1939_tx_ring_t;

static j1939_rx_ring_t j_rxbuf;
static j1939_tx_ring_t j_txbuf;
static uint32_t j_tx_watchdog_last_log_tick;

#define J1939_CRITICAL_ENTER() __disable_irq()
#define J1939_CRITICAL_EXIT() __enable_irq()

static void J_SendTpRts(void);
static void J_SendTpDtRange(uint8_t start_packet, uint8_t count);
static void J_TxCheckTimeout(void);
static void J_TxReset(void);
static void J1939_ProcessRxFrame(const j1939_rx_frame_t *frame);
static void J1939_RxBufferAdd(const j1939_rx_frame_t *frame);
static uint8_t J1939_RxBufferGet(j1939_rx_frame_t *frame);
static void J1939_TxBufferAdd(const j1939_tx_frame_t *frame);
static uint8_t J1939_TxBufferPeek(j1939_tx_frame_t *frame);
static void J1939_TxBufferDropFirst(void);
static void J1939_TxQueueByPgn(uint32_t PGN, uint8_t pri, uint8_t DLC, const uint8_t *data, uint8_t pad_ff);

#define J1939_TP_CTS_WAIT_TIMEOUT_MS 1000U
#define J1939_TP_ACK_WAIT_TIMEOUT_MS 1000U

static void J_TxReset(void){
	if(j_tx.state != 0){
		log_printf(LOG_WARN, "J1939 chunk send failed: TX session reset (state=%u pgn=0x%lX next=%u/%u)\n",
				j_tx.state, j_tx.PGN, j_tx.next_packet, j_tx.packets);
	}
	memset(&j_tx, 0, sizeof(j_tx));
}

void J1939_InitBuffers(void){
	memset(&j_rxbuf, 0, sizeof(j_rxbuf));
	memset(&j_txbuf, 0, sizeof(j_txbuf));
	j_tx_watchdog_last_log_tick = 0U;
	J_TxReset();
}

uint16_t J1939_GetRxBufferCount(void){
	uint16_t c;
	J1939_CRITICAL_ENTER();
	c = j_rxbuf.count;
	J1939_CRITICAL_EXIT();
	return c;
}

uint16_t J1939_GetTxBufferCount(void){
	uint16_t c;
	J1939_CRITICAL_ENTER();
	c = j_txbuf.count;
	J1939_CRITICAL_EXIT();
	return c;
}

uint32_t J1939_GetRxOverflowCount(void){
	return j_rxbuf.overflow;
}

uint32_t J1939_GetTxOverflowCount(void){
	return j_txbuf.overflow;
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
	j1939_rx_frame_t frame;
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8] = {0};

	if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) != HAL_OK) return;

	frame.ExtId = rx_header.ExtId;
	frame.DLC = rx_header.DLC;
	frame.tick = HAL_GetTick();
	memcpy(frame.data, rx_data, sizeof(frame.data));
	J1939_RxBufferAdd(&frame);
}

static void J1939_ProcessRxFrame(const j1939_rx_frame_t *frame){
	uint32_t pgn;

	if((frame->ExtId & 0x00FFFF) != ((J_ID_EV << 8) | J_ID_SE)) return;  // SA, DA match

	switch ((frame->ExtId >> 8) & 0x00FF00){
	case 0xEC00:
		if(frame->data[0] == 16){ // RTS
			j_rx.size = frame->data[1] | (frame->data[2] << 8);
			j_rx.packet = 1;
			j_rx.packets = frame->data[3];
			j_rx.step = 2;
			j_rx.step_cts_remain = j_rx.step;
			j_rx.PGN = (frame->data[7] << 16) | (frame->data[6] << 8) | frame->data[5];
			if(j_rx.size < 256){
				J_SendCTS(j_rx);
				j_rx.state = 1;
			}
		}
		if(frame->data[0] == 17){ // CTS
			pgn = (frame->data[7] << 16) | (frame->data[6] << 8) | frame->data[5];
			if((j_tx.state == 1) && (pgn == j_tx.PGN)){
				uint8_t req_count = frame->data[1];
				uint8_t req_start_packet = frame->data[2];
				log_printf(LOG_DEBUG, "J1939 event: TP CTS received (pgn=0x%lX cnt=%u start=%u exp=%u)\n",
						pgn, req_count, req_start_packet, j_tx.next_packet);
				if(req_count == 0){
					log_printf(LOG_DEBUG, "J1939 event: TP CTS wait (pgn=0x%lX)\n", j_tx.PGN);
				}else{
					j_tx.tick = frame->tick;
					J_SendTpDtRange(req_start_packet, req_count);
				}
			}else{
				log_printf(LOG_WARN, "J1939 chunk send failed: CTS ignored (tx_state=%u tx_pgn=0x%lX rx_pgn=0x%lX)\n",
						j_tx.state, j_tx.PGN, pgn);
			}
		}
		if(frame->data[0] == 19){ // ACK
			pgn = (frame->data[7] << 16) | (frame->data[6] << 8) | frame->data[5];
			if((j_tx.state == 2) && (pgn == j_tx.PGN)){
				log_printf(LOG_DEBUG, "J1939 event: TP ACK received, TX complete (pgn=0x%lX)\n", pgn);
				j_tx.state = 0;
			}
		}
		if(frame->data[0] == 255){
			j_rx.state = 0;
			J_TxReset();
		}
		break;

	case 0xEB00:
		if(j_rx.state != 1) return;
		if((frame->data[0] > 0) && (frame->data[0] < 35) && (j_rx.packet == frame->data[0])){
			memcpy(&j_rx.data[(frame->data[0]-1)*7], &frame->data[1], 7);
			j_rx.packet++;
			if(j_rx.packet > j_rx.packets){
				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:
		log_printf(LOG_WARN, "J1939 fault received: BEM, forcing stop (data=%02X %02X %02X %02X %02X %02X %02X %02X)\n",
				frame->data[0], frame->data[1], frame->data[2], frame->data[3],
				frame->data[4], frame->data[5], frame->data[6], frame->data[7]);
		GBT_ForceStop();
		break;

	case 0x1900:
		/* Repeated BST frames during STOP flow are normal on the bus.
		 * Handle/log only once to avoid flooding UART logs.
		 */
		if((GBT_State != GBT_STOP) && (GBT_State != GBT_STOP_CSD) && (GBT_State != GBT_COMPLETE)){
			log_printf(LOG_DEBUG, "J1939 event: BST received, initiating stop flow (data=%02X %02X %02X %02X %02X %02X %02X %02X)\n",
					frame->data[0], frame->data[1], frame->data[2], frame->data[3],
					frame->data[4], frame->data[5], frame->data[6], frame->data[7]);
			GBT_StopEV(GBT_CST_BMS_ACTIVELY_SUSPENDS);
		}
		break;

	default:{
		uint32_t short_pgn = (frame->ExtId >> 8) & 0x00FF00U;
		/* Однокадровые PGN обрабатываем сразу; j_rx зарезервирован только под TP RX. */
		if((short_pgn != 0xEC00U) && (short_pgn != 0xEB00U)){
			GBT_ApplyShortPacket(short_pgn, frame->data, frame->DLC);
		}
		break;
	}
	}
}

void J1939_ExchangeRxBuffer(void){
	j1939_rx_frame_t frame;
	J_TxCheckTimeout();
	if(J1939_RxBufferGet(&frame)){
		J1939_ProcessRxFrame(&frame);
	}
}

void J1939_ExchangeTxBuffer(void){
	j1939_tx_frame_t frame;
	CAN_TxHeaderTypeDef tx_header;
	uint32_t tx_mailbox;
	HAL_StatusTypeDef tx_status;

	if(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) return;

	/* Atomically reserve first queued frame without holding IRQ over HAL call */
	J1939_CRITICAL_ENTER();
	if(j_txbuf.busy || (j_txbuf.count == 0U)){
		J1939_CRITICAL_EXIT();
		return;
	}
	j_txbuf.busy = 1U;
	frame = j_txbuf.buffer[j_txbuf.tail];
	J1939_CRITICAL_EXIT();

	tx_header.ExtId = frame.ExtId;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.IDE = CAN_ID_EXT;
	tx_header.DLC = frame.DLC;

	tx_status = HAL_CAN_AddTxMessage(&hcan1, &tx_header, frame.data, &tx_mailbox);

	J1939_CRITICAL_ENTER();
	if((tx_status == HAL_OK) && (j_txbuf.count > 0U)){
		j_txbuf.tail = (j_txbuf.tail + 1U) % J1939_BUFFER_SIZE;
		j_txbuf.count--;
	}
	j_txbuf.busy = 0U;
	J1939_CRITICAL_EXIT();

	if(tx_status != HAL_OK){
		log_printf(LOG_WARN, "J1939 send failed: CAN TX error (st=%d extid=0x%lX dlc=%u free_mb=%lu err=0x%lX)\n",
				(int)tx_status, frame.ExtId, frame.DLC, HAL_CAN_GetTxMailboxesFreeLevel(&hcan1), HAL_CAN_GetError(&hcan1));
		if((tx_status == HAL_ERROR) && (HAL_CAN_GetError(&hcan1) & HAL_CAN_ERROR_NOT_INITIALIZED)){
			log_printf(LOG_WARN, "J1939 send failed: CAN not initialized, reinitializing\n");
			GBT_CAN_ReInit();
		}
	}
}

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();
	J1939_InitBuffers();
}

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

        if(DLC > 8){
        	/* Transport protocol for multi-packet payloads */
        	if(DLC > sizeof(j_tx.data)) return;
        	if(j_tx.state != 0){
        		log_printf(LOG_WARN, "J1939 chunk send failed: TX busy (state=%u req_pgn=0x%lX cur_pgn=0x%lX)\n",
        				j_tx.state, PGN, j_tx.PGN);
        		return;
        	}

        	memset(&j_tx, 0, sizeof(j_tx));
        	memcpy(j_tx.data, data, DLC);
        	j_tx.PGN = PGN;
        	j_tx.size = DLC;
        	j_tx.packets = (DLC + 6) / 7;
        	j_tx.next_packet = 1;
        	j_tx.step = 2;
        	j_tx.state = 1;
        	j_tx.tick = HAL_GetTick();
        	log_printf(LOG_DEBUG, "J1939 event: TP RTS sent (pgn=0x%lX size=%u packets=%u)\n",
        			j_tx.PGN, j_tx.size, j_tx.packets);

        	J_SendTpRts();
        	return;
        }
        J1939_TxQueueByPgn(PGN, pri, DLC, data, 1);

}

static void J_TxCheckTimeout(void){
	if(j_tx.state != 0){
		uint32_t now = HAL_GetTick();
		if((now - j_tx_watchdog_last_log_tick) >= 500U){
			log_printf(LOG_DEBUG, "J1939 event: TP TX watchdog (state=%u pgn=0x%lX next=%u/%u age=%lu)\n",
					j_tx.state, j_tx.PGN, j_tx.next_packet, j_tx.packets, (now - j_tx.tick));
			j_tx_watchdog_last_log_tick = now;
		}
	}else{
		j_tx_watchdog_last_log_tick = HAL_GetTick();
	}

	if((j_tx.state == 1) && ((HAL_GetTick() - j_tx.tick) > J1939_TP_CTS_WAIT_TIMEOUT_MS)){
		log_printf(LOG_WARN, "J1939 chunk send failed: CTS timeout (pgn=0x%lX), resetting TX session\n", j_tx.PGN);
		J_TxReset();
	}else if((j_tx.state == 2) && ((HAL_GetTick() - j_tx.tick) > J1939_TP_ACK_WAIT_TIMEOUT_MS)){
		log_printf(LOG_WARN, "J1939 chunk send failed: ACK timeout (pgn=0x%lX), resetting TX session\n", j_tx.PGN);
		J_TxReset();
	}
}

static void J_SendTpRts(void){
	uint8_t data[8];

	data[0] = 16; // CONTROL_BYTE_TP_CM_RTS
	data[1] = j_tx.size & 0xFF;
	data[2] = j_tx.size >> 8;
	data[3] = j_tx.packets;
	data[4] = j_tx.step; // max packets before next CTS
	data[5] = j_tx.PGN;
	data[6] = j_tx.PGN >> 8;
	data[7] = j_tx.PGN >> 16;

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

static void J_SendTpDtRange(uint8_t start_packet, uint8_t count){
	uint8_t dt[8];
	uint8_t packet;
	uint8_t i;
	uint16_t idx;

	if(j_tx.state != 1){
		log_printf(LOG_WARN, "J1939 chunk send failed: TP DT ignored (invalid TX state=%u)\n", j_tx.state);
		return;
	}

	if((start_packet == 0) || (start_packet > j_tx.packets)) return;
	if(count == 0) return;
	if(start_packet != j_tx.next_packet){
		log_printf(LOG_WARN, "J1939 chunk send failed: TP desync (start=%u expected=%u), resetting TX session\n", start_packet, j_tx.next_packet);
		J_TxReset();
		return;
	}
	if((start_packet + count - 1) > j_tx.packets){
		count = j_tx.packets - start_packet + 1;
	}

	for(packet = 0; packet < count; packet++){
		uint8_t seq = start_packet + packet;
		dt[0] = seq;
		idx = (uint16_t)(seq - 1) * 7;
		for(i = 0; i < 7; i++){
			if((idx + i) < j_tx.size) dt[i + 1] = j_tx.data[idx + i];
			else dt[i + 1] = 0xFF;
		}
		J1939_TxQueueByPgn(0x00EB00, 7, 8, dt, 0);
	}
	log_printf(LOG_DEBUG, "J1939 event: TP DT sent (pgn=0x%lX start=%u cnt=%u)\n", j_tx.PGN, start_packet, count);

	j_tx.next_packet = start_packet + count;
	j_tx.tick = HAL_GetTick();
	if(j_tx.next_packet > j_tx.packets){
		j_tx.state = 2; // wait ACK
	}else{
		j_tx.state = 1; // wait next CTS
	}
}

//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();
	  }

}

static void J1939_RxBufferAdd(const j1939_rx_frame_t *frame){
	J1939_CRITICAL_ENTER();
	j_rxbuf.buffer[j_rxbuf.head] = *frame;
	j_rxbuf.head = (j_rxbuf.head + 1U) % J1939_BUFFER_SIZE;
	if(j_rxbuf.count == J1939_BUFFER_SIZE){
		j_rxbuf.tail = (j_rxbuf.tail + 1U) % J1939_BUFFER_SIZE;
		j_rxbuf.overflow++;
		log_printf(LOG_WARN, "J1939 RX buffer overflow: dropped oldest frame, overflow_count=%lu\n", j_rxbuf.overflow);
	}else{
		j_rxbuf.count++;
	}
	J1939_CRITICAL_EXIT();
}

static uint8_t J1939_RxBufferGet(j1939_rx_frame_t *frame){
	uint8_t ok = 0;
	J1939_CRITICAL_ENTER();
	if(j_rxbuf.count > 0){
		*frame = j_rxbuf.buffer[j_rxbuf.tail];
		j_rxbuf.tail = (j_rxbuf.tail + 1U) % J1939_BUFFER_SIZE;
		j_rxbuf.count--;
		ok = 1;
	}
	J1939_CRITICAL_EXIT();
	return ok;
}

static void J1939_TxBufferAdd(const j1939_tx_frame_t *frame){
	J1939_CRITICAL_ENTER();
	j_txbuf.buffer[j_txbuf.head] = *frame;
	j_txbuf.head = (j_txbuf.head + 1U) % J1939_BUFFER_SIZE;
	if(j_txbuf.count == J1939_BUFFER_SIZE){
		j_txbuf.tail = (j_txbuf.tail + 1U) % J1939_BUFFER_SIZE;
		j_txbuf.overflow++;
		log_printf(LOG_WARN, "J1939 TX buffer overflow: dropped oldest frame, overflow_count=%lu\n", j_txbuf.overflow);
	}else{
		j_txbuf.count++;
	}
	J1939_CRITICAL_EXIT();
}

static uint8_t J1939_TxBufferPeek(j1939_tx_frame_t *frame){
	uint8_t ok = 0;
	J1939_CRITICAL_ENTER();
	if(j_txbuf.count > 0){
		*frame = j_txbuf.buffer[j_txbuf.tail];
		ok = 1;
	}
	J1939_CRITICAL_EXIT();
	return ok;
}

static void J1939_TxBufferDropFirst(void){
	J1939_CRITICAL_ENTER();
	if(j_txbuf.count > 0){
		j_txbuf.tail = (j_txbuf.tail + 1U) % J1939_BUFFER_SIZE;
		j_txbuf.count--;
	}
	J1939_CRITICAL_EXIT();
}

static void J1939_TxQueueByPgn(uint32_t PGN, uint8_t pri, uint8_t DLC, const uint8_t *data, uint8_t pad_ff){
	j1939_tx_frame_t frame;
	uint8_t i;

	frame.ExtId = (pri << 26) | (PGN << 8) | (J_ID_SE << 8) | J_ID_EV;
	frame.DLC = DLC;
	for(i = 0; i < 8; i++){
		if(i < DLC) frame.data[i] = data[i];
		else frame.data[i] = pad_ff ? 0xFF : 0x00;
	}
	J1939_TxBufferAdd(&frame);
}