/*
 * bma_accelerometer.h
 *
 *  Created on: 08 нояб. 2015 г.
 *      Author: Sergey
 */

#ifndef SRC_BMA_ACCELEROMETER_H_
#define SRC_BMA_ACCELEROMETER_H_

#include "stm32f1xx_hal.h"
#include "accelerometer.h"
extern "C"{
#include "bma2x2.h"
}

#define SPI_BUFFER_LEN 10

void swap_bytes(s16* value){
	s8* bytes = (s8*)value;
	auto tmp = bytes[0];
	bytes[0] = bytes[1];
	bytes[1] = tmp;
}

template<SPI_HandleTypeDef* spi>
class bma_accelerometer : public Accelerometer {
public:
	bma_accelerometer(){
		BMA_bind_SPI(&_bma2x2);
	}
	bool init(){
		auto result = bma2x2_init(&_bma2x2);
		result += bma2x2_set_bw(BMA2x2_BW_62_50HZ);
		return result == 0;
	}
	bool set_power(bool on) {
		return 0 == bma2x2_set_power_mode(on ? BMA2x2_MODE_NORMAL : BMA2x2_MODE_DEEP_SUSPEND);
	}

	float3 get_acceleration() override {
		u8 range = 0;
		if(0 != bma2x2_get_range(&range)){
			return float3(0,0,0);
		}

		//14 bit resolution. Hardcoded for now.
		constexpr float max_resolution_val = (float)0x1fff;

		float range_mul = 2.0f;
		if(range == BMA2x2_RANGE_4G){
			range_mul = 4.0f;
		}else if(range == BMA2x2_RANGE_8G){
			range_mul = 8.0f;
		}else if(range == BMA2x2_RANGE_16G){
			range_mul = 16.0f;
		}

		//BMA2x2_RANGE_2G
		bma2x2_accel_data sample_xyz;


		if(0 == bma2x2_read_accel_xyz(&sample_xyz)){
			return float3(
					(float)(sample_xyz.x) / max_resolution_val,
					(float)(sample_xyz.y) / max_resolution_val,
					(float)(sample_xyz.z) / max_resolution_val) * range_mul;
		}else{
			return float3(0,0,0);
		}
	}
private:
	static void SPI_begin(){
		HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_RESET);
	}

	static void SPI_end(){
		HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_SET);
	}

	static s8 SPI_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
		u8 array[SPI_BUFFER_LEN]={0xFF};
		u8 stringpos;


		array[0] = reg_addr|0x80;
		SPI_begin();
		auto spi_result = HAL_SPI_TransmitReceive(spi, array, array, cnt+1, HAL_MAX_DELAY);
		SPI_end();
		for (stringpos = 0; stringpos < cnt; stringpos++) {
			*(reg_data + stringpos) = array[stringpos+1];
		}
		return spi_result == HAL_OK ? 0 : -1;
	}

	static s8 SPI_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
		u8 array[SPI_BUFFER_LEN * 2];
		u8 stringpos = 0;
		for (stringpos = 0; stringpos < cnt; stringpos++) {

			array[stringpos * 2] = (reg_addr++) & 0x7F;
			array[stringpos * 2 + 1] = *(reg_data + stringpos);
		}

		SPI_begin();
		auto spi_result = HAL_SPI_Transmit(spi, array, cnt * 2, HAL_MAX_DELAY);
		SPI_end();

		return spi_result == HAL_OK ? 0 : -1;
	}

	static void delay_msek(u32 msek){
		HAL_Delay(msek);
	}

	static s8 BMA_bind_SPI(bma2x2_t* bma) {
		bma->bus_write = SPI_bus_write;
		bma->bus_read = SPI_bus_read;
		bma->delay_msec = delay_msek;
		return 0;
	}

	bma2x2_t _bma2x2;
};


#endif /* SRC_BMA_ACCELEROMETER_H_ */