#include "canform.h"

uint8_t raw_bits[MAX_BITS];     // До стаффинга
uint32_t raw_len = 0;

uint8_t can_bits[MAX_BITS]; // После стаффинга
uint32_t can_bits_len = 0;

#define MAX_ERR_BITS 32
uint32_t err_indices[MAX_ERR_BITS];
uint8_t err_indices_count = 0;


void append_bit(uint8_t *buf, uint32_t *len, uint8_t bit) {
    if (*len < MAX_BITS) {
        buf[(*len)++] = bit;
    }
}

void append_bits(uint8_t *buf, uint32_t *len, uint32_t value, uint8_t count) {
    for (int i = count - 1; i >= 0; i--) {
        append_bit(buf, len, (value >> i) & 1);
    }
}

uint16_t compute_crc15(const uint8_t *bits, uint32_t len) {
    uint16_t crc = 0;
    for (uint32_t i = 0; i < len; i++) {
        uint8_t bit = bits[i];
        uint8_t crc_msb = (crc >> 14) & 1;
        crc <<= 1;
        if (bit ^ crc_msb) {
            crc ^= 0x4599;
        }
        crc &= 0x7FFF; // Обрезаем до 15 бит
    }
    return crc;
}
int stuff_to_skip = 1;
void apply_bit_stuffing_with_error(const uint8_t *src, uint32_t src_len, uint8_t *dst, uint32_t *dst_len, uint8_t skip_stuff) {
  *dst_len = 0;
  uint8_t last_bit = src[0];
  uint8_t count = 1;
  append_bit(dst, dst_len, last_bit);

  for (uint32_t i = 1; i < src_len; i++) {
    uint8_t bit = src[i];
    append_bit(dst, dst_len, bit);

    if (bit == last_bit) {
      count++;
      if (count == 5) {
        if(skip_stuff != stuff_to_skip)
          append_bit(dst, dst_len, !bit); // вставляем противоположный бит
        count = 0;
        if (skip_stuff) {
          skip_stuff++;
        }        
      }
    } else {
      count = 1;
      last_bit = bit;
    }
  }
}

// Вспомогательная функция для инверсии бита
void flip_bit(uint32_t bit_index) {
    if (bit_index < raw_len) {
        raw_bits[bit_index] = !raw_bits[bit_index];
    }
}

