#ifndef _MOUSE_SENSOR_H
#define _MOUSE_SENSOR_H

#include "../../stdafx.h"
#include "sensor.h"

class Mouse_Sensor : public Sensor
{
public:
	Mouse_Sensor()
	{
		this->parent = NULL;
		this->moduleId = 0;
		this->hardwarePort = NULL;
		this->hardwareDDR = NULL;
		this->hardwarePin = NULL;
		this->pinSDA = 0;
		this->pinSCK = 0;
		this->registerConfig = 0;
		this->registerPixelData = 0;
		this->registerSqual = 0;
		this->registerDeltaX = 0;
		this->registerDeltaY = 0;
		this->configReset = 0;
		this->configAwake = 0;
		this->newImage = false;
		this->positionX = 0;
		this->positionY = 0;
	}

	Mouse_Sensor(uint32 sensorId)
	{
		this->parent = NULL;
		this->moduleId = sensorId;
		this->newImage = false;

		switch(sensorId)
		{
			case IO_SENSOR_MOUSE_LEFT:
				this->hardwarePort = &PORTC;
				this->hardwareDDR = &DDRC;
				this->hardwarePin = &PINC;
				this->pinSDA = (1 << 4);
				this->pinSCK = (1 << 6);
				this->registerConfig = 0x00;
				this->registerPixelData = 0x08;
				this->registerSqual = 0x04;
				this->registerDeltaX = 0x03;
				this->registerDeltaY = 0x02;
				this->configReset = 0x80;
				this->configAwake = 0x01;
				this->positionX = -3.88f * TICKS_PER_CM;
				this->positionY = -3.88f * TICKS_PER_CM;
				break;
			case IO_SENSOR_MOUSE_RIGHT:
				this->hardwarePort = &PORTC;
				this->hardwareDDR = &DDRC;
				this->hardwarePin = &PINC;
				this->pinSDA = (1 << 7);
				this->pinSCK = (1 << 5);
				this->registerConfig = 0x00;
				this->registerPixelData = 0x08;
				this->registerSqual = 0x04;
				this->registerDeltaX = 0x03;
				this->registerDeltaY = 0x02;
				this->configReset = 0x80;
				this->configAwake = 0x01;
				this->positionX = -3.88f * TICKS_PER_CM;
				this->positionY = 3.88f * TICKS_PER_CM;
				break;
			default:
				this->hardwarePort = NULL;
				this->hardwareDDR = NULL;
				this->hardwarePin = NULL;
				this->pinSDA = 0;
				this->pinSCK = 0;
				this->registerConfig = 0;
				this->registerPixelData = 0;
				this->registerSqual = 0;
				this->registerDeltaX = 0;
				this->registerDeltaY = 0;
				this->configReset = 0;
				this->configAwake = 0;
				break;
		}

		*hardwareDDR  |= pinSCK;
		*hardwarePort &= ~pinSCK;
	
		Write(registerConfig, configReset);
		Write(registerConfig, configAwake);
	}

protected:
	//Hardware
	volatile uint8* hardwarePort;
	volatile uint8* hardwareDDR;
	volatile uint8* hardwarePin;
	uint8 pinSDA;
	uint8 pinSCK;
	bool newImage;
	//Registers and Settings
	uint8 registerConfig;
	uint8 registerPixelData;
	uint8 registerSqual;
	uint8 registerDeltaX;
	uint8 registerDeltaY;
	uint8 configReset;
	uint8 configAwake;
  //Information
	float positionX;
	float positionY;

	void WriteByte(uint8 newByte)
	{
		*hardwareDDR |= pinSDA;//Set SDA output
	
		for(uint8 i = 0; i < 8; i++)
		{
			*hardwarePort &= ~pinSCK;//prepare SCK
		
			//write data
			*hardwarePort = (*hardwarePort & (~(*hardwarePin))) | 
				((newByte >> 7) * pinSDA);

			newByte = newByte << 1;//prepare next byte
			asm volatile("nop");
		
			*hardwarePort |= pinSCK;
		}
	}

	void Write(int8 adr, uint8 data)
	{	
		WriteByte(adr | 0x80);
		WriteByte(data);
		usleep(100);
	}

	uint8 ReadByte()
	{
		uint8 data=0;

		*hardwareDDR  &= ~pinSDA;//Set SDA input

		for(uint8 i = 0; i < 8; i++)
		{
			*hardwarePort &= ~pinSCK;//Prepare data
			data = data << 1;

			asm volatile("nop");
			*hardwarePort |= pinSCK;//Prepare for reading

			data |= (*hardwarePin & pinSDA) / pinSDA;
		}

		return data;
	}

	uint8 Read(uint8 adr)
	{
		WriteByte(adr);
		usleep(100);
	
		return ReadByte();
	}

	void ImagePrepare()
	{
		Write(registerConfig, configAwake);
		Write(registerPixelData, 0x00);

		newImage = true;
	}

	uint8 ImageRead()
	{
		uint8 pixel = Read(registerPixelData);
		if(newImage)
		{
			while (!(pixel & 0x80))//0x80 indicates first pixel
			{
				pixel=Read(registerPixelData);
			}
			newImage = false;
		}
	
		return pixel;
	}


public:	
	uint8 GetSqual()
	{
		return Read(registerSqual);
	}

	int8 GetXMovement()
	{
		return (int8)(Read(registerDeltaX));
	}

	int8 GetYMovement()
	{
		return (int8)(Read(registerDeltaY));
	}

	float GetPositionX()
	{
		return positionX;
	}

	float GetPositionY()
	{
		return positionY;
	}
};

#endif