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

void SoftTWI::set_pin(uint8_t pin, bool state)  {
	uint8_t bitmask = digitalPinToBitMask(pin);
	volatile uint8_t* reg = portModeRegister(digitalPinToPort(pin));
	if(state) {
		*reg &= ~bitmask;
	} else {
		*reg |= bitmask;
	}
}

void SoftTWI::begin() {
	digitalWrite(_sda, 0);
	digitalWrite(_scl, 0);
}

bool SoftTWI::read_pin(uint8_t pin) {
	return digitalRead(pin);
}

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
}

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);
	}
}

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;
}