// MOTION Class
// This defines a bresenham line drawing algorithm in 3D or 2D
struct MOTION
{
	void init(word, word, word, word);
	void init(word, word, word, word, word, word);
	void moveonesquare(void);
	bool atdestination(void);
	bool nextdiagonal(void);
	word difference(short, short);
	short curx;
	short cury;
	short curz;
	short destx;
	short desty;
	short destz;
	char alwaysx;
	char alwaysy;
	char alwaysz;
	char sometimesx;
	char sometimesy;
	char sometimesz;
	char rarelyx;
	char rarelyy;
	char rarelyz;
	short bigdiff;
	short meddiff;
	short smldiff;
	short medbig_counter;
	short smlmed_counter;
};

// Initialise a 2D motion from x1,y1 to x2,y2
void MOTION::init(word x1, word y1, word x2, word y2)
{
	word diffx=difference(x1, x2);
	word diffy=difference(y1, y2);
	curx=x1;
	cury=y1;
	curz=0;
	destx=x2;
	desty=y2;
	destz=0;
	alwaysz=0;
	sometimesx=0;
	sometimesy=0;
	sometimesz=0;
	if (x2==x1)
		alwaysx=0;
	else
		if (x2>x1)
			alwaysx=1;
		else
			alwaysx=-1;
	if (y2==y1)
		alwaysy=0;
	else
		if (y2>y1)
			alwaysy=1;
		else
			alwaysy=-1;
	if (diffx>diffy)
	{
		bigdiff=diffx;
		meddiff=diffy;
		sometimesy=alwaysy;
		alwaysy=0;
	}
	else
	{
		bigdiff=diffy;
		meddiff=diffx;
		sometimesx=alwaysx;
		alwaysx=0;
	}
	medbig_counter=bigdiff;
	smlmed_counter=1;
	smldiff=0;
}

// Initialise a 3D motion from x1,y1,z1 to x2,y2,z2
void MOTION::init(word x1, word y1, word z1, word x2, word y2, word z2)
{
	word diffx=difference(x1, x2);
	word diffy=difference(y1, y2);
	word diffz=difference(z1, z2);
	curx=x1;
	cury=y1;
	curz=z1;
	destx=x2;
	desty=y2;
	destz=z2;
	sometimesx=0;
	sometimesy=0;
	sometimesz=0;
	rarelyx=0;
	rarelyy=0;
	rarelyz=0;
	if (x2==x1)
		alwaysx=0;
	else
		if (x2>x1)
			alwaysx=1;
		else
			alwaysx=-1;
	if (y2==y1)
		alwaysy=0;
	else
		if (y2>y1)
			alwaysy=1;
		else
			alwaysy=-1;
	if (z2==z1)
		alwaysz=0;
	else
		if (z2>z1)
			alwaysz=1;
		else
			alwaysz=-1;
	if (diffx>diffy)
	{
		if (diffz>diffx)
		{
			bigdiff=diffz;
			meddiff=diffx;
			smldiff=diffy;
			sometimesx=alwaysx;
			alwaysx=0;
			rarelyy=alwaysy;
			alwaysy=0;
		}
		else
			if (diffz>diffy)
			{
				bigdiff=diffx;
				meddiff=diffz;
				smldiff=diffy;
				sometimesz=alwaysz;
				alwaysz=0;
				rarelyy=alwaysy;
				alwaysy=0;
			}
			else
			{
				bigdiff=diffx;
				meddiff=diffy;
				smldiff=diffz;
				sometimesy=alwaysy;
				alwaysy=0;
				rarelyz=alwaysz;
				alwaysz=0;
			}
	}
	else
	{
		if (diffz>diffy)
		{
			bigdiff=diffz;
			meddiff=diffy;
			smldiff=diffx;
			sometimesy=alwaysy;
			alwaysy=0;
			rarelyx=alwaysx;
			alwaysx=0;
		}
		else
			if (diffz>diffx)
			{
				bigdiff=diffy;
				meddiff=diffz;
				smldiff=diffx;
				sometimesz=alwaysz;
				alwaysz=0;
				rarelyx=alwaysx;
				alwaysx=0;
			}
			else
			{
				bigdiff=diffy;
				meddiff=diffx;
				smldiff=diffz;
				sometimesx=alwaysx;
				alwaysx=0;
				rarelyz=alwaysz;
				alwaysz=0;
			}
	}
	medbig_counter=bigdiff;
	smlmed_counter=meddiff;
}

// Move one square towards the destination
void MOTION::moveonesquare(void)
{
	curx+=alwaysx;
	cury+=alwaysy;
	curz+=alwaysz;
	medbig_counter-=meddiff;
	if (medbig_counter<=0)
	{
		curx+=sometimesx;
		cury+=sometimesy;
		curz+=sometimesz;
		medbig_counter+=bigdiff;
		smlmed_counter-=smldiff;
		if (smlmed_counter<=0)
		{
			curx+=rarelyx;
			cury+=rarelyy;
			curz+=rarelyz;
			smlmed_counter+=meddiff;
		}
	}
}

// Check for the end of a motion
bool MOTION::atdestination(void)
{
	if ((curx==destx) && (cury==desty) && (curz==destz))
		return (true);
	else
		return (false);
}

// Check if the next step is diagonal and return true if so
bool MOTION::nextdiagonal(void)
{
	if (medbig_counter-meddiff<=0)
		return (true);
	else
		return (false);
}

// Return the +ve difference between two numbers
word MOTION::difference(short a, short b)
{
	if (a>b)
		return (a-b);
	else
		return (b-a);
}