/* IKW1281Connection.c — byte pump, RX FIFO, no HAL_Delay() in hot path */

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "IKW1281Connection.h"
#include "main.h"

extern UART_HandleTypeDef huart1;

uint8_t K_TxData[KLINE_BUFFER_SIZE];
volatile uint8_t rx_done_flag = 0;

// ---------- RX FIFO ----------
static uint8_t rx_fifo[KLINE_RX_FIFO_SZ];
static volatile uint16_t rx_head = 0, rx_tail = 0;

static inline int rx_fifo_push(uint8_t b){
    uint16_t next = (uint16_t)((rx_head + 1U) % KLINE_RX_FIFO_SZ);
    if (next == rx_tail) rx_tail = (uint16_t)((rx_tail + 1U) % KLINE_RX_FIFO_SZ); // drop oldest
    rx_fifo[rx_head] = b;
    rx_head = next;
    rx_done_flag = 1;
    return 1;
}
int KLine_RxFifo_Pop(uint8_t *out){
    if (rx_head == rx_tail) return 0;
    uint8_t b = rx_fifo[rx_tail];
    rx_tail = (uint16_t)((rx_tail + 1U) % KLINE_RX_FIFO_SZ);
    if (out) *out = b;
    return 1;
}

// ---------- Non-blocking TX "byte pump" ----------
volatile KTxEngine ktx = {0};
static uint8_t tx_byte_1;

static uint8_t _packetCounter = 0;
static uint8_t _packetCounterInitialized = 0;


void KLine_BytePump_Init(void){ memset((void*)&ktx, 0, sizeof(ktx)); }

int KLine_TxStart(const uint8_t *data, uint16_t len, uint8_t append_end, uint8_t require_ack){
    if (ktx.active==1) return 0;
    if (len > KLINE_TXBUF_MAX) {
            // defensive: refuse too-long sends (KWP1281 blocks are small, so this is fine)
            return 0;
        }
    memcpy(ktx.ibuf, data, len);
        ktx.buf        = ktx.ibuf;
        ktx.using_ibuf = 1;
    ktx.active = 1; ktx.len = len; ktx.idx = 0;
    ktx.append_end = append_end; ktx.last_tx = 0; ktx.awaiting_echo = 0;
    ktx.require_ack = require_ack; ktx.tx_inflight = 0; ktx.next_time_ms = HAL_GetTick();
    return 1;
}
int KLine_TxBusy(void){ return ktx.active || ktx.tx_inflight || ktx.awaiting_echo; }

void KLine_BytePump_Service(void){
    if (!ktx.active) return;
    uint32_t now = HAL_GetTick();
    if (ktx.awaiting_echo) return;
    //if (ktx.tx_inflight)  return;
    if ((int32_t)(now - ktx.next_time_ms) < 0) return;

    uint8_t b;
    if (ktx.idx < ktx.len) b = ktx.buf[ktx.idx++];
    else if (ktx.append_end) {
    	b = PACKET_END_EXPECTED;
    	ktx.append_end = 0;
        ktx.active        = 2; //ENDING
    }
    else {         // === FINISHED: make sure state is fully reset ===
        ktx.active        = 0;
        //ktx.awaiting_echo  = 0;   // <— add //TODO
        //ktx.tx_inflight   = 0;   // <— add
        // (optional) ktx.next_time_ms = now;
        return;
    }

    tx_byte_1 = b;
    if (HAL_UART_Transmit_IT(&huart1, &tx_byte_1, 1) == HAL_OK){
        //ktx.tx_inflight = 1;
        ktx.last_tx     = b;
        if (ktx.require_ack && ktx.active == 1){
            ktx.awaiting_echo = 1;         // expect tester's ~b
        } else {
            ktx.awaiting_echo = 0;
        }
        ktx.next_time_ms = now + KWP_P1_GAP_MS;

    }
}
//int pepe = 0;
// Called from HAL_UART_RxCpltCallback (kline.c)
void KLine_OnByteReceived(uint8_t byte)
{

    uint32_t now = HAL_GetTick();

    // ---- Self-echo suppression: only within a tiny time window after our TX ----
    if(ktx.active){
    	if ((uint32_t)(now - ktx.last_tx_done_ms) <= KWP_ECHO_SUPPRESS_MS && byte == ktx.last_tx) {
			if(ktx.active == 2){
				ktx.active = 0;
				ktx.awaiting_echo = 0;
			}
			return;  // echo of our own last TX byte
			//no se bien si me quita el echo o el counter, habra que ver despues cuando los mensajes no sean de 03
		}
    }
    /*if(pepe==2){
    	pepe++;
    }*/

	/*if(ktx.awaiting_ack && byte == ktx.last_tx){
		return;
	}*/

    // ---- Tester complement for our TX byte? consume only if it matches ~last_tx ----
    if (ktx.active == 1) {
    	if(ktx.awaiting_echo){
    		if (byte == (uint8_t)~ktx.last_tx) {
				ktx.awaiting_echo = 0;
				//return; // consumed tester's complement
			} else {
				// Not a complement: treat as inbound payload, drop awaiting flag to resync
				ktx.awaiting_echo = 0;
				//ktx.active        = 0; //we are done, communication failed drop message.
				//return;
				// fall through to inbound handling
			}
    	}
    }
    else{
    	//WriteComplement(byte); //then its inboud payload, echo
        rx_fifo_push(byte);
    }

    // ---- Inbound byte from tester: ACK it immediately so we never race timing ----
    // (Tiny 1-byte blocking write; safe and deterministic at 9600 bps.)
    //WriteComplement(byte);

    // Push for higher-level parsing
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart){
    if (huart->Instance == USART1) {
        //ktx.tx_inflight    = 0;
        ktx.last_tx_done_ms = HAL_GetTick();   // <-- NEW
    }
}

