#include "soft_twi.hpp"
#include <Arduino.h>

SoftTWI::SoftTWI(unsigned pin_sda, unsigned pin_scl, unsigned delay) {
	_pin_scl = pin_scl;
	_pin_sda = pin_sda;
	_delay = delay;
}

void SoftTWI::begin() {
	digitalWrite(_pin_sda, 0);
	digitalWrite(_pin_scl, 0);
}

void SoftTWI::sleep(unsigned t) {
	if(_delay) {
		delayMicroseconds(_delay * t);
	}
}

void SoftTWI::set_sda(bool s) {
	pinMode(_pin_sda, s ? INPUT : OUTPUT);
}

void SoftTWI::set_scl(bool s) {
	pinMode(_pin_scl, s ? INPUT : OUTPUT);
}

bool SoftTWI::read_sda() {
	return digitalRead(_pin_sda);
}

bool SoftTWI::read_scl() {
	return digitalRead(_pin_scl);
}

void SoftTWI::wait_scl() {
	while(!read_scl()) {}
}

bool SoftTWI::read_bit() {
	set_sda(1);
	sleep();
	set_scl(1);
	sleep(2);
	wait_scl();
	bool s = read_sda();
	set_scl(0);
	sleep();
	return s;
}

void SoftTWI::write_bit(bool s) {
	set_sda(s);
	sleep();
	set_scl(1);
	sleep(2);
	wait_scl();
	set_scl(0);
	sleep();
}

void SoftTWI::start() {
	set_sda(1);
	sleep();
	set_scl(1);
	sleep();
	set_sda(0);
	sleep();
	set_scl(0);
	sleep();
}

void SoftTWI::end() {
	set_sda(0);
	sleep();
	set_scl(1);
	sleep();
	set_sda(1);
	sleep(2);
}

bool SoftTWI::start_read(uint8_t addr) {
	start();
	return write((addr << 1) | 1);
}

bool SoftTWI::start_write(uint8_t addr) {
	start();
	return write(addr << 1);
}

uint8_t SoftTWI::read(bool ack) {
	uint8_t v = 0;
	for(unsigned m = 0x80; m; m >>= 1) {
		if(read_bit()) {
			v |= m;
		}
	}
	write_bit(!ack); // send ACK
	return v;
}

bool SoftTWI::write(uint8_t v) {
	for(unsigned m = 0x80; m; m >>= 1) {
		write_bit(v & m);
	}
	return !read_bit(); // get ACK
}

unsigned SoftTWI::write(const uint8_t* data, unsigned data_len) {
	for(unsigned i = 0; i < data_len; i++) {
		if(!write(data[i])) {
			return i;
		}
	}
	return data_len;
}

void SoftTWI::read(uint8_t* data, unsigned data_len, bool nack_at_end) {
	for(unsigned i = 0; i < data_len; i++) {
		data[i] = read(nack_at_end ? (i < data_len - 1) : true);
	}
}