#include "accel.h"
#include "time.h"    // sensor strobe settings
#include "gpio.h"    // turn sensor power on/off

#include <drivers/spi/adi_spi.h>    // SPI driver
#include <drivers/xint/adi_xint.h>  // external int for FIFO complete IRQ

//=========================== typedef =========================================

//=========================== variables =======================================

//==== Accelerometer variables
static ADI_SPI_HANDLE ADXL372_SPI;
static uint8_t        ADXL372_SPI_DevMem[ADI_SPI_MEMORY_SIZE];
static IRQn_Type      SYSWAKE3_IRQ = XINT_EVT3_IRQn;

int16_t               accel_fifo[ADXL372_MAX_FIFO_SAMPLES];  /* Buffer to read samples */
accel_cbt             accel_cb = NULL;

//=========================== prototypes ======================================

static bool SPI_FifoRead(void *pBuffer, uint16_t nSize);
static bool ADXL372_REG_READ(uint8_t RegAddr, uint8_t *pBuffer);
static bool ADXL372_REG_WRITE(uint8_t RegAddr, uint8_t Value);

/* Accelerometer callback - to indicate when data is ready */
static void accel_callback(void *pCBParam, uint32_t Port, void *pin)
{

    

    return;
}

/* Opens and sets up the accelerometer driver */
bool accel_init(accel_cbt processData)
{
    uint8_t data[6];

    /* POWER_CTL register contents */
    uint8_t                 IntSource;

    //gpio_acc_On(); // TODO check timing here
 
	  if (processData != NULL)
			accel_cb = processData;

    // open the spi driver
    adi_spi_Open(2, ADXL372_SPI_DevMem, ADI_SPI_MEMORY_SIZE, &ADXL372_SPI);  

    // WHO AM I REGISTER
    if(!ADXL372_REG_READ(ADXL372_PARTID, data))
        return false;

    if (data[0] != 0xF0)
    {
			ERR_MSG("ADXL372 not connected! data: 0x%X\n", data[0]);
        return false;
    }
    // reset
		ADXL372_REG_WRITE(ADXL372_RESET, ADXL372_RESET_CODE);
		
		
    // read status to clear interrupts
    ADXL372_REG_READ(ADXL372_STATUS_1, &IntSource);
		DEBUG_MSG("Status: 0x%X\n", IntSource);
		
		//ADXL372_REG_WRITE(ADXL372_FIFO_CTL, 0x09);     // FIFO enable
		
		//ADXL372_REG_WRITE(ADXL372_FIFO_CTL, 0x08);     // FIFO enable
		ADXL372_REG_WRITE(ADXL372_FIFO_SAMPLES, 248); //
		ADXL372_REG_WRITE(ADXL372_FIFO_CTL, 3);     // FIFO stream enable
    ADXL372_REG_WRITE(ADXL372_TIMING,0x0B); // External sampling
		ADXL372_REG_WRITE(ADXL372_INT1_MAP, 0x04); // FIFO watermark interrupt		
		
		 // Start measurements
    

    /* Set the current contents of Power control register */
    ADXL372_REG_WRITE(ADXL372_POWER_CTL, 0x03);
		
		adi_xint_RegisterCallback(ADI_XINT_EVENT_INT3, accel_callback, &SYSWAKE3_IRQ);
    adi_xint_EnableIRQ(ADI_XINT_EVENT_INT3, ADI_XINT_IRQ_RISING_EDGE); // polarity can be set in INTMAP1
		NVIC_SetPriority(SPI2_EVT_IRQn, 1);    // High priority
		NVIC_SetPriority(XINT_EVT3_IRQn, 2);   // Medium priority

    // Start Sensor strobe service
    time_configureSensorStrobe(ACCEL_ODR);

    return true;
}

bool accel_deinit(void)
{
    // TODO all this function
    // disable adxl363
    // gpio_acc_Off();
    return true;
}

/*==========================================================================
     Device register access functions.
 *=========================================================================*/

/* Function to read the FIFO data using SPI interface. */
static bool SPI_FifoRead(void *pBuffer, uint16_t nSize)
{
    /* SPI transceiver instance */
    ADI_SPI_TRANSCEIVER Transceiver;
    uint8_t nTemp[2];
    nTemp[0] = (ADXL372_FIFO_DATA<<1)|0x01;
    nTemp[1] = 0x00u;  /* Essentially unused */

    /* Initialize the transceiver */
    Transceiver.pReceiver           =   pBuffer;
    Transceiver.ReceiverBytes       =   nSize * sizeof(uint16_t);
    Transceiver.nRxIncrement        =   1u;
    Transceiver.pTransmitter        =   nTemp;
    Transceiver.TransmitterBytes    =   1u;
    Transceiver.nTxIncrement        =   1u;
    Transceiver.bDMA                =   true;
    Transceiver.bRD_CTL             =   true;

    /* Transmit the sequence */
    if(adi_spi_MasterReadWrite(ADXL372_SPI, &Transceiver) != ADI_SPI_SUCCESS)
    {
        return false;
    }

    return true;
}


/* Function to read the data using SPI interface. */
static bool ADXL372_REG_READ(uint8_t RegAddr, uint8_t *pBuffer)
{
    /* SPI transceiver instance */
    ADI_SPI_TRANSCEIVER Transceiver;
    uint8_t nTemp[2];
    nTemp[0] = (RegAddr<<1) | 0x01; // read flag

    /* Initialize the transceiver */
    Transceiver.pReceiver           =   pBuffer;
    Transceiver.ReceiverBytes       =   1u;
    Transceiver.nRxIncrement        =   1u;
    Transceiver.pTransmitter        =   nTemp;
    Transceiver.TransmitterBytes    =   1;
    Transceiver.nTxIncrement        =   1u;
    Transceiver.bDMA                =   false;
    Transceiver.bRD_CTL             =   true;

    /* Transmit the first sequence */
    if(adi_spi_MasterReadWrite(ADXL372_SPI, &Transceiver) != ADI_SPI_SUCCESS)
    {
        return false;
    }

    return true;
}

/* Function to write the data using SPI interface. Assumes single register write */
static bool ADXL372_REG_WRITE(uint8_t RegAddr, uint8_t Value)
{
    /* SPI transceiver instance */
    ADI_SPI_TRANSCEIVER Transceiver;
    uint8_t                  LocalBuf[2];

    LocalBuf[0] = RegAddr<<1;
    LocalBuf[1] = Value;
    /* Initialize the transceiver */
    Transceiver.pReceiver           =   NULL;
    Transceiver.ReceiverBytes       =   0u;
    Transceiver.nRxIncrement        =   0u;
    Transceiver.TransmitterBytes    =   2u;
    Transceiver.pTransmitter        =   LocalBuf;
    Transceiver.nTxIncrement        =   1u;
    Transceiver.bDMA                =   false;
    Transceiver.bRD_CTL             =   false;

    /* Transmit the first sequence */
    if(adi_spi_MasterReadWrite(ADXL372_SPI, &Transceiver) != ADI_SPI_SUCCESS)
    {
        return false;
    }

    return true;
}


