extern "C" {
	#include <avr/pgmspace.h>
	#include <avr/interrupt.h>
	#include <stdio.h>
	#include <string.h>
	#include <inttypes.h>
	#include <stdlib.h>

	#include "wiring_private.h"
	#include "wiring.h"
	#include "WConstants.h"
	#include <HardwareSerial.h>
	
	#include "pins_arduino.h"
}

#include "avrmacros.h"
#include "hdq.h"

#define _HDQ_readPin() (*inputReg & bitmask)>>pin /* Change me to inline!*/

HDQ::HDQ(uint8_t pinArg)
{
	pin			= pinArg;
	port		= digitalPinToPort(pin);
	bitmask		= digitalPinToBitMask(pin);
	outputReg	= portOutputRegister(port);
	inputReg	= portInputRegister(port);
	modeReg		= portModeRegister(port);
}

void HDQ::doBreak(void)
{
	sbi(*modeReg, pin);	// Set pin as output
	cbi(*outputReg, pin);	// Bring pin low
	
	// Wait the minimum break time
	delayMicroseconds(HDQ_DELAY_TB);
	
	sbi(*outputReg, pin);	// Bring the pin high again
	
	// Wait the minimum break time
	delayMicroseconds(HDQ_DELAY_TBR);
}

void HDQ::writeByte(uint8_t payload)
{
	
	for (uint8_t ii = 0; ii < 8; ii++)
	{
		sbi(*modeReg, pin);	// Set pin as output
		cbi(*outputReg, pin); // Bring pin low
		// Wait the minimum break time
		delayMicroseconds(HDQ_DELAY_BIT_START);
	
		// Toggle the pin for this bit, LSB first
		if (payload>>ii & 0x01) {
			sbi(*outputReg, pin);
		}
		else {
			cbi(*outputReg, pin);
		}
	
		delayMicroseconds(HDQ_DELAY_BIT_WRITE);
	
		sbi(*outputReg, pin);

		delayMicroseconds(HDQ_DELAY_BIT_END);
	}
	
	return;
}

boolean HDQ::write(uint8_t reg, uint8_t payload, boolean verif)
{
	HDQ::write(reg, payload);
	
	if (payload == HDQ::read(reg)) return true;
	
	return false;
}

boolean HDQ::write(uint8_t reg, uint8_t payload)
{
	HDQ::doBreak();
	
	HDQ::writeByte((reg |= HDQ_ADDR_MASK_WRITE));
	
	delayMicroseconds((HDQ_DELAY_TRSPS_MAX - HDQ_DELAY_BIT_TOTAL) / 2);
	
	HDQ::writeByte(payload);
	
	return false;
}

uint8_t HDQ::read(uint8_t reg)
{
	uint8_t result		= 0;
	uint8_t maxTries	= HDQ_DELAY_FAIL_TRIES; // ~128uS at 8Mhz with 8 instructions per loop
	uint8_t ii;
	
	HDQ::doBreak();
	
	HDQ::writeByte((reg |= HDQ_ADDR_MASK_READ));
	
	for (ii = 0; ii < 8; ii++)
	{
		// Wait for the slave to toggle a low
		maxTries = HDQ_DELAY_FAIL_TRIES;
		while (_HDQ_readPin() != 0 && maxTries-- > 0)
			if (maxTries == 1) return 0xFF;
		
		delayMicroseconds(HDQ_DELAY_BIT_WRITE / 2 + HDQ_DELAY_BIT_START); // 100 / 2 + 30
		
		result |= _HDQ_readPin()<<ii;
		
		delayMicroseconds(HDQ_DELAY_TSSUB - (HDQ_DELAY_BIT_WRITE / 2 + HDQ_DELAY_BIT_START) + 10); // pass Tssub
		
		// Wait for the slave to toggle a high again
		maxTries = HDQ_DELAY_FAIL_TRIES;
		while (_HDQ_readPin() != 1 && maxTries-- > 0)
			if (maxTries == 1) return 0xFF;
		
	}
	
	return result;
}