#include "main.h"
#include "LSM9DS1.h"
#include "lcd.h"

#define FAILSAFE_PRE_OP for (size_t t = 0; t < 5; t++) {
#define FAILSAFE_POST_OP_ACCEL(prefix) if (op_result) print_error_accel(op_result, err_count, t, prefix); else break; }
#define FAILSAFE_POST_OP_MAGNET(prefix) if (op_result) print_error_magnet(op_result, err_count, t, prefix); else break; }

extern I2C_HandleTypeDef hi2c1;

static void print_error_message(HAL_StatusTypeDef result, size_t *err_count, size_t t, char *prefix)
{
	DISPLAY_SET_CURSOR(1, 0);
	display_write_data_seq(prefix);
	display_write_data_byte(' ');

	(*err_count)++;

	display_write_data_byte('1' + t);
	display_write_data_seq("/5 ");

	switch (result) {
	case HAL_ERROR:
		display_write_data_seq("ERROR");
		break;
	case HAL_BUSY:
		display_write_data_seq("BUSY");
		break;
	case HAL_TIMEOUT:
		display_write_data_seq("TIMEOUT");
		break;
	}

	HAL_Delay(1000);
}

static void print_error_accel(HAL_StatusTypeDef result, size_t *err_count, size_t t, char *prefix)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LSM9DS1 Accel");

	print_error_message(result, err_count, t, prefix);
}

static void print_error_magnet(HAL_StatusTypeDef result, size_t *err_count, size_t t, char *prefix)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LSM9DS1 Magnet");

	print_error_message(result, err_count, t, prefix);
}

void LSM9DS1_test_accel(void)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LSM9DS1 Accel");

	HAL_StatusTypeDef op_result;
	uint8_t buffer[6];
	size_t err_count = 0;

	// enable sampling at 10 Hz
	buffer[0] = 0x20;
	buffer[1] = 0x20;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6, buffer, 2, 1000);
	FAILSAFE_POST_OP_ACCEL("E");

	HAL_Delay(100);

	// set future read address
	buffer[0] = 0x28;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6, buffer, 1, 1000);
	FAILSAFE_POST_OP_ACCEL("A");

	// read from registers sequentially
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Receive(&hi2c1, 0xD7, buffer, 6, 1000);
	FAILSAFE_POST_OP_ACCEL("R");

	if (!err_count) {
		DISPLAY_SET_CURSOR(1, 1);
		display_write_data_seq("OK");
	} else {
		DISPLAY_SET_CURSOR(1, 0);
		display_write_data_byte('0' + ((err_count / 10) % 10));
		display_write_data_byte('0' + (err_count % 10));
		display_write_data_seq(" errors");
	}
}

void LSM9DS1_cleanup_accel(void)
{
	// power down the accelerometer
	uint8_t buffer[2] = {0x20, 0x00};
	HAL_I2C_Master_Transmit(&hi2c1, 0xD6, buffer, 2, 1000);
}

void LSM9DS1_test_magnet(void)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LSM9DS1 Magnet");

	HAL_StatusTypeDef op_result;
	uint8_t buffer[6];
	size_t err_count = 0;

	// set future read address
	buffer[0] = 0x28;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0x3C, buffer, 1, 1000);
	FAILSAFE_POST_OP_ACCEL("A");

	// read from registers sequentially
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Receive(&hi2c1, 0x3D, buffer, 6, 1000);
	FAILSAFE_POST_OP_ACCEL("R");

	if (!err_count) {
		DISPLAY_SET_CURSOR(1, 1);
		display_write_data_seq("OK");
	} else {
		DISPLAY_SET_CURSOR(1, 0);
		display_write_data_byte('0' + ((err_count / 10) % 10));
		display_write_data_byte('0' + (err_count % 10));
		display_write_data_seq(" errors");
	}
}