#include "raven_spn.h"
#include "spn_regs.h"
#include "init.h"

#include <cmath> // log

using spn =spn_regs<0x90000000>;

// huge arrays of XSPN input and referance data
extern std::array<uint8_t, 50000> input_data;
extern std::array<double, 10000> ref_data;
constexpr auto ln2 = std::log(2);

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

void run_xspn() {
	spn::mode_reg() = 0;
	spn::input_length_reg() = 100; 		// each sample consists of 5 uint8 values
	spn::input_addr_reg() = 0x800C0000;
	spn::output_addr_reg() = 0x800A0000;
	spn::num_of_in_beats_reg() = 8;		// Number of AXI4 burst beats needed to load all input data
	spn::num_of_out_beats_reg() = 13;	// Number of AXI4 burst beats needed to store all result data
	spn::start_reg() = 1;

	wait_for_interrupt();
	spn::interrupt_reg() = 1;
}

/*! \brief main function
 *
 */
int main() {
    platform_init();
    spn::mode_reg() = 1;
    spn::start_reg() = 1;
	wait_for_interrupt();
    spn::interrupt_reg() = 1;
    printf("READOUT HW:0x%x\n", spn::readout_reg());

	for (int k = 0; k < 10000; k+=100) {
        // write input samples into the memory
        int * mem_base = (int *) 0x800C0000;
        int offset = k * 5;

        for(size_t i = 0, j = 0; j < 125; i += 4, j++) {
        	*(mem_base+j) = input_data.at(offset+i);
        	*(mem_base+j) |= (input_data.at(offset+i+1) << 8)  & 0x0000FF00;
        	*(mem_base+j) |= (input_data.at(offset+i+2) << 16) & 0x00FF0000;
        	*(mem_base+j) |= (input_data.at(offset+i+3) << 24) & 0xFF000000;
        }

        printf("XSPN process samples %d - %d\n", k, k+100);
    	run_xspn();

    	// read calculation results from the memory
        double * res_base = (double*) 0x800A0000;

        for (int i = 0; i < 100; i++) {
            if (!double_equals(res_base[i] * ln2, ref_data.at(k+i))) {
                printf("XSPN ref %d comparison FAILED\n", k+i);
            }
    	}

	}

	return 0;
}