#include "hdlc_frame.h"

#define START_FLAG 0x7E
#define END_FLAG 0x7E
#define ESCAPE_FLAG 0x7D
#define ESCAPE_XOR 0x20

void initialize_frame(HDLCFrame* frame, uint8_t address, uint8_t control, uint8_t* data, size_t data_length) {
    frame->address = address;
    frame->control = control;
    frame->data = data;
    frame->data_length = data_length;
}

uint16_t calculate_fcs(HDLCFrame* frame) {
    uint16_t fcs = 0xFFFF;
    uint8_t data_bytes[MAX_FRAME_LENGTH];
    data_bytes[0] = frame->address;
    data_bytes[1] = frame->control;

    for (size_t i = 0; i < frame->data_length; i++) {
        data_bytes[i + 2] = frame->data[i];
    }

    for (size_t i = 0; i < frame->data_length + 2; i++) {
        uint8_t byte = data_bytes[i];
        fcs = (fcs >> 8) ^ (fcs << 8) ^ byte;
        fcs &= 0xFFFF;
    }

    return fcs;
}

void create_frame(HDLCFrame* frame, uint8_t* frame_data, size_t* frame_length) {
    size_t index = 0;
    frame_data[index++] = START_FLAG;
    frame_data[index++] = frame->address;
    frame_data[index++] = frame->control;

    for (size_t i = 0; i < frame->data_length; i++) {
        uint8_t byte = frame->data[i];
        if (byte == START_FLAG || byte == END_FLAG || byte == ESCAPE_FLAG) {
            frame_data[index++] = ESCAPE_FLAG;
            frame_data[index++] = byte ^ ESCAPE_XOR;
        } else {
            frame_data[index++] = byte;
        }
    }

    uint16_t fcs = calculate_fcs(frame);
    frame_data[index++] = fcs & 0xFF;
    frame_data[index++] = (fcs >> 8) & 0xFF;
    frame_data[index] = END_FLAG;

    *frame_length = index + 1;
}

HDLCFrame* create_u_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length) {
    HDLCFrame* frame = (HDLCFrame*)malloc(sizeof(HDLCFrame));
    initialize_frame(frame, address, control, data, data_length);
    return frame;
}

HDLCFrame* create_s_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length, uint8_t receive_sequence_number, uint8_t send_sequence_number) {
    HDLCFrame* frame = (HDLCFrame*)malloc(sizeof(HDLCFrame));
    initialize_frame(frame, address, control, data, data_length);
    frame->control |= (receive_sequence_number << 2) & 0xFC;
    frame->control |= (send_sequence_number << 1) & 0xFE;
    return frame;
}

HDLCFrame* create_i_frame(uint8_t address, uint8_t control, uint8_t* data, size_t data_length, uint8_t sequence_number) {
    HDLCFrame* frame = (HDLCFrame*)malloc(sizeof(HDLCFrame));
    initialize_frame(frame, address, control, data, data_length);
    frame->control |= (sequence_number << 1) & 0xFE;
    return frame;
}