// ---------- Legacy/compat API (kept) ----------
uint8_t ReadByte(){
    uint32_t timeout = HAL_GetTick() + 1000;
    uint8_t b;
    while (!KLine_RxFifo_Pop(&b)){
        if ((int32_t)(HAL_GetTick() - timeout) >= 0) return 0xFF;
    }
    return b;
}
uint8_t ReadAndAckByte(void){ uint8_t b = ReadByte(); WriteComplement(b); return b; }
void WriteComplement(uint8_t b){
	uint8_t c = (uint8_t)~b;
	WriteByteRaw(c); }

void WriteByteRaw(uint8_t b){
	ktx.active = 0;
    KLine_TxStart(&b, 1, 0, 0);
    while (KLine_TxBusy()) KLine_BytePump_Service();

	/*ktx.last_tx = b;
	ktx.active = 2;

	pepe++;
	if(b == 0x01){
		pepe++;
	}

    extern UART_HandleTypeDef huart1;
    (void)HAL_UART_Transmit(&huart1, &b, 1, 5);*/
}

#define MAX_PACKETS 16
static ParsedPacket packets_buffer[MAX_PACKETS];

ParsedPacket* ReceivePackets(int *out_count){
    int count = 0;
    while (1){
        if (count >= MAX_PACKETS) break;
        ParsedPacket p = ReceivePacket();
        packets_buffer[count++] = p;
        if (p.isAckNak) break;
        SendAckPacket();
    }
    if (out_count) *out_count = count;
    return packets_buffer;
}



ParsedPacket ReceivePacket(){
    ParsedPacket packet = (ParsedPacket){0};
    uint8_t idx = 0;

    uint8_t packetLength  = ReadAndAckByte(); packet.raw[idx++] = packetLength;
    uint8_t packetCounter = ReadPacketCounter(); packet.raw[idx++] = packetCounter;
    uint8_t packetCommand = ReadAndAckByte(); packet.raw[idx++] = packetCommand;

    for (int i=0;i<packetLength-3;i++){
        uint8_t b = ReadAndAckByte(); packet.raw[idx++] = b;
    }
    uint8_t end = ReadByte(); packet.raw[idx++] = end; packet.length = idx;
    if (end != PACKET_END_EXPECTED) return packet;

    packet.title = packetCommand;
    switch (packetCommand){
        case PACKET_CMD_ACK: packet.type = PACKET_TYPE_ACK; packet.isAckNak = 1; break;
        case PACKET_CMD_NAK: packet.type = PACKET_TYPE_NAK; packet.isAckNak = 1; break;
        case PACKET_CMD_AsciiData:
            packet.type = (packet.raw[3] == 0x00) ? PACKET_TYPE_CODING_WSC : PACKET_TYPE_ASCII_DATA; break;
        case PACKET_CMD_ReadEepromResponse: packet.type = PACKET_TYPE_READ_EEPROM_RESPONSE; break;
        case PACKET_CMD_ReadRomEepromResponse: packet.type = PACKET_TYPE_READ_ROM_EEPROM_RESPONSE; break;
        default: packet.type = PACKET_TYPE_UNKNOWN; break;
    }
    return packet;
}
void ResetPacketCounter(){ _packetCounter = 0; _packetCounterInitialized = 0; }
uint8_t ReadPacketCounter(){
    uint8_t v = ReadAndAckByte();
    if (!_packetCounterInitialized){ _packetCounter = v; _packetCounterInitialized = 1; }
    else if (v != _packetCounter){ /* optional mismatch handling */ }
    _packetCounter++;
    return v;
}
inline int32_t tick_diff(uint32_t a, uint32_t b){ return (int32_t)(a - b); }

int ReadByte_Tmo(uint32_t ms, uint8_t *out){
    uint32_t deadline = HAL_GetTick() + ms; uint8_t b;
    while (!KLine_RxFifo_Pop(&b)){
        if ((int32_t)(HAL_GetTick() - deadline) >= 0) return 0;
    }
    if (out) *out = b; return 1;
}
int ReadPacketCounter_Tmo(uint32_t ms, uint8_t *out){
    uint8_t v; if (!ReadByte_Tmo(ms, &v)) return 0;
    if (!_packetCounterInitialized){ _packetCounter = v; _packetCounterInitialized = 1; }
    else if (v != _packetCounter){ _packetCounter = v+1; return 0; }
    _packetCounter++; WriteComplement(v);
    if (out) *out = v; return 1;
}
int ReadAndAckByte_Tmo(uint32_t ms, uint8_t *out){
    uint8_t b; if (!ReadByte_Tmo(ms, &b)) return 0; WriteComplement(b); if (out) *out = b; return 1;
}

void SendAckPacket(){ uint8_t a = (uint8_t)PACKET_CMD_ACK; SendPacket(&a, 1); }
void SendPacket(uint8_t* payload, uint8_t length){
    uint8_t packetLength = (uint8_t)(length + 2);
    static uint8_t packet[1 + 1 + MAX_PACKET_SIZE + 1];
    int idx = 0;
    packet[idx++] = packetLength;
    packet[idx++] = _packetCounter++;
    for (int i=0;i<length;i++) packet[idx++] = payload[i];
    KLine_TxStart(packet, idx, 1 /*append 0x03*/, 1 /*expect complement*/);
    while (KLine_TxBusy()) KLine_BytePump_Service(); // cooperative wait (no HAL_Delay)
}

// Convenience used by your code
ParsedPacket* SendCustom(const uint8_t* data, int len, int *out_count, uint8_t isBRKT){
    (void)isBRKT; SendPacket((uint8_t*)data, len);
    int count = 0; ParsedPacket* p = ReceivePackets(&count);
    if (out_count) *out_count = count; return p;
}