/*
 *  Title: FEC
 */

#include <stdint.h>
#include <stdlib.h> // serve ad abs()

#include "arphy.h"
#include "arphy_fec.h"
#include "arphy_interleaver.h"

#include "../../modem/modem_types.h"

// buffer per FEC // perche 16bit ?
int16_t tx_FHT_Buffer[MAX_FEC_OUTPUT_SIZE], rx_FHT_Buffer[MAX_FEC_OUTPUT_SIZE];

block_code fec;

/// Calculate FEC parameters

void arphy_set_up_FEC(block_code* fec_params) {
    if (fec_params->type == ARPHY_FEC_TYPE_NONE) {
        fec_params->in_bit_size = 8;
        fec_params->out_bit_size = fec_params->in_bit_size;
    } else if (fec_params->type == ARPHY_FEC_TYPE_HADAMARD) {
        fec_params->in_bit_size = 4; // dimensione singolo dato in ingresso
        fec_params->out_bit_size = 1 << (fec_params->in_bit_size - 1); // dimensione  del singolo codice hadamard in uscita 2^(n-1)
    }

    fec_params->out_symbols_size = (fec_params->out_bit_size / mod.bits_per_symbol);
}

void arphy_FEC_encode(uint8_t Chunk, uint8_t ChunkSize, int16_t *DataBuffer) {
    if (fec.type == ARPHY_FEC_TYPE_NONE) {
        uint8_t i;
        for (i = 0; i < ChunkSize; i++) {
            DataBuffer[i] |= (Chunk >> i) & 1;
        }
    } else if (fec.type == ARPHY_FEC_TYPE_HADAMARD) {
        // il valore massimo del dato e' 2 * size(matrice)      
        // inserimento chunk nella matrice, alti negativi, bassi positivi   
        if (Chunk < (fec.out_bit_size * 2)) {
            if (Chunk < fec.out_bit_size) DataBuffer[Chunk] = 1;
            else DataBuffer[Chunk - fec.out_bit_size] = -1;
        }

        // trasformata inversa di hadamard
        InverseFastHadamardTransform(DataBuffer, fec.out_bit_size);

        // riduci a binario
        uint8_t MatrixIndex;
        for (MatrixIndex = 0; MatrixIndex < fec.out_bit_size; MatrixIndex++) {
            if (DataBuffer[MatrixIndex] < 0) DataBuffer[MatrixIndex] = 0;
        }

    }
}

uint8_t arphy_FEC_decode(int16_t *DataMatrix) {
    uint8_t DataChunk = 0;
    uint16_t TimeBit;
    if (fec.type == ARPHY_FEC_TYPE_NONE) {
        for (TimeBit = 0; TimeBit < fec.out_bit_size; TimeBit++) {
            DataChunk |= ((DataMatrix[TimeBit] & 1) << TimeBit);
        }
    } else if (fec.type == ARPHY_FEC_TYPE_HADAMARD) {
        // converti 0 in -1
        for (TimeBit = 0; TimeBit < fec.out_bit_size; TimeBit++) {
            if (DataMatrix[TimeBit] == 0) DataMatrix[TimeBit] = -1;
        }

        // decodifica
        FastHadamardTransform(DataMatrix, fec.out_bit_size);
        // estrae dalla matrice
        int16_t Peak = 0;
        uint8_t PeakPos = 0;
        fec.sqrsum = 0;

        for (TimeBit = 0; TimeBit < fec.out_bit_size; TimeBit++) {
            int16_t Signal = DataMatrix[TimeBit];
            fec.sqrsum += Signal * Signal;
            if (abs(Signal) > abs(Peak)) {
                Peak = Signal;
                PeakPos = TimeBit;
            }
        }
        // info
        DataChunk = PeakPos;
        if (Peak < 0) DataChunk += fec.out_bit_size;
        fec.sqrsum -= Peak * Peak;

    }
    return DataChunk;
}

// Forward Fast Hadamard Transform

void FastHadamardTransform(int16_t *Data, uint8_t fec_size) {
    int16_t Bit1, Bit2, NewBit1, NewBit2 = 0;
    uint16_t Step, Ptr, Ptr2;
    for (Step = 1; Step < fec_size; Step <<= 1) {
        for (Ptr = 0; Ptr < fec_size; Ptr += (Step << 1)) {
            for (Ptr2 = Ptr; Ptr2 - Ptr < Step; ++Ptr2) {
                Bit1 = Data[Ptr2];
                Bit2 = Data[Ptr2 + Step];
                NewBit1 = Bit2;
                NewBit1 += Bit1;
                NewBit2 = Bit2;
                NewBit2 -= Bit1;
                Data[Ptr2] = NewBit1;
                Data[Ptr2 + Step] = NewBit2;
            }
        }
    }
}

// Inverse Fast Hadamard Transform

void InverseFastHadamardTransform(int16_t *Data, uint8_t fec_size) {
    int16_t Bit1, Bit2, NewBit1, NewBit2 = 0;
    uint16_t Step, Ptr, Ptr2;
    for (Step = (fec_size >> 1); Step; Step >>= 1) {
        for (Ptr = 0; Ptr < fec_size; Ptr += (Step << 1)) {
            for (Ptr2 = Ptr; Ptr2 - Ptr < Step; ++Ptr2) {
                Bit1 = Data[Ptr2];
                Bit2 = Data[Ptr2 + Step];
                NewBit1 = Bit1;
                NewBit1 -= Bit2;
                NewBit2 = Bit1;
                NewBit2 += Bit2;
                Data[Ptr2] = NewBit1;
                Data[Ptr2 + Step] = NewBit2;
            }
        }
    }
}