#include <c_BinaryOutStream.h>

#include "c_Float.h"


C_STATIC_FORCE_INLINE
c_err_t c_BinaryOutStream_ClearBuffer(c_BinaryOutStream_t* self) {
    if (!self || self->n<0 || !self->base_io) return C_ERR_PARAM;
    if (self->n==0) return C_ERR_OK;
    unsigned char final_byte = (unsigned char)(self->buffer << (8 - self->n));
    c_size_t written = 0;
    c_err_t err= c_OutStream_Write(self->base_io, &final_byte, 1, &written);
    if (err==C_ERR_OK) {
        self->buffer = 0;
        self->n = 0;
    }
    return err;
}

/* ------------------------------------------------------------------------------------------------------------------ */
/*  */


c_err_t c_BinaryOutStream_Init(c_BinaryOutStream_t* self, c_OutStream_t* underlying_io_port) {
    if (!self || !underlying_io_port) return C_ERR_PARAM;
    self->base_io = underlying_io_port;
    self->buffer = 0;
    self->n = 0;
    return C_SUCCESS;
}

c_err_t c_BinaryOutStream_Flush(c_BinaryOutStream_t* self) {
    if (!self || !self->base_io) return C_ERR_PARAM;
    if (self->n == 0) return C_SUCCESS;
    unsigned char final_byte = (unsigned char)(self->buffer << (8 - self->n));
    c_size_t written = 0;
    c_err_t err = c_OutStream_Write(self->base_io, &final_byte, 1, &written);
    if (err!=C_ERR_OK) {
        return err;
    }
    self->buffer = 0;
    self->n = 0;
    return c_OutStream_Flush(self->base_io);
}


c_err_t c_BinaryOutStream_WriteBit(c_BinaryOutStream_t* self, int x) {
    if (!self || (x!=0 && x!=1)) return C_ERR_PARAM;

    self->buffer <<=1;
    if (x) self->buffer|=1;
    self->n++;
    if (self->n==8) {
        c_BinaryOutStream_ClearBuffer(self);
    }

    return C_ERR_OK;
}

c_err_t c_BinaryOutStream_WriteByte(c_BinaryOutStream_t* self, int x) {
    if (!self || (x<0 || x>255)) return C_ERR_PARAM;
    if (self->n ==0) {
        c_size_t written = 0;
        c_err_t err= c_OutStream_Write(self->base_io, &x, 1, &written);
        return err;
    }

    c_err_t err = C_ERR_OK;
    for (int i=0; i<8; i++) {
        int bit = ((x >> (8-i-1)) & 1) == 1;
        if ((err=c_BinaryOutStream_WriteBit(self, bit))!=C_ERR_OK) {
            return err;
        }
    }
    return C_ERR_OK;
}

c_err_t c_BinaryOutStream_WriteBits(c_BinaryOutStream_t* self, uint32_t x, int bit_count) {
    if (!self || !self->base_io || bit_count < 1 || bit_count > 32) return C_ERR_PARAM;
    if (x >= (1<<bit_count)) {
        return C_ERR_PARAM;
    }
    c_err_t err = C_ERR_OK;
    for (int i=0; i<bit_count; i++) {
        int bit = (int)((x >> (bit_count - i - 1)) & 1);
        if ((err = c_BinaryOutStream_WriteBit(self, bit))!=C_ERR_OK) {
            return err;
        }
    }
    return err;
}

c_err_t c_BinaryOutStream_WriteChar(c_BinaryOutStream_t* self, char x) {
    if (!self || !self->base_io ) return C_ERR_PARAM;
    return c_BinaryOutStream_WriteByte(self, x);
}

c_err_t c_BinaryOutStream_WriteUInt16(c_BinaryOutStream_t* self, uint16_t x) {
    if (!self || !self->base_io) return C_ERR_PARAM;
    c_err_t err = C_ERR_OK;
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 8) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 0) & 0xff))!=C_ERR_OK) {
        return err;
    }
    return err;
}

c_err_t c_BinaryOutStream_WriteUInt32(c_BinaryOutStream_t* self, uint32_t x) {
    if (!self || !self->base_io) return C_ERR_PARAM;
    c_err_t err = C_ERR_OK;
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 24) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 16) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 8) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 0) & 0xff))!=C_ERR_OK) {
        return err;
    }
    return err;
}

c_err_t c_BinaryOutStream_WriteUInt64(c_BinaryOutStream_t* self, uint64_t x) {
    if (!self || !self->base_io) return C_ERR_PARAM;
    c_err_t err = C_ERR_OK;
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 56) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 48) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 40) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 32) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 24) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 16) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 8) & 0xff))!=C_ERR_OK) {
        return err;
    }
    if ((err = c_BinaryOutStream_WriteByte(self, (x >> 0) & 0xff))!=C_ERR_OK) {
        return err;
    }
    return err;
}

c_err_t c_BinaryOutStream_WriteFloat(c_BinaryOutStream_t* self, float x) {
    uint32_t u32_val = c_Float_ToBits(x);
    return c_BinaryOutStream_WriteUInt32(self, u32_val);
}

c_err_t c_BinaryOutStream_WriteDouble(c_BinaryOutStream_t* self, double x) {
    uint64_t u64_val = c_Double_ToBits(x);
    return c_BinaryOutStream_WriteUInt64(self, u64_val);
}

c_err_t c_BinaryOutStream_WriteString(c_BinaryOutStream_t* self, const char* s) {
    if (!self || !self->base_io || !s) return C_ERR_PARAM;
    c_size_t s_len = strlen(s);
    c_err_t err = C_ERR_OK;
    for (int i=0; i<s_len; i++) {
        if ((err=c_BinaryOutStream_WriteChar(self, s[i]))!=C_ERR_OK) {
            return err;
        }
    }
    return err;
}