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

uint8_t SoftTWI::read(bool ack) {
	port::Drain sda(_sda);
	port::Drain scl(_scl);

	uint8_t v = 0;
	for(unsigned m = 0x80; m; m >>= 1) {
		sda.set(1);
		scl.set(1);
		while(!scl.get()) {}
		if(sda.get()) {
			v |= m;
		}
		scl.set(0);
	}
	
	// send ACK
	if(ack) {
		sda.set(!ack);
		scl.set(1);
		while(!scl.get()) {}
		scl.set(0);
	}

	return v;
}

bool SoftTWI::write(uint8_t v) {
	port::Drain sda(_sda);
	port::Drain scl(_scl);

	for(unsigned m = 0x80; m; m >>= 1) {
		sda.set(v & m);
		scl.set(1);
		while(!scl.get()) {}
		scl.set(0);
	}

	// get ACK
	sda.set(1);
	scl.set(1);
	while(!scl.get()) {}
	uint8_t s = sda.get();
	scl.set(0);

	return !s;
}

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

bool SoftTWI::start(uint8_t addr) {
	port::Drain sda(_sda);
	port::Drain scl(_scl);
	sda.set(1);
	scl.set(1);
	sda.set(0);
	scl.set(0);
	write(addr);
}

void SoftTWI::end() {
	port::Drain sda(_sda);
	port::Drain scl(_scl);
	sda.set(0);
	scl.set(1);
	sda.set(1);
}