/*
 * can_encode.c
 *
 *  Created on: Sep 16, 2025
 *      Author: herli
 */
#include "can_encode.h"
#include <string.h>
#include <math.h>

/* -------- Bit helpers (Intel) --------
 * Intel (little-endian bit numbering): bit 0 is LSB of byte 0, bit 7 is MSB of byte 0,
 * then bit 8 is LSB of byte 1, etc.
 */
uint64_t _can_get_bits_intel(const uint8_t data[8], uint8_t start_bit, uint8_t bit_len)
{
    uint64_t acc = 0;
    for (uint8_t i = 0; i < bit_len; ++i) {
        uint16_t bit_index = start_bit + i;
        uint8_t  byte_idx  = (uint8_t)(bit_index >> 3);
        uint8_t  bit_in_byte = (uint8_t)(bit_index & 7);
        uint8_t  b = (data[byte_idx] >> bit_in_byte) & 1u;
        acc |= ((uint64_t)b) << i;
    }
    return acc;
}

void _can_set_bits_intel(uint8_t data[8], uint8_t start_bit, uint8_t bit_len, uint64_t raw)
{
    for (uint8_t i = 0; i < bit_len; ++i) {
        uint16_t bit_index = start_bit + i;
        uint8_t  byte_idx  = (uint8_t)(bit_index >> 3);
        uint8_t  bit_in_byte = (uint8_t)(bit_index & 7);
        uint8_t  bit = (uint8_t)((raw >> i) & 1u);
        data[byte_idx] = (uint8_t)((data[byte_idx] & ~(1u << bit_in_byte)) | (bit << bit_in_byte));
    }
}

/* -------- Bit helpers (Motorola) --------
 * Motorola (big-endian) uses a different bit ordering: start_bit points to a bit index where
 * bytes count from MSB to LSB. Implementation below follows standard DBC Motorola mapping.
 * This implementation is conservative and handles the general case.
 */
static inline uint8_t _motorola_byte(uint8_t bit_index) { return (uint8_t)(7 - (bit_index & 7)); }

uint64_t _can_get_bits_motorola(const uint8_t data[8], uint8_t start_bit, uint8_t bit_len)
{
    uint64_t acc = 0;
    uint8_t cur_bit = start_bit;
    for (uint8_t i = 0; i < bit_len; ++i) {
        uint8_t byte_idx = (uint8_t)(cur_bit >> 3);
        uint8_t bit_in_byte = _motorola_byte(cur_bit);
        uint8_t b = (data[byte_idx] >> bit_in_byte) & 1u;
        acc = (acc << 1) | b;
        // advance to next bit (Motorola decrements bit index)
        if (bit_in_byte == 0) {
            // next byte
            if (byte_idx == 0) break; // avoid underflow; malformed definition
            cur_bit = (uint8_t)(((byte_idx - 1) << 3) + 7);
        } else {
            cur_bit = (uint8_t)((byte_idx << 3) + (bit_in_byte - 1));
        }
    }
    return acc;
}

void _can_set_bits_motorola(uint8_t data[8], uint8_t start_bit, uint8_t bit_len, uint64_t raw)
{
    uint8_t cur_bit = start_bit;
    for (uint8_t i = 0; i < bit_len; ++i) {
        uint8_t bit_val = (uint8_t)((raw >> (bit_len - 1 - i)) & 1u);
        uint8_t byte_idx = (uint8_t)(cur_bit >> 3);
        uint8_t bit_in_byte = _motorola_byte(cur_bit);
        data[byte_idx] = (uint8_t)((data[byte_idx] & ~(1u << bit_in_byte)) | (bit_val << bit_in_byte));
        // advance (Motorola)
        if (bit_in_byte == 0) {
            if (byte_idx == 0) break;
            cur_bit = (uint8_t)(((byte_idx - 1) << 3) + 7);
        } else {
            cur_bit = (uint8_t)((byte_idx << 3) + (bit_in_byte - 1));
        }
    }
}

/* -------- Effective factor/offset for a symbol -------- */
static inline void eff_scale(const CanSymbolDef *s, float *f, float *o)
{
    if ((s->factor != 0.0f) || (s->offset != 0.0f)) {
        *f = s->factor; *o = s->offset;
    } else if (s->scale) {
        *f = s->scale->factor; *o = s->scale->offset;
    } else {
        *f = 1.0f; *o = 0.0f;
    }
}

