#include <stdarg.h>
#include <stdio.h>

#include "FreeRTOS.h"
#include "task.h"

#include "project.h"

#define NUM_BITS_ADDR 		(UTIL_ADDR + 0x100)
#define DEV_SEL_ADDR 		(UTIL_ADDR + 0x104)
#define SPI_DATA_ADDR 		(UTIL_ADDR + 0x108)
#define SPI_CLK_DIV_ADDR 	(UTIL_ADDR + 0x10C)
#define SPI_ACTIVE_ADDR 	(UTIL_ADDR + 0x110)
#define SPI_CLK_DIV 		16

uint32_t reverse_bits(int num_bits, uint32_t data) {
    uint32_t result = 0;
    for (int i = 0; i < num_bits; i++) {
        result |= ((data >> i) & 1) << (num_bits - 1 - i);
    }
    return result;
}

int32_t rf_spi_write(int dev_sel, int num_bits, int data) {

	uint32_t bit_reversed = reverse_bits(num_bits, data);

	Xil_Out32(DEV_SEL_ADDR, dev_sel);
	Xil_Out32(SPI_CLK_DIV_ADDR, SPI_CLK_DIV);
	Xil_Out32(SPI_DATA_ADDR, bit_reversed);
	// Writing this reg starts the transaction
	Xil_Out32(NUM_BITS_ADDR, num_bits - 1);

	// Poll to wait for transcation to complete
	while (Xil_In32(SPI_ACTIVE_ADDR)) {
		// Wait
	}

	return 0;
}