#include "stm8s.h"
#include <stdio.h>
#include <stdbool.h>
#include "bootstrap.h"

void main(void)
{
	SimpleInit();


	//GPIOC->DDR |= GPIO_PIN_4; // out
	//GPIOC->CR1 &= ~GPIO_PIN_4; // open drain
	//GPIOC->CR2 |= GPIO_PIN_4; // fast

	// Rows
	GPIO_Init(GPIOC, GPIO_PIN_3, GPIO_MODE_OUT_OD_HIZ_FAST);
	GPIO_Init(GPIOC, GPIO_PIN_4, GPIO_MODE_OUT_OD_HIZ_FAST);
	GPIO_Init(GPIOC, GPIO_PIN_5, GPIO_MODE_OUT_OD_HIZ_FAST);
	GPIO_Init(GPIOC, GPIO_PIN_6, GPIO_MODE_OUT_OD_HIZ_FAST);

	// Columns (feedback)
	GPIO_Init(GPIOD, GPIO_PIN_1, GPIO_MODE_IN_PU_NO_IT);
	GPIO_Init(GPIOD, GPIO_PIN_2, GPIO_MODE_IN_PU_NO_IT);
	GPIO_Init(GPIOD, GPIO_PIN_3, GPIO_MODE_IN_PU_NO_IT);
	GPIO_Init(GPIOD, GPIO_PIN_4, GPIO_MODE_IN_PU_NO_IT);

	bool matrix[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	uint16_t counter[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	const char *digits = "123A456B789C*0#D";

#define DEBO 8

	while (1) {
		int b = 0;

		GPIO_WriteLow(GPIOC, GPIO_PIN_3);
		Delay(1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_2);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_3);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_4);
		GPIO_WriteHigh(GPIOC, GPIO_PIN_3);

		GPIO_WriteLow(GPIOC, GPIO_PIN_4);
		Delay(1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_2);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_3);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_4);
		GPIO_WriteHigh(GPIOC, GPIO_PIN_4);

		GPIO_WriteLow(GPIOC, GPIO_PIN_5);
		Delay(1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_2);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_3);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_4);
		GPIO_WriteHigh(GPIOC, GPIO_PIN_5);

		GPIO_WriteLow(GPIOC, GPIO_PIN_6);
		Delay(1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_1);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_2);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_3);
		matrix[b++] = GPIO_ReadInputPin(GPIOD, GPIO_PIN_4);
		GPIO_WriteHigh(GPIOC, GPIO_PIN_6);

		for (int i = 0; i < 16; i++) {
			if (matrix[i] == 0) {
				// pushed
				if (counter[i] < DEBO) {
					counter[i]++;
					if (counter[i] == DEBO) {
						GPIO_WriteLow(GPIOB, GPIO_PIN_5); // LED on
						putchar(digits[i]);
					}
				}
			} else {
				counter[i] = 0;
			}
		}

		Delay(5);
		GPIO_WriteHigh(GPIOB, GPIO_PIN_5); // LED off
	}
}