#include "stm32l0xx_hal_conf.h"
#include "stm32l0xx_hal.h"
#include <math.h>
#include "init.h"
#include "akcelerometr.h"



// #########################################################################################################
uint8_t Read(uint8_t ADDR)
{
    return ADDR | 0b10000000;
};

// #########################################################################################################
uint8_t Write(uint8_t ADDR)
{
    return ADDR & 0b01111111;
};

// #########################################################################################################
void Akcelerometr_Init(void)
{
       HAL_GPIO_WritePin (CS_AKC, LOW);
       SPI_TransmitReceive(Write(CTRL1));            // write CTRL1 control register 1
       SPI_TransmitReceive(0b00110100);              // xxHz, high power 14bit resolution
       SPI_TransmitReceive(0b00000110);              // CTRL2 control register 2 - register addres increment enable, i2c disable
       HAL_GPIO_WritePin (CS_AKC, HIGH); 
};

// #########################################################################################################
void Akcelerometr_ReadRaw(uint8_t* Data)
{
       uint8_t tmp = 0;

       HAL_GPIO_WritePin (CS_AKC, LOW);
       SPI_TransmitReceive(0x27 | 0b10000000);       //STATUS read
       tmp = SPI_TransmitReceive(0xFF);
       HAL_GPIO_WritePin (CS_AKC, HIGH);
    
       if(tmp & 0b00000001)     //if data ready
       {
        HAL_GPIO_WritePin (CS_AKC, LOW);
        SPI_TransmitReceive(Read(OUT_X_L));
        for(uint8_t i = 6; i>0;i--)
        {
            *Data = SPI_TransmitReceive(0xFF);
            *Data++;
        }
        HAL_GPIO_WritePin (CS_AKC, HIGH);       
       }
};

// #########################################################################################################
void Akcelerometr_SklonNaklon(volatile int16_t *sklon,volatile int16_t *naklon,volatile int16_t *odklon)
{
    volatile int16_t Osa_x;
    volatile int16_t Osa_y;
    volatile int16_t Osa_z;
    volatile uint8_t Data[6];
    volatile int X = 0;
    volatile int Y = 0;
    volatile int Z = 0;

    Akcelerometr_ReadRaw((uint8_t*)&Data);
    Osa_x = ((Data[1] & 0x00FF) << 8 ) | Data[0];             // uprava raw dat 
    Osa_y = ((Data[3] & 0x00FF) << 8 ) | Data[2];   
    Osa_z = ((Data[5] & 0x00FF) << 8 ) | Data[4]; 

    X = (AKCELEROMETR_ROZSAH*Osa_x)/AKCELEROMETR_ROZLISENI;   // prepocet na jednotky: mg
    Y = (AKCELEROMETR_ROZSAH*Osa_y)/AKCELEROMETR_ROZLISENI; 
    Z = (AKCELEROMETR_ROZSAH*Osa_z)/AKCELEROMETR_ROZLISENI;

    *sklon = round(57.2958 * atan(X/(sqrt(Y*Y+Z*Z))));        // jednotky: °
    *naklon = round(57.2958 * atan(Y/(sqrt(X*X+Z*Z)))); 
    *odklon  = round(57.2958 * atan(Z/(sqrt(X*X+Y*Y)))); 
};





