/* Includes ------------------------------------------------------------------*/
#include <string.h>

#include "stm32f0xx_hal.h"
#include "globals.h"
#include "sensors.h"

static void Handle_LSM6DSL_Read(void);
static void Handle_LIS3MDL_Read(void);
/*These are the functions for interfacing with the SPI sensors*/
//LIS3MDL
static const uint8_t LIS3MDL_Config_[]={0x60,0x02,0x00,0x00,0x00,0x40};/*Write registers 0x20 (0x40-Address incr+0x20=0x60) to 0x24*/
static const uint8_t LIS3MDL_Bytes=6;/*^ sets 1khz update with BDU and little endianess, +-4Guass sensor range*/
static const uint8_t LIS3MDL_Read_Bytes[]={0xE8,0x00,0x00,0x00,0x00,0x00,0x00};
static const uint8_t LIS3MDL_Read_Transaction=7;

#define LIS3_CS_LOW HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2,GPIO_PIN_RESET)
#define LIS3_CS_HIGH HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2,GPIO_PIN_SET)
//LSM6DSL
static const uint8_t LSM6DSL_Config_One[]={0x0D,0x03};/*Single byte to set up DRDY on INT1 pin*/
static const uint8_t LSM6DSL_Config_One_Bytes=2;
static const uint8_t LSM6DSL_Config_Two[]={0x10,0xAE,0xAC,0x44,0x06,0x00,0x03};
/*6.66khz output with 1.5khz analogue front end +-8G, 6.66khz +-2kdbps, BDU addr inc, LPF on gyro, no ST, 937hz gyro front end*/
static const uint8_t LSM6DSL_Config_Two_Bytes=7;
static const uint8_t LSM6DSL_Read_Bytes[]={0xA0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
static const uint8_t LSM6DSL_Read_Transaction=15;/*One byte to config, then fourteen bytes to read*/

#define LSM6_CS_LOW HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3,GPIO_PIN_RESET);
#define LSM6_CS_HIGH HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3,GPIO_PIN_SET);
//Buffers which are used to store the raw data
volatile uint8_t rawbuff[15]={};/*This is used for all the sensors for the dma, memcpy is then used to copy to main data buffer in callback function*/
volatile uint8_t* LSM6DSL_datacallback=NULL;
volatile uint8_t* LIS3MDL_datacallback=NULL;
volatile uint8_t LIS3MDL_Flag=0,LSM6DSL_Flag=0;

uint8_t Sensors=0;          /*Used to hold flags for the sensors*/
void (* volatile handlercallback)(void);/*Callback function pointer*/

//These four functions are used to interface

/*This function configures the LIS3MDL magnetometer*/
uint8_t LIS3MDL_Config() {
    LIS3_CS_LOW;
    uint8_t status=0;
    status=HAL_SPI_Transmit_DMA(&hspi1, LIS3MDL_Config_, LIS3MDL_Bytes);
    if(status) {
        LIS3_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for transmit to be completed*/
    LIS3_CS_HIGH;
    uint8_t device_id=0xCF;
    asm volatile("dmb");
    LIS3_CS_LOW;
    status=HAL_SPI_Transmit_DMA(&hspi1, &device_id, 1);
    if(status) {
        LIS3_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for transmit to be completed*/
    status=HAL_SPI_Receive_DMA(&hspi1, &device_id, 1);
    if(status) {
        LIS3_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for receive to be completed*/
    LIS3_CS_HIGH;
    if(device_id==0x3D)     /*Sensor repies with the correct ID byte*/
        return 0;
    else
        return 1;
}

/*This function configures the LIS3MDL magnetometer*/
uint8_t LSM6DSL_Config() {
    LSM6_CS_LOW;
    uint8_t status=0;
    status=HAL_SPI_Transmit_DMA(&hspi1, LSM6DSL_Config_One, LSM6DSL_Config_One_Bytes);
    if(status) {
        LSM6_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for transmit to be completed*/
    LSM6_CS_HIGH;
    asm volatile("dmb");
    LSM6_CS_LOW;
    status=HAL_SPI_Transmit_DMA(&hspi1, LSM6DSL_Config_Two, LSM6DSL_Config_Two_Bytes);
    if(status) {
        LSM6_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for transmit to be completed*/
    LSM6_CS_HIGH;
    uint8_t device_id=0x8F;
    asm volatile("dmb");
    LSM6_CS_LOW;
    asm volatile("dmb");
    status=HAL_SPI_Transmit_DMA(&hspi1, &device_id, 1);
    if(status) {
        LSM6_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for transmit to be completed*/
    status=HAL_SPI_Receive_DMA(&hspi1, &device_id, 1);
    if(status) {
        LSM6_CS_HIGH;
        return status;      /*An error*/
    }
    while(hspi1.State != HAL_SPI_STATE_READY);/*Wait for receive to be completed*/
    LSM6_CS_HIGH;
    if(device_id==0x6A)     /*Sensor repies with the correct ID byte*/
        return 0;
    else
        return 1;
}

uint8_t LSM6DSL_Read(lsm6sensortype* receivedata, uint32_t timestamp) {
    receivedata->updateflag=1;  /*This can be used to check for an ongoing update, but it isnt thread safe, be careful where it is checked from*/
    receivedata->timestamp=timestamp;
    LSM6_CS_LOW;
    LSM6DSL_datacallback=(uint8_t*)receivedata;
    void (* volatile handlercallback_)(void);
    handlercallback_=handlercallback;
    handlercallback=Handle_LSM6DSL_Read;//tx/rx uses rx complete callback handling
    HAL_StatusTypeDef her=HAL_SPI_TransmitReceive_DMA(&hspi1, LSM6DSL_Read_Bytes, rawbuff, LSM6DSL_Read_Transaction);
    if(her==HAL_BUSY) {//The tx/rx
        handlercallback=handlercallback_;//Restore previous value
        LSM6DSL_Flag=1;//Flag this so next transaction can begin immediatly afterwards
        her=HAL_OK;
    }
    else if(her==HAL_OK)
        LSM6DSL_Flag=0;
    return her;
}

uint8_t LIS3MDL_Read(lis3sensortype* receivedata, uint32_t timestamp) {
    receivedata->updateflag=1;  /*This can be used to check for an ongoing update, but it isnt thread safe, be careful where it is checked from*/
    receivedata->timestamp=timestamp;
    LIS3MDL_datacallback=(uint8_t*)receivedata;
    if(!LIS3MDL_Flag) {
        LIS3MDL_Flag=1;//This is flagged and will be handled by the next LSM6DSL callback (as the two sensors share the bus and LSM6DSM is much faster)
        return 0;
    }
    else
        return 1;           /*This is an error*/
}

uint8_t Sensors_CheckChanged(samplestype* data) {
    static samplestype internalsample;
    static uint8_t unchanged;
    uint8_t Fault=0;
    if(!memcmp(data,&internalsample,sizeof(sensortype)*2))
        Fault|=(1<<LSM6);
    if(!memcmp(&(data->mag),&(internalsample.mag),sizeof(sensortype)))
        Fault|=(1<<LIS3);
    memcpy(&internalsample,data,sizeof(samplestype));
    if(Fault)
        unchanged++;
    else
        unchanged=0;
    if(unchanged>30)            /*30 consecutive calls with unchanged data leads to Fault condition*/
        unchanged=30;
    else
        Fault=0;
    return Fault;
}

//Used to check what the status is without using globals
uint8_t LIS3MDL_Being_Read(void) {
    return LIS3MDL_Flag;
}

//user function for HAL
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
    if(handlercallback)
        handlercallback();
}

//Internal functions

/* This is used as a callback function to handle read of LSM6DSL */
static void Handle_LSM6DSL_Read(void) {
    if(LSM6DSL_datacallback) {
        memcpy(LSM6DSL_datacallback,&(rawbuff[1]),14);//Copy 14 bytes of raw data, ignoring the first byte which was address
        ((lsm6sensortype*)LSM6DSL_datacallback)->updateflag=0;/*Clear ongoing update - isnt thread safe, be careful where it is checked from*/
    }
    LSM6_CS_HIGH;           /*Deselect the sensor*/
    if(LIS3MDL_Flag && !LSM6DSL_Flag) { /*There is some LIS3MDL data ready*/
        LIS3_CS_LOW;
        handlercallback=Handle_LIS3MDL_Read;
        if(HAL_SPI_TransmitReceive_DMA(&hspi1, LIS3MDL_Read_Bytes, rawbuff, LIS3MDL_Read_Transaction)) {
            handlercallback=NULL;
            LIS3_CS_HIGH;
        }
    }
    else if(LSM6DSL_Flag) {
        asm volatile("dmb");
        LSM6_CS_LOW;
        HAL_SPI_TransmitReceive_DMA(&hspi1, LSM6DSL_Read_Bytes, rawbuff, LSM6DSL_Read_Transaction);
        LSM6DSL_Flag=0;     //No queued request
    }
    else
        handlercallback=NULL;
}

static void Handle_LIS3MDL_Read(void) {
    if(LIS3MDL_datacallback) {
        memcpy(LIS3MDL_datacallback,&(rawbuff[1]),6);//Copy 6 bytes of raw data, ignoring the first byte which was address
        ((lis3sensortype*)LIS3MDL_datacallback)->updateflag=0;/*Clear ongoing update - isnt thread safe, be careful where it is checked from*/
    }
    LIS3_CS_HIGH;
    LIS3MDL_Flag=0;
    if(LSM6DSL_Flag) {
        LSM6_CS_LOW;
        handlercallback=Handle_LSM6DSL_Read;
        HAL_SPI_TransmitReceive_DMA(&hspi1, LSM6DSL_Read_Bytes, rawbuff, LSM6DSL_Read_Transaction);
        LSM6DSL_Flag=0;     //No queued request
    }
    else
        handlercallback=NULL;
}