#pragma once

#include <inttypes.h>
#include "util.hpp"

struct SoftTWI {
	
	constexpr SoftTWI(unsigned sda, unsigned scl, unsigned delay = 0);

	void begin();
	inline void sleep(uint8_t=1) AW_IN {}
	inline void wait_scl() AW_IN;
	
	void set_pin(uint8_t, bool);
	inline void set_sda(bool) AW_IN;
	inline void set_scl(bool) AW_IN;
	
	bool read_pin(uint8_t);
	inline bool read_sda() AW_IN;
	inline bool read_scl() AW_IN;

	inline void write_bit(bool) AW_IN;
	bool write(uint8_t);
	unsigned write(const uint8_t*, unsigned);
	
	inline bool read_bit() AW_IN;
	uint8_t read(bool=true);
	void read(uint8_t*, unsigned, bool=false);

	inline bool start_read(uint8_t) AW_IN;
	inline bool start_write(uint8_t) AW_IN;

	inline void start() AW_IN;
	inline void end() AW_IN;
	
private:
	unsigned _sda;
	unsigned _scl;
	unsigned _delay;
};

constexpr SoftTWI::SoftTWI(unsigned sda, unsigned scl, unsigned delay)
	: _sda(sda)
	, _scl(scl)
	, _delay(delay) {
}

inline void SoftTWI::set_sda(bool s) {
	set_pin(_sda, s);
}

inline void SoftTWI::set_scl(bool s) {
	set_pin(_scl, s);
}

inline bool SoftTWI::read_sda() {
	return read_pin(_sda);
}

inline bool SoftTWI::read_scl() {
	return read_pin(_scl);
}

inline void SoftTWI::wait_scl() {
	while(!read_pin(_scl)) {}
}

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

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

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

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

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

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