#include <stdbool.h>
#include "mapleComms.h"
#include "mapleWire.h"
#include "printf.h"

static uint8_t mMapleCommsCrc;
static uint8_t mMapleRxBuf[1026] __attribute__ ((aligned (4)));	//one byte over longer possible legal transmission (header + 255 words + crc)

#define SUBDEVICE_ADDR_MASK		0x3F

void mapleCommsStart(uint8_t cmd, uint8_t from, uint8_t to, uint8_t numExtraWords)
{
	mMapleCommsCrc = cmd ^ from ^ to ^ numExtraWords;
	
	mapleWireTxStart();
	
	mapleWireTxByte(numExtraWords);
	mapleWireTxByte(from);
	mapleWireTxByte(to);
	mapleWireTxByte(cmd);
}

void mapleCommsSendWord(uint32_t word)
{
	uint32_t crcLocal = 0, i;
	
	for (i = 0; i < 4; i++, word >>= 8) {
		
		crcLocal ^= word;
		mapleWireTxByte(word);
	}
	
	mMapleCommsCrc ^= crcLocal;
}

void mapleCommsFinish(void)
{
	mapleWireTxByte(mMapleCommsCrc);
	mapleWireTxStop();
}

static bool mapleCommsCrcVerify(const uint8_t *data, uint32_t totalLenInclCrc)
{
	uint32_t crc = 0;
	
	while(totalLenInclCrc--)
		crc ^= *data++;
	
	return !crc;
}

static uint8_t mapleWaitReply(uint8_t devAddr, uint8_t meAddr, uint32_t expectedLen)
{
	uint32_t len;
	
	do {
		len = mapleWireRx(mMapleRxBuf, sizeof(mMapleRxBuf));
		if (len != expectedLen)
			return MAPLE_COMMS_ERR_WRONG_REPLY_LEN;
		
		if (!mapleCommsCrcVerify(mMapleRxBuf, len))
			return MAPLE_COMMS_ERR_CRC_FAIL;
			
	} while (mMapleRxBuf[2] != meAddr || mMapleRxBuf[1] != devAddr);
	
	return 0;
}

static uint8_t mapleWaitAckReply(uint8_t devAddr, uint8_t meAddr)
{
	uint8_t ret;
	
	ret = mapleWaitReply(devAddr, meAddr, 5);
	if (ret)
		return ret;
	
	if (mMapleRxBuf[3] != MAPLE_RSP_MAPLE_CMD_ACK || mMapleRxBuf[0] != 0)
		return MAPLE_COMMS_ERR_NAK;
	
	return 0;
}

uint8_t mapleCommsWriteBlock(uint8_t toAddr, uint8_t partition, uint16_t block, const void *data)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	const uint8_t *dataP = (const uint8_t*)data;
	uint32_t ptWord, i, j, k, v;
	
	ptWord = partition;
	ptWord <<= 24;
	ptWord += block;
	
	for (i = 0; i < 4; i++, ptWord += 0x00010000 /*inc phase */) {
		
		pr(" ph %u\n", i);
		
		mapleCommsStart(MAPLE_CMD_BLK_WR, fromAddr, toAddr, 0x22);
		mapleCommsSendWord(MAPLE_COMMS_FUNC_MEMORY);
		mapleCommsSendWord(ptWord);
		
		for (j = 0; j < 128; j += 4) {
			
			for (k = 0; k < 4; k++)
				v = (v << 8) | *dataP++;
			
			mapleCommsSendWord(v);
		}
		mapleCommsFinish();
	
		ret = mapleWaitAckReply(toAddr, fromAddr);
		if (ret)
			return ret;
		
		pr("OK, now we wait a little bit here\n");
	}

	pr("waiting a little bit now....\n");	//we need a few ms of delay here or vmu will not reply
	mapleCommsStart(MAPLE_CMD_BLK_WR_FINISH, fromAddr, toAddr, 2);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_MEMORY);
	mapleCommsSendWord(ptWord);
	mapleCommsFinish();
	
	ret = mapleWaitAckReply(toAddr, fromAddr);
	if (ret)
		return ret;
	
	pr("REPLY OK\n");
	
	return 0;
}

uint8_t mapleCommsWriteScreen(uint8_t toAddr, const void *data)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	const uint8_t *dataP = (const uint8_t*)data;
	uint32_t j, k, v;
	
	mapleCommsStart(MAPLE_CMD_BLK_WR, fromAddr, toAddr, 0x32);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_DISPLAY);
	mapleCommsSendWord(0);	//pt word
	
	for (j = 0; j < 192; j += 4) {
		
		for (k = 0; k < 4; k++)
			v = (v << 8) | *dataP++;
		
		mapleCommsSendWord(v);
	}
	mapleCommsFinish();
	
	ret = mapleWaitAckReply(toAddr, fromAddr);
	if (ret)
		return ret;
	
	return 0;
}


