#include "main.h"
#include "lcd.h"
#include "LIS302DL.h"

#define FAILSAFE_PRE_OP for (size_t t = 0; t < 5; t++) {
#define FAILSAFE_POST_OP(prefix) if (op_result) print_error_message(op_result, err_count, t, prefix, postfix); else break; }

extern I2C_HandleTypeDef hi2c1;

static char convert_char_at(uint8_t value, size_t position)
{
	char d = (value >> (4 * position)) % 16;

	if (d > 9)
		d += 7;

	return '0' + d;
}

static void print_at(uint8_t value, size_t offset)
{
	DISPLAY_SET_CURSOR(1, offset);

	// most significant digit
	display_write_data_byte(convert_char_at(value, 1));

	// least significant digit
	display_write_data_byte(convert_char_at(value, 0));
}

static void print_error_message(HAL_StatusTypeDef result, size_t *err_count, size_t t, char *prefix, char *postfix)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LIS302DL Accel ");
	display_write_data_seq(postfix);
	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(2000);
}

static void retrieve_data(size_t *err_count, uint8_t *data_xyz, char *postfix)
{
	HAL_StatusTypeDef op_result;
	uint8_t data_buffer[2];

	// enable device
	data_buffer[0] = 0x20;
	data_buffer[1] = 0x47;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6, data_buffer, 2, 2000);
	FAILSAFE_POST_OP("E");

	// select OutX register
	data_buffer[0] = 0x29;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6 | 0x1, data_buffer, 1, 2000);
	FAILSAFE_POST_OP("SX");

	// receive data from it
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Receive(&hi2c1, 0xD6 | 0x1, data_xyz, 1, 2000);
	FAILSAFE_POST_OP("RX");

	// select OutY register
	data_buffer[0] = 0x2B;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6 | 0x1, data_buffer, 1, 2000);
	FAILSAFE_POST_OP("SY");

	// receive data from it
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Receive(&hi2c1, 0xD6 | 0x1, &(data_xyz[1]), 1, 2000);
	FAILSAFE_POST_OP("RY");

	// select OutZ register
	data_buffer[0] = 0x2D;
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Transmit(&hi2c1, 0xD6 | 0x1, data_buffer, 1, 2000);
	FAILSAFE_POST_OP("SZ");

	// receive data from it
	FAILSAFE_PRE_OP;
	op_result = HAL_I2C_Master_Receive(&hi2c1, 0xD6 | 0x1, &(data_xyz[2]), 1, 2000);
	FAILSAFE_POST_OP("RZ");
}

void LIS302DL_run_test(void)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LIS302DL Accel");

	size_t err_count = 0;
	uint8_t data_xyz[3];

	retrieve_data(&err_count, data_xyz, "");

	DISPLAY_CLEAR;
	display_write_data_seq("LIS302DL Accel");

	// output retrieved values
	print_at(data_xyz[2], 14);
	print_at(data_xyz[1], 11);
	print_at(data_xyz[0], 8);

	// print the execution stats
	if (err_count) {
		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(" errs");
	} else {
		DISPLAY_SET_CURSOR(1, 1);
		display_write_data_seq("OK");
	}
}

void LIS302DL_run_test_dynamic(void)
{
	DISPLAY_CLEAR;
	display_write_data_seq("LIS302DL Accel D");

	size_t err_count = 0;
	uint8_t data_xyz[3];

	retrieve_data(&err_count, data_xyz, "D");

	DISPLAY_CLEAR;
	display_write_data_seq("LIS302DL Accel D");

	// output retrieved values
	print_at(data_xyz[2], 14);
	print_at(data_xyz[1], 11);
	print_at(data_xyz[0], 8);

	// print the execution stats
	if (err_count) {
		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(" errs");
	} else {
		DISPLAY_SET_CURSOR(1, 1);
		display_write_data_seq("OK");
	}
}