/* -------- Public encode/decode -------- */
void can_encode_message(const CanMessageDef *msg, uint8_t out_data[8])
{
    if (!msg || !out_data) return;

    if (msg->tx_template) {
        memcpy(out_data, msg->tx_template, msg->dlc);
        if (msg->dlc < 8) memset(out_data + msg->dlc, 0, 8 - msg->dlc);
    } else {
        memset(out_data, 0, 8);
    }

    for (uint8_t i = 0; i < msg->symbol_count; ++i) {
        const CanSymbolDef *s = &msg->symbols[i];
        if (!s->ptr || s->bit_len == 0) continue;

        int64_t raw_q;
        if (s->storage == CAN_STORE_FLOAT) {
            float f, o; eff_scale(s, &f, &o);
            if (f == 0.0f) f = 1.0f;                   // guard
            float phys   = *(const float*)s->ptr;
            float scaled = (phys - o) / f;
            raw_q = (int64_t)llroundf(scaled);
        } else {
            switch (s->storage) {
                case CAN_STORE_U32: raw_q = (int64_t)(*(const uint32_t*)s->ptr); break;
                case CAN_STORE_S32: raw_q = (int64_t)(*(const int32_t *)s->ptr); break;
                case CAN_STORE_U16: raw_q = (int64_t)(*(const uint16_t*)s->ptr); break;
                case CAN_STORE_S16: raw_q = (int64_t)(*(const int16_t *)s->ptr); break;
                case CAN_STORE_U8:  raw_q = (int64_t)(*(const uint8_t *)s->ptr); break;
                case CAN_STORE_S8:  raw_q = (int64_t)(*(const int8_t  *)s->ptr); break;
                default:            raw_q = 0; break;
            }
            if(s->inverted){
            	raw_q = !raw_q;
            }
        }

        if (s->raw_type == CAN_SYM_SX) {
            int64_t minv = -(1LL << (s->bit_len - 1));
            int64_t maxv =  (1LL << (s->bit_len - 1)) - 1;
            if (raw_q < minv) raw_q = minv;
            if (raw_q > maxv) raw_q = maxv;
            uint64_t raw_u = (uint64_t)((int64_t)raw_q & ((1ULL << s->bit_len) - 1ULL));
            (s->endian == CAN_ENDIAN_INTEL)
                ? _can_set_bits_intel(out_data, s->start_bit, s->bit_len, raw_u)
                : _can_set_bits_motorola(out_data, s->start_bit, s->bit_len, raw_u);
        } else {
            if (raw_q < 0) raw_q = 0;
            uint64_t raw_u = (uint64_t)raw_q & ((s->bit_len == 64) ? ~0ULL : ((1ULL << s->bit_len) - 1ULL));
            (s->endian == CAN_ENDIAN_INTEL)
                ? _can_set_bits_intel(out_data, s->start_bit, s->bit_len, raw_u)
                : _can_set_bits_motorola(out_data, s->start_bit, s->bit_len, raw_u);
        }
    }
}

void can_decode_message(const CanMessageDef *msg, const uint8_t in_data[8])
{
    if (!msg || !in_data) return;

    for (uint8_t i = 0; i < msg->symbol_count; ++i) {
        const CanSymbolDef *s = &msg->symbols[i];
        if (!s->ptr || s->bit_len == 0) continue;

        uint64_t raw = (s->endian == CAN_ENDIAN_INTEL)
            ? _can_get_bits_intel(in_data, s->start_bit, s->bit_len)
            : _can_get_bits_motorola(in_data, s->start_bit, s->bit_len);

        // 1) Sign-extend (or not) -> produce raw_s
        int64_t raw_s;
        if (s->raw_type == CAN_SYM_SX) {
            uint64_t sign_bit = 1ULL << (s->bit_len - 1);
            raw_s = (raw & sign_bit)
                ? (int64_t)(raw | (~((1ULL << s->bit_len) - 1ULL)))  // extend
                : (int64_t)raw;
        } else {
            raw_s = (int64_t)raw;
        }

        // 2) Store according to 'storage'
        if (s->storage == CAN_STORE_FLOAT) {
            float f, o; eff_scale(s, &f, &o);
            // f==0 guard (shouldn't happen if scales are defined, but safe)
            if (f == 0.0f) f = 1.0f;
            *(float*)s->ptr = (float)raw_s * f + o;
        } else {
            switch (s->storage) {
                case CAN_STORE_U32: *(uint32_t*)s->ptr = (uint32_t)raw_s; break;
                case CAN_STORE_S32: *(int32_t *)s->ptr = (int32_t) raw_s; break;
                case CAN_STORE_U16: *(uint16_t*)s->ptr = (uint16_t)raw_s; break;
                case CAN_STORE_S16: *(int16_t *)s->ptr = (int16_t) raw_s; break;
                case CAN_STORE_U8:  *(uint8_t *)s->ptr = (uint8_t) raw_s; break;
                case CAN_STORE_S8:  *(int8_t  *)s->ptr = (int8_t)  raw_s; break;
                default: break;
            }
        }
    }
}

