#include "raven_spn.h"
#include "spn_regs.h"
#include "dma_regs.h"
#include "init.h"
#include <math.h>

using spn = spn_regs<0x90000000>;
using dma = dma_regs<0xB0000000>;

// huge arrays of XSPN input and referance data
extern std::array<uint8_t, 50000> input_data;
extern std::array<double, 10000> ref_data;

bool double_equals(double a, double b, double epsilon = 0.001)
{
    return std::abs(a - b) < epsilon;
}

void run_xspn(int in_addr, int out_addr, int num_samples, int in_beats, int out_beats) {
	spn::mode_reg() = 0;
	spn::input_length_reg() = num_samples; 		// each sample consists of 5 uint8 values
	spn::input_addr_reg() = in_addr;
	spn::output_addr_reg() = out_addr;
	spn::num_of_in_beats_reg() = in_beats;	// Number of AXI4 burst beats needed to load all input data
	spn::num_of_out_beats_reg() = out_beats;	// Number of AXI4 burst beats needed to store all result data
    printf("Starting XSPN\n");
	spn::start_reg() = 1;
}

void fpga_dma(int direction, int fpga_address, int sc_address, int num_bytes) {
    dma::write_reg() = direction;
    dma::fpga_address_reg() = fpga_address;
    dma::sc_address_reg() = sc_address;
    dma::bytes_reg() = num_bytes;
    dma::start_reg() = 1;
    wait_for_interrupt();
    dma::clear_interrupt_reg() = 1;
}

void check_results(int addr, int k, int step) {
    int k0 = 0;
    bool result = 0;
    double *res_base = (double*) (addr);
    int * error_exit = (int *)0xF0000000;
    printf("Start result comparison %d - %d\n", k, k+step);

    for (int i = 0; i < step; i++) {

        if (!double_equals(res_base[i], ref_data.at(k0 + i))) {
            printf("XSPN ref %d comparison FAILED\n", k0 + i);
            result = 1;
        }
    }
    if (result == 1) *error_exit = 0x1;
    printf("Compared samples %d - %d with the reference\n", k, k+step);
}

/*! \brief main function
 *
 */
int main() {


    platform_init();

    spn::mode_reg() = 1;
    spn::start_reg() = 1;
    wait_for_interrupt();
    spn::interrupt_reg() = 1;
    uint32_t readout = spn::readout_reg();
    printf("READOUT HW:0x%x\n", readout);

    uint32_t axi_bytes = readout;
    axi_bytes = axi_bytes & 0xff;
    axi_bytes = 1 << axi_bytes;

    printf("AXI Bytes: %d\n", axi_bytes);

    uint32_t sample_bytes = readout;
    sample_bytes = sample_bytes >> 16;
    sample_bytes = sample_bytes / 8;

    printf("Sample Bytes: %d\n", sample_bytes);

    uint32_t result_bytes = 8;

    printf("Result Bytes: %d\n", result_bytes);

    uint32_t step = 500;
    uint32_t iterations = 5;

    
    uint32_t in_beats = (step * sample_bytes) / axi_bytes;
    if (in_beats * axi_bytes < step * sample_bytes) in_beats++;
    uint32_t out_beats = (step * result_bytes) / axi_bytes;
    if (out_beats * axi_bytes < step * result_bytes) out_beats++;

	int in_addr  = (int)input_data.data();
	int out_addr = 0x800B0000;
    int fpga_address_in = 0x10000000;
    int fpga_address_out = 0x20000000;

    //run_xspn(in_addr, out_addr);
    for (int k = 0; k < iterations*step; k+=step) {
        printf("XSPN processes samples %d - %d\n", k, k+step);
        fpga_dma(1, fpga_address_in, in_addr, step * sample_bytes);
        run_xspn(fpga_address_in, fpga_address_out, step, in_beats, out_beats);
        wait_for_interrupt();
        printf("XSPN finished\n");
    	spn::interrupt_reg() = 1;
        fpga_dma(0, fpga_address_out, out_addr, step * result_bytes);
		check_results(out_addr, k, step);

		//in_addr += step * sample_bytes; // 5 bytes in each sample
	}

	return 0;
}