// Copyright (2019-2022) Paul Scherrer Institute
// SPDX-License-Identifier: GPL-3.0-or-later

#include "JFConversionFixedPoint.h"
#include "../common/RawToConvertedGeometry.h"

#define FIXED_PRECISION 14

JFConversionFixedPoint::JFConversionFixedPoint() {
    gain_g0 = (int32_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(int32_t));
    gain_g1 = (int32_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(int32_t));
    gain_g2 = (int32_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(int32_t));
    pedestal_g0 = (uint16_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(uint16_t));
    pedestal_g1 = (uint16_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(uint16_t));
    pedestal_g2 = (uint16_t *) std::aligned_alloc(64, RAW_MODULE_SIZE * sizeof(uint16_t));
}

JFConversionFixedPoint::~JFConversionFixedPoint() {
    std::free(gain_g0);
    std::free(gain_g1);
    std::free(gain_g2);
    std::free(pedestal_g0);
    std::free(pedestal_g1);
    std::free(pedestal_g2);
}

inline int32_t one_over_gain_energy(double gain_factor, double energy) {
    double tmp = gain_factor * energy;
    if (!std::isfinite(tmp) || (tmp == 0.0))
        return INT16_MIN;
    else
        return std::lround((1 << FIXED_PRECISION) / (gain_factor * energy));
}

void JFConversionFixedPoint::Setup(const JFModuleGainCalibration &gain_calibration,
                                      const JFModulePedestal &in_pedestal_g0,
                                      const JFModulePedestal &in_pedestal_g1,
                                      const JFModulePedestal &in_pedestal_g2,
                                      double energy) {
    auto &gain_arr = gain_calibration.GetGainCalibration();
    memcpy(pedestal_g0, in_pedestal_g0.GetPedestal(), RAW_MODULE_SIZE * sizeof(uint16_t));
    memcpy(pedestal_g1, in_pedestal_g1.GetPedestal(), RAW_MODULE_SIZE * sizeof(uint16_t));
    memcpy(pedestal_g2, in_pedestal_g2.GetPedestal(), RAW_MODULE_SIZE * sizeof(uint16_t));

    for (int i = 0; i < RAW_MODULE_SIZE; i++) {
        gain_g0[i] = one_over_gain_energy(gain_arr[i], energy);
        gain_g1[i] = one_over_gain_energy(gain_arr[i + RAW_MODULE_SIZE], energy);
        gain_g2[i] = one_over_gain_energy(gain_arr[i + 2 * RAW_MODULE_SIZE], energy);
    }
}

inline int32_t jf_round(int32_t in) {
    const int32_t half = (1L << (FIXED_PRECISION-1));
    if (in > 0)
        return in + half;
    else
        return in - half;
}

void JFConversionFixedPoint::ConvertLine(int16_t *dest, const uint16_t *source, int line) {

    auto gain_g0_aligned = std::assume_aligned<64>(gain_g0);
    auto gain_g1_aligned = std::assume_aligned<64>(gain_g1);
    auto gain_g2_aligned = std::assume_aligned<64>(gain_g2);
    auto pedestal_g0_aligned = std::assume_aligned<64>(pedestal_g0);
    auto pedestal_g1_aligned = std::assume_aligned<64>(pedestal_g1);
    auto pedestal_g2_aligned = std::assume_aligned<64>(pedestal_g2);

    for (int i = 0; i < RAW_MODULE_COLS; i++) {
        uint16_t gainbits = source[i] & 0xc000;
        int32_t adc = source[i] & 0x3fff;
        int32_t val = 0;

        bool overflow = false;
        bool wrong = false;

        if ((source[i] == 0x4000) || (source[i] == 0xffff)) wrong = true;
        else if (source[i] == 0xc000) overflow = true;

        switch (gainbits) {
            case 0:
                [[likely]]
                    val = jf_round((adc - pedestal_g0_aligned[i + line * RAW_MODULE_COLS]) * gain_g0_aligned[i + line * RAW_MODULE_COLS]);
                break;
            case 0x4000:
                val = jf_round((adc - pedestal_g1_aligned[i + line * RAW_MODULE_COLS]) * gain_g1_aligned[i + line * RAW_MODULE_COLS]);
                break;
            case 0xc000:
                val = jf_round((adc - pedestal_g2_aligned[i + line * RAW_MODULE_COLS]) * gain_g2_aligned[i + line * RAW_MODULE_COLS]);
                break;
            default:
                wrong = true;
                break;
        }

        if (wrong || (val <= INT16_MIN * (1L << FIXED_PRECISION)))
            [[unlikely]]
                    dest[i] = INT16_MIN;
        else if (overflow || (val >= INT16_MAX * (1L << FIXED_PRECISION)))
            [[unlikely]]
                    dest[i] = INT16_MAX;
        else
            dest[i] = static_cast<int16_t>(val / (1L << FIXED_PRECISION));
    }
}

void JFConversionFixedPoint::ConvertPacket(int16_t *dest, const uint16_t *source, uint16_t packet_number) {
    for (int i = 0; i < 4; i++)
        ConvertLine(dest + i * RAW_MODULE_COLS, source + i * RAW_MODULE_COLS, 4 * packet_number + i);
}

void JFConversionFixedPoint::ConvertAdjustGeom(int16_t *dest, const uint16_t *source, int64_t slow_dir_step,
                                               int64_t fast_dir_step, int64_t offset) {
    for (int line = 0; line < RAW_MODULE_LINES; line++) {
        int16_t tmp[RAW_MODULE_COLS];
        ConvertLine(tmp, source + line * RAW_MODULE_COLS, line);

        if ((line != 255) && (line != 256)) {
            LineCopyAndAdjustMultipixel<int16_t, int16_t>(dest + slow_dir_step * (line + ((line > 255) ? 2 : 0)),
                                                          tmp, INT16_MIN, INT16_MAX, fast_dir_step, offset);
        } else {
            LineCopyAndAdjustMultipixelMidRow<int16_t, int16_t>(dest + slow_dir_step * (line + 1),
                                                      tmp, INT16_MIN, INT16_MAX, fast_dir_step, offset);
            LineCopyAndAdjustMultipixelMidRow<int16_t, int16_t>(dest + slow_dir_step * (line + ((line > 255) ? 2 : 0)),
                                                                tmp, INT16_MIN, INT16_MAX, fast_dir_step, offset);
        }
    }
}

JFConversionFixedPoint::JFConversionFixedPoint(JFConversionFixedPoint &&other) noexcept {
    gain_g0 = other.gain_g0;
    other.gain_g0 = nullptr;

    gain_g1 = other.gain_g1;
    other.gain_g1 = nullptr;

    gain_g2 = other.gain_g2;
    other.gain_g2 = nullptr;

    pedestal_g0 = other.pedestal_g0;
    other.pedestal_g0 = nullptr;

    pedestal_g1 = other.pedestal_g1;
    other.pedestal_g1 = nullptr;

    pedestal_g2 = other.pedestal_g2;
    other.pedestal_g2 = nullptr;
}