#include "raw_reader.h"
#include "data_types.h"

#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

int64_t read_raw_m03(FILE *fp, int64_t n_frames, char *frame_out,
                     Header *header_out) {

    const size_t frame_size = 400 * 400 * 2;
    char *tmp = malloc(frame_size);
    Header h;

    int64_t frames_read = 0;
    while (frames_read < n_frames) {
        // Read header to temp buffer, return on fail
        if (fread(&h, sizeof(Header), 1, fp) != 1) {
            break;
        }
        if (header_out) {
            memcpy(header_out, &h, sizeof(Header));
            header_out++;
        }

        // Read frame to temporary buffer
        if (fread(tmp, frame_size, 1, fp) != 1) {
            break;
        }

        // Decode frame and write to numpy output array
        decode_moench03((uint16_t *)tmp, (uint16_t *)frame_out);

        frame_out += frame_size;
        frames_read++;
    }

    free(tmp);
    return frames_read;
}

int64_t read_raw_m04(FILE *fp, int64_t n_frames, char *frame_out,
                     char *digital_out, Header *header_out) {

    const size_t frame_size = 400 * 400 * 2;
    const size_t digital_frame_size = 5000 * 8;
    void *tmp = malloc(frame_size);
    void *digital_tmp = malloc(digital_frame_size);
    Header h;

    int64_t frames_read = 0;
#ifdef CR_VERBOSE
    printf("MOENCH04: digital_out: %p\n", digital_out);
#endif
    while (frames_read < n_frames) {
        // Read header to temp buffer, return on fail
        if (fread(&h, sizeof(Header), 1, fp) != 1) {
            break;
        }
        if (header_out) {
            memcpy(header_out, &h, sizeof(Header));
            header_out++;
        }

        // Read frame to temporary buffer
        if (fread(tmp, frame_size, 1, fp) != 1) {
            break;
        }

        if (digital_out) {
            if (fread(digital_tmp, digital_frame_size, 1, fp) != 1) {
                break;
            }
        }

        // Decode frame and write to numpy output array
        // decode_moench03((uint16_t *)tmp, (uint16_t *)frame_out);
        decode_moench04(tmp, digital_tmp, (uint16_t*)frame_out, (uint8_t*)digital_out);

        frame_out += frame_size;
        if (digital_out)
            digital_out += 400*400; //one byte per pixel

        frames_read++;
    }

    free(tmp);
    free(digital_tmp);
    return frames_read;
}

void decode_moench03(const uint16_t *buf, uint16_t *out_buf) {
    int adc_numbers[32] = {12, 13, 14, 15, 12, 13, 14, 15, 8, 9, 10,
                           11, 8,  9,  10, 11, 4,  5,  6,  7, 4, 5,
                           6,  7,  0,  1,  2,  3,  0,  1,  2, 3};
    for (int n_pixel = 0; n_pixel < 5000; n_pixel++) {

        for (int i_sc = 0; i_sc < 32; i_sc++) {
            // analog part
            int adc_nr = adc_numbers[i_sc];
            int col = ((adc_nr * 25) + (n_pixel % 25));
            int row = 0;
            if (i_sc / 4 % 2 == 0) {
                row = 199 - (n_pixel / 25);
            } else {
                row = 200 + (n_pixel / 25);
            }
            int i_analog = n_pixel * 32 + i_sc;
            out_buf[row * 400 + col] = buf[i_analog] & 0x3FFF;
        }
    }
}

void decode_moench04(const uint16_t *analog_data, const uint64_t *digital_data,
                     uint16_t *analog_frame, uint8_t *digital_frame) {

    int adc_numbers[32] = {9,  8,  11, 10, 13, 12, 15, 14, 1,  0,  3,
                           2,  5,  4,  7,  6,  23, 22, 21, 20, 19, 18,
                           17, 16, 31, 30, 29, 28, 27, 26, 25, 24};

    int dbits_bit[32] = {-1, -1, -1, -1, -1, -1, 1,  3,  5,  7,  -1,
                         -1, -1, -1, -1, -1, 62, 60, 58, 56, 54, 52,
                         50, 48, 63, 61, 59, 57, 55, 53, 51, 49};

    for (int n_pixel = 0; n_pixel < 5000; n_pixel++) {

        uint64_t current_dbits = digital_data[n_pixel];
        for (int i_sc = 0; i_sc < 32; i_sc++) {
            // analog part
            int adc_nr = adc_numbers[i_sc];
            int col = ((adc_nr % 16) * 25) + (n_pixel % 25);
            int row = 0;
            if (i_sc < 16) {
                row = 199 - (n_pixel / 25);
            } else {
                row = 200 + (n_pixel / 25);
            }
            int i_analog = n_pixel * 32 + i_sc;
            analog_frame[row * 400 + col] = analog_data[i_analog] & 0x3FFF;

            // if we pass digital data to be decoded
            if (digital_frame) {
                int bit_loc = dbits_bit[adc_nr];

                if (bit_loc < 0) {
                    digital_frame[row * 400 + col] = 2;
                } else {
                    uint8_t dbit = (current_dbits >> bit_loc) & 1U;

                    if ((6 <= adc_nr) & (adc_nr <= 9)) {
                        digital_frame[row * 400 + col] = dbit;
                    } else {
                        digital_frame[row * 400 + col] = 1 - dbit;
                    }
                }
            } else {
                // digital_frame[row * 400 + col] = 2;
            }
        }
    }
}