void form_CAN_bitstream_full(const CAN_TxHeaderTypeDef *header, const uint8_t *data, CAN_ErrorFlags_t errors) {
  raw_len = 0;
  err_indices_count = 0;
  uint32_t data_idx  = 0xFF;
  uint32_t srr_idx   = 0xFF;
  uint32_t id_start  = 0xFF;
  uint32_t id2_start = 0xFF;
  uint32_t rtr_idx   = 0xFF;
  uint32_t ide_idx   = 0xFF;
  uint32_t r1_idx    = 0xFF;
  uint32_t r0_idx    = 0xFF;
  // 1. SOF (start of frame)
  append_bit(raw_bits, &raw_len, 0); // dominant

  // Запоминаем индексы для ошибок
  uint32_t idx_start = raw_len; // начало Arbitration + Control

  if (header->IDE == 0) {
      // --- Standard Frame (11-bit ID) ---
      // ID[10:0]
      append_bits(raw_bits, &raw_len, header->StdId & 0x7FF, 11);
      id_start = raw_len - 11; // начало ID

      // RTR
      append_bit(raw_bits, &raw_len, header->RTR);
      rtr_idx = raw_len - 1;

      // IDE = 0
      append_bit(raw_bits, &raw_len, 0);
      ide_idx = raw_len - 1;

      // r0
      append_bit(raw_bits, &raw_len, 0);
      r0_idx = raw_len - 1;


  } else {
      // --- Extended Frame (29-bit ID) ---
      uint32_t ext_id = header->ExtId & 0x1FFFFFFF;
      append_bits(raw_bits, &raw_len, (ext_id >> 18) & 0x7FF, 11);   // ID[28:18]
      id_start = raw_len - 11;

      // SRR
      append_bit(raw_bits, &raw_len, 1);
      srr_idx = raw_len - 1;

      // IDE = 1
      append_bit(raw_bits, &raw_len, 1);
      ide_idx = raw_len - 1;

      append_bits(raw_bits, &raw_len, (ext_id >> 0) & 0x3FFFF, 18);  // ID[17:0]
      id2_start = raw_len - 18;

      // RTR
      append_bit(raw_bits, &raw_len, header->RTR);
      rtr_idx = raw_len - 1;

      // r1
      append_bit(raw_bits, &raw_len, 0);
      r1_idx = raw_len - 1;

      // r0
      append_bit(raw_bits, &raw_len, 0);
      r0_idx = raw_len - 1;
  }

  
  // 3. DLC
  append_bits(raw_bits, &raw_len, header->DLC & 0xF, 4);

  // 4. Data field (если не RTR)
  if (!header->RTR) {
      uint8_t dlc = header->DLC & 0xF;
    
      if(dlc)
        data_idx = raw_len;
      
      for (uint8_t i = 0; i < dlc && i < 8; i++) {
          uint8_t byte_val = data[i];
          // Time-stamp override on last 2 bytes
          if (header->TransmitGlobalTime == ENABLE && i >= 6) {
              append_bits(raw_bits, &raw_len, 0x00, 8); // Will be replaced by timestamp elsewhere
          } else {
            append_bits(raw_bits, &raw_len, byte_val, 8);
          }
      }
  }
#ifdef CAN_ERROR_WITH_CORRECT_CRC
  // Ошибки
  if (errors.bit.FF_SRS) flip_bit(srr_idx);
  if (errors.bit.FF_IDE) flip_bit(ide_idx);
  if (errors.bit.FF_RTR) flip_bit(rtr_idx);
  if (errors.bit.FF_R1) flip_bit(r1_idx);
  if (errors.bit.FF_R0) flip_bit(r0_idx);
  if (errors.bit.DATA) flip_bit(data_idx);
  // MSGID_ERR — переворачиваем 1-й бит ID (MSB ID[10]) для примера
  if (errors.bit.MSGID) flip_bit(id_start); 
#endif
  
  
  // 5. CRC
  uint16_t crc = compute_crc15(raw_bits, raw_len);
  uint32_t crc_start = raw_len;
  append_bits(raw_bits, &raw_len, crc, 15);

  if (errors.bit.CRC_ERR) {
      flip_bit(crc_start); // инвертируем 1-й бит CRC для примера
  }

  // 6. CRC Delimiter (recessive)
  append_bit(raw_bits, &raw_len, 1);

#ifndef CAN_ERROR_WITH_CORRECT_CRC
  // Ошибки
  if (errors.bit.FF_SRS) flip_bit(srr_idx);
  if (errors.bit.FF_IDE) flip_bit(ide_idx);
  if (errors.bit.FF_RTR) flip_bit(rtr_idx);
  if (errors.bit.FF_R1) flip_bit(r1_idx);
  if (errors.bit.FF_R0) flip_bit(r0_idx);
  if (errors.bit.DATA) flip_bit(data_idx);
  // MSGID_ERR — переворачиваем 1-й бит ID (MSB ID[10]) для примера
  if (errors.bit.MSGID) flip_bit(id_start); 
#endif
  
  // 10. Apply bit stuffing, пропускаем вставку stuff bits если ошибка ERR_STUFF_BITS
  apply_bit_stuffing_with_error(raw_bits, raw_len, can_bits, &can_bits_len, errors.bit.STUFF_BITS);
  
  // 7. ACK Slot (dominant)
  append_bit(can_bits, &can_bits_len, 1);

  // 8. ACK Delimiter (recessive)
  append_bit(can_bits, &can_bits_len, 1);

  // 9. End of Frame (7 recessive bits)
  for (int i = 0; i < 7; i++) {
      append_bit(can_bits, &can_bits_len, 1);
  }
  
}
