#ifndef F_CPU
#define F_CPU 8000000UL
#endif

#include <avr/io.h>
#include <util/delay.h>
#include "MCP23017_LIB.h"
#include "TWI_LIB.h"


void mcp23017_init(uint8_t i2c_address)
{
	twi_master_init();
	twi_start(i2c_address, 0); // 0 write 1 read
	twi_write_master(0x00); //IO DIR
	twi_write_master(0); // BANKA = Outputs
	twi_stop_master();
	_delay_us(100); // (!)
	twi_start(i2c_address, 0); // 0 write 1 read
	twi_write_master(0x10); //IO DIR
	twi_write_master(0); // BANKB = Outputs
	twi_stop_master();
	_delay_us(100); // (!)
}

void mcp23017_write_gpio_a(uint8_t i2c_address, uint8_t data)
{
	twi_start(i2c_address, 0); // 0 write 1 read
	twi_write_master(0x0a); //OLAT
	twi_write_master(data); // data
	twi_stop_master();
	_delay_us(100); // (!)
}

void mcp23017_write_gpio_b(uint8_t i2c_address, uint8_t data)
{
	twi_start(i2c_address, 0); // 0 write 1 read
	twi_write_master(0x1a); //OLAT
	twi_write_master(data); // data
	twi_stop_master();
	_delay_us(100); // (!)
}

void mcp23017_clear_all_outputs(uint8_t i2c_address)
{
	mcp23017_write_gpio_a(i2c_address, 0x00);
	mcp23017_write_gpio_b(i2c_address, 0x00);
}

void mcp23017_drive_output(uint8_t i2c_address, uint8_t output_no) //output_no 1..16
{
	if (output_no <= 8) // I/O BANK_A
	{
		mcp23017_write_gpio_b(i2c_address, 0x00);
		mcp23017_write_gpio_a(i2c_address, 1<<(output_no-1));
	
	}
	else if (output_no <=16) // I/O BANK_B
	{
		output_no -= 9;
		mcp23017_write_gpio_a(i2c_address, 0x00);
		mcp23017_write_gpio_b(i2c_address, 1<<output_no);
	}
	else
	{
		//ERROR
	}
	
}