/*
 * MLX90614.cpp
 *
 *  Created on: Oct 21, 2015
 *      Author: christoph
 */

#include "MLX90614.h"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <Os/Log.h>
#include <config/Board.h>

namespace MLX90614
{
	namespace TWIMaster
	{
		void init()
		{
			TWIC.CTRL = TWI_SDAHOLD_OFF_gc;
			TWIC.MASTER.CTRLA = TWI_MASTER_INTLVL_MED_gc +
								TWI_MASTER_RIEN_bm +
								TWI_MASTER_WIEN_bm +
								TWI_MASTER_ENABLE_bm;
			TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_50US_gc;
							//TWI_MASTER_SMEN_bm; can be considered
			TWIC.MASTER.BAUD = 155; // set to 100khz

			//Board::PinMlcScl::setDirection(true);
			//Board::PinMlcSda::setDirection(true);

			LOG_MSG("TWIC Init Complete",570);
		}

		const uint8_t SEND_BUFF_SIZE = 8;
		uint8_t sendBuff[SEND_BUFF_SIZE];
		uint8_t sendBuffPos = 0;
		uint8_t bytesToSend = 0;

		const uint8_t RECEIVE_BUFF_SIZE = 8;
		uint8_t receiveBuff[SEND_BUFF_SIZE];
		uint8_t bytesToReceive = 0;
		uint8_t bytesReceived = 0;

		void sendReceive(uint8_t address)
		{
			LOG_MSG("Start",580);
			TWIC.MASTER.CTRLC = TWI_MASTER_CMD_REPSTART_gc;
			sendBuffPos = 0;
			bytesReceived = 0;
			LOG_MSG("Send Address",581);
			TWIC.MASTER.ADDR = address << 1;
		}

		ISR(TWIC_TWIM_vect)
		{
			if(TWIC.MASTER.STATUS & (TWI_MASTER_RXACK_bm | TWI_MASTER_ARBLOST_bm | TWI_MASTER_BUSERR_bm))
			{
				LOG_MSG("Stop (ERROR)",586);
				TWIC.MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc;
			}
			else if(TWIC.MASTER.STATUS & TWI_MASTER_WIF_bm)
			{


				if(sendBuffPos < bytesToSend)
				{
					LOG_MSG("Send Data",582);
					TWIC.MASTER.DATA = sendBuff[sendBuffPos++];
				}
				else
				{
					LOG_MSG("Send Receive Address",583);
					TWIC.MASTER.ADDR |= 0x01; //Go to Read Address
					//TWIC.MASTER.CTRLC = TWI_MASTER_CMD_REPSTART_gc; //Repeated Start Happens automatically
				}
			}
			else if(TWIC.MASTER.STATUS & TWI_MASTER_RIF_bm)
			{
				if(bytesReceived < bytesToReceive)
				{
					LOG_MSG("Receive Data",584);
					receiveBuff[bytesReceived++] = TWIC.MASTER.DATA;
				}
				else
				{
					LOG_MSG("Stop",585);
					TWIC.MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc;
				}
			}

		}

	}

	void init()
	{
		TWIMaster::init();
	}

	void timerEvent()
	{
		//Board::PinMlcScl::toggle();
		//Board::PinMlcSda::toggle();
		TWIC.MASTER.CTRLC = TWI_MASTER_CMD_REPSTART_gc;

		TWIMaster::sendBuff[0] = 0x07;
		TWIMaster::bytesToSend = 1;
		TWIMaster::bytesToReceive = 2;
		TWIMaster::sendReceive(0x5A);

	}
}