Creating new library for axis interpolation

Hi,

I am creating a new library for axis interpolation (up to 3 axis and rotary movement too) which can be executed line by line in the compiled program, difference here is that I don’t want to use/interpret G-code, think of it as an Accelstepper with interpolation.

Well, I started today, and the first step is straight line interpolation between two axis.
That’s the first code I made, still unfinished and without direct port access. Now I’m more focused on creating a mathematic efficient and accurate movement:

Line(x,y,1000,-999,relative); //Declaration: type_of_movement(axis1,axis2,distance_axis1,distance_axis2,relative_or_absolute_movement);


a = 1000; //distance_axis1
b = -999; //distance_axis2
speed = 10000; //Depends on another class not showed here
//stop = max(a2,b2);

//Relative or absolute movement
if(relative){
	a1 = actualPositionX; //Where we are
	b1 = actualPositionY;
	a2 = abs(a); //Where we want to be (in positive)
	b2 = abs(b);
}
else{
	a1 = actualPositionX;
	b1 = actualPositionY;
	a2 = abs(a);
	b2 = abs(b);
}

//Set the direction of the movement
if(a > 0){
  	digitalWrite(directionA,HIGH);
  	incrementA = 1; //To increase or decrease position based on direction
}
else{
	digitalWrite(directionA,LOW);
	incrementA = -1;

}
if(b > 0){
  	digitalWrite(directionB,HIGH);
  	incrementB = 1;
}
else{
	digitalWrite(directionB,LOW);
  	incrementB = -1;

}

//Calculate and execute movement
}
if(a2>b2){
	diference = a2/b2;
	//stop = a2;
	for(i=1,i<=diference,i++){
		digitalWrite(x,HIGH);
		delayMicroseconds(speed); //Where to put the speed limitation? Here can be useful because also keeps pin HIGH for a while
		digitalWrite(x,LOW);
		actualPositionX = actualPosition + incrementA;
		a2--;
	}
	digitalWrite(y,HIGH);
	delayMicroseconds(1); //It is not needed with digitalWrite, but it will with direct port address
	digitalWrite(y,LOW);
	b2--;
}

else if(a2<b2){
	//TO IMPLEMENT WHEN a2>b2 is finished but inversed

}
else{
	for(i=1,i<=a2,i++){
		digitalWrite(x,HIGH);
		digitalWrite(y,HIGH);
		delayMicroseconds(speed);
		digitalWrite(x,LOW);
		digitalWrite(y,LOW);
		a2--;
		b2--;
	}
	//actualPositionX = actualPositionX + a2; //Will this increment the position to the right value when all the movements in al the if statements are finished???
	//actualPositionY = actualPositionY + b2; //Will this increment the position to the right value when all the movements in al the if statements are finished???

}

Any suggestions?