#include "main.h"
#include "PCA9685.h"
#include "lcd.h"

extern I2C_HandleTypeDef hi2c1;

void PCA9685_run_test(void)
{
	DISPLAY_CLEAR;
	display_write_data_seq("PCA9685 PWM");

	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, GPIO_PIN_RESET);

	HAL_StatusTypeDef op_result;
	size_t err_count = 0;
	uint8_t tx_data[10] = {
			0x00, 0x01,
			0x06, 0x00,
			0x07, 0x00,
			0x08, 0xFF,
			0x09, 0x0C,
	};

	for (size_t i = 0; i < 10; i += 2) {
		for (size_t t = 0; t < 5; t++) {
			op_result = HAL_I2C_Master_Transmit(&hi2c1, 0x80, &(tx_data[i]), 2, 2000);

			if (op_result) {
				err_count++;

				DISPLAY_CLEAR;
				display_write_data_seq("PCA9685 PWM");
				DISPLAY_SET_CURSOR(1, 0);

				display_write_data_seq("B");
				display_write_data_byte('0' + i);
				display_write_data_byte(':');

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

				switch (op_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);
			} else {
				break;
			}
		}
	}

	DISPLAY_CLEAR;
	display_write_data_seq("PCA9685 PWM");

	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(" errors");
	} else {
		DISPLAY_SET_CURSOR(1, 1);
		display_write_data_seq("OK");
	}
}

void PCA9685_cleanup(void)
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, GPIO_PIN_SET);
}