#include <pca9555.h>
#include <i2c.h>

static void pca_write_register(uint8_t reg, uint8_t value)
{
	uint8_t i2c_command[2];

	i2c_command[0] = reg;
	i2c_command[1] = value;
	i2c_write(PCA9555_I2C_ADDR, i2c_command, 2u);
}

static uint8_t pca_read_register(uint8_t reg)
{
	uint8_t val = 0;

	i2c_read(PCA9555_I2C_ADDR, reg, &val, 1u);

	return val;
}

void port_expander_init(void)
{
	i2c_init();

	/* Port 0: Input for buttons */
	pca_write_register(PCA9555_REG_CONFIG0, 0xFF);

	/* Port 1: Output for LEDs */
	pca_write_register(PCA9555_REG_CONFIG1, 0x00);
}

uint8_t port_expander_get_buttons(void)
{
	return pca_read_register(PCA9555_REG_IN_PORT0);
}

void port_expander_set_leds(uint8_t leds)
{
	pca_write_register(PCA9555_REG_OUT_PORT1, ~leds);
}

uint8_t port_expander_get_leds(void)
{
	return ~pca_read_register(PCA9555_REG_OUT_PORT1);
}

void port_expander_set_single_led(uint8_t pos, uint8_t val)
{
	uint8_t port_reg = 0;

	if (pos > 7)
		return;

	val = val ? 1 : 0;
	val = (val << pos);

	port_reg = port_expander_get_leds();
	port_reg &= ~(1 << pos);
	port_reg |= val;

	port_expander_set_leds(port_reg);
}