uint8_t mapleCommsReadBlock(uint8_t toAddr, uint8_t partition, uint16_t block, const void** dataP)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	uint32_t i, j, ptWord;
	
	ptWord = partition;
	ptWord <<= 24;
	ptWord += block;
	
	mapleCommsStart(MAPLE_CMD_BLK_RD, fromAddr, toAddr, 2);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_MEMORY);		//function number (memory)
	mapleCommsSendWord(ptWord);
	mapleCommsFinish();
	
	ret = mapleWaitReply(toAddr, fromAddr, 525);
	if (ret)
		return ret;
	
	if (mMapleRxBuf[3] != MAPLE_RSP_DATA_XFER || mMapleRxBuf[0] != 0x82)
		return MAPLE_COMMS_ERR_NAK;
	
	for (i = 0; i < 512; i += 4) {
		
		for (j = 0; j < 4; j++)
			mMapleRxBuf[i + j] = mMapleRxBuf[i + 12 + 3 - j];
	}
	
	if (dataP)
		*dataP = mMapleRxBuf;
	
	return 0;
}

uint8_t mapleCommsReadButtons(uint8_t toAddr, uint8_t *buttonsP)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	
	mapleCommsStart(MAPLE_CMD_GET_COND, fromAddr, toAddr, 1);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_CLOCK);		//function number (memory)
	mapleCommsFinish();
	
	ret = mapleWaitReply(toAddr, fromAddr, 13);
	if (ret)
		return ret;
	
	if (mMapleRxBuf[3] != MAPLE_RSP_DATA_XFER || mMapleRxBuf[0] != 2)
		return MAPLE_COMMS_ERR_NAK;
	
	if (buttonsP)
		*buttonsP = mMapleRxBuf[11];
	
	return 0;
}

uint8_t mapleCommsGetClock(uint8_t toAddr, uint16_t *yearP, uint8_t *monthP, uint8_t *dayP, uint8_t *hrP, uint8_t *minP, uint8_t *secP)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	uint32_t i, j;
	
	mapleCommsStart(MAPLE_CMD_BLK_RD, fromAddr, toAddr, 2);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_CLOCK);		//function number (memory)
	mapleCommsSendWord(0);
	mapleCommsFinish();
	
	ret = mapleWaitReply(toAddr, fromAddr, 17);
	if (ret)
		return ret;
	
	if (mMapleRxBuf[3] != MAPLE_RSP_DATA_XFER || mMapleRxBuf[0] != 3)
		return MAPLE_COMMS_ERR_NAK;
	
	if (dayP)
		*dayP = mMapleRxBuf[8];
		
	if (monthP)
		*monthP = mMapleRxBuf[9];
	
	if (yearP)
		*yearP = (((uint16_t)mMapleRxBuf[10]) << 8) + mMapleRxBuf[11];
	
	if (secP)
		*secP = mMapleRxBuf[13];
	
	if (minP)
		*minP = mMapleRxBuf[14];
	
	if (hrP)
		*hrP = mMapleRxBuf[15];
	
	return 0;
}

uint8_t mapleCommsSetClock(uint8_t toAddr, uint16_t year, uint8_t month, uint8_t day, uint8_t hr, uint8_t min, uint8_t sec)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	
	mapleCommsStart(MAPLE_CMD_BLK_WR, fromAddr, toAddr, 4);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_CLOCK);		//function number (memory)
	mapleCommsSendWord(0);
	
	mapleCommsSendWord((((uint32_t)(year & 0xFF)) << 24) | (((uint32_t)(year & 0xFF00)) << 8) | (((uint32_t)month) << 8) | day);
	mapleCommsSendWord((((uint32_t)hr) << 24) | (((uint32_t)min) << 16) | (((uint32_t)sec) << 8));
	mapleCommsFinish();
	
	ret = mapleWaitAckReply(toAddr, fromAddr);
	if (ret)
		return ret;
	
	return 0;
}

uint8_t mapleCommsSetBuzzer(uint8_t toAddr, uint8_t period, uint8_t dutyCy)
{
	uint8_t ret, fromAddr = toAddr &~ SUBDEVICE_ADDR_MASK;
	
	mapleCommsStart(MAPLE_CMD_SET_COND, fromAddr, toAddr, 2);
	mapleCommsSendWord(MAPLE_COMMS_FUNC_CLOCK);		//function number (memory)
	mapleCommsSendWord((((uint32_t)dutyCy) << 16) |  (((uint32_t)period) << 24));
	mapleCommsFinish();
	
	ret = mapleWaitAckReply(toAddr, fromAddr);
	if (ret)
		return ret;
	
	return 0;
}

