#include "raven_spn.h"
#include "spn_regs.h"
#include "delay.h"
#include "bsp.h"
#include "plic/plic_driver.h"

#include <cstdio>
#include <cstdint>
#include <array>

using spn =spn_regs<0x90000000>;

#define IOF_ENABLE_TERMINAL (0x30000)


typedef void (*function_ptr_t) (void);
//! Instance data for the PLIC.
plic_instance_t g_plic;
std::array<function_ptr_t,PLIC_NUM_INTERRUPTS>  g_ext_interrupt_handlers;


/*! \brief external interrupt handler
 *
 * routes the peripheral interrupts to the the respective handler
 *
 */
extern "C" void handle_m_ext_interrupt() {
    plic_source int_num  = PLIC_claim_interrupt(&g_plic);
    if ((int_num >=1 ) && (int_num < PLIC_NUM_INTERRUPTS))
      g_ext_interrupt_handlers[int_num]();
    else
      exit(1 + (uintptr_t) int_num);
    PLIC_complete_interrupt(&g_plic, int_num);
}
/*! \brief dummy interrupt handler
 *
 */
void no_interrupt_handler (void) {};
/*! \brief configure the per-interrupt handler
 *
 */
void configure_irq(size_t irq_num, function_ptr_t handler, unsigned char prio=1) {
    g_ext_interrupt_handlers[irq_num] = handler;
    // Priority must be set > 0 to trigger the interrupt.
    PLIC_set_priority(&g_plic, irq_num, prio);
    // Have to enable the interrupt both at the GPIO level, and at the PLIC level.
    PLIC_enable_interrupt(&g_plic, irq_num);
}

static void msi_interrupt_handler(){
    int * local_mem_base = (int *) 0x80000100;
    printf("INterrupt handler call\n");
}

/*!\brief initializes platform
 *
 */
void platform_init(){
	// UART init section TODO: clarify how to get the functions from init.c?
	GPIO_REG(GPIO_IOF_SEL) &= ~IOF0_UART0_MASK;
	GPIO_REG(GPIO_IOF_EN) |= IOF0_UART0_MASK;
	UART0_REG(UART_REG_TXCTRL) |= UART_TXEN;

    F_CPU=PRCI_measure_mcycle_freq(20, RTC_FREQ);
    printf("core freq at %d Hz\n", F_CPU);
    // initialie interupt & trap handling
    write_csr(mtvec, &trap_entry);

    PLIC_init(&g_plic, PLIC_CTRL_ADDR, PLIC_NUM_INTERRUPTS, PLIC_NUM_PRIORITIES, 0);
    // Disable the machine & timer interrupts until setup is done.
    clear_csr(mie, MIP_MEIP);
    clear_csr(mie, MIP_MTIP);
    for (auto& h:g_ext_interrupt_handlers) h=no_interrupt_handler;
    configure_irq(1, msi_interrupt_handler);
    // Enable interrupts in general.
    set_csr(mstatus, MSTATUS_MIE);
    // Enable the Machine-External bit in MIE
    set_csr(mie, MIP_MEIP);
}

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


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

    // write input samples into the memory
    int * mem_base = (int *) 0x80001000;
    std::array<uint8_t, 64> input_data = {
    		0, 0, 0, 1, 0,
			0, 1, 5, 2, 0,
    		0, 0, 8, 0, 0,
    		0, 0, 5, 4, 0,
    		0, 0, 0, 1, 0,
    		0, 0, 0, 1, 0,
    		0, 0, 0, 1, 0,
    		0, 0, 0, 1, 0
    };

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

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

    printf("Start SPN HW accelerator\n");
    spn::mode_reg() = 0;
    spn::input_length_reg() = 8;
    spn::input_addr_reg() = 0x80001000;
    spn::output_addr_reg() = 0x80001000;
    spn::num_of_in_beats_reg() = 1;
    spn::num_of_out_beats_reg() = 1;
    spn::start_reg() = 1;

    delayUS(50);
    printf("Cycle count:0x%x\n", spn::start_reg());
    spn::interrupt_reg() = 1;
    delayUS(10);

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

    if (double_equals(res_base[0], -3.5530456851)) {
        printf("XSPN reference value comparison PASSED\n");
    } else {
        printf("XSPN reference value comparison FAILED\n");
    	return 1;
    }


    printf("End of execution");
	return 0;
}