#macro disp_arm(arm, arm_parts)
	//Displays an arm given the position array and an array of limb-parts.

	#declare i = dimension_size(arm, 1)-1;
	#declare armout = union{};
	#while(i>=0)
		#declare armout = union {object{arm_parts[i]}  object{armout translate arm[i][2]} rotate arm[i][1]*arm[i][0]} 
		#declare i=i-1;
	#end
	object{armout}
#end

#macro chdistance_arm(arm, limb, limbrotation, goal)
	//This performs the error-function on an arm-position array after a
	// potential movement of one joint --- that is, it only pretends to move
	//the limb, and seens wha thte result would be. It takes an arm position array,
	// the number of the joint moved in that array, the amount moved, and the
	// goal location of the tip of the arm.

	#declare i = dimension_size(arm, 1)-1;
	#declare endloc = <0, 0, 0>;
	#declare armerr = 0;
	#declare endloc_sum =0;
	#while(i>=0)
		#if(i=limb)
			#declare endloc =  vrotate(endloc + arm[i][2], (arm[i][1]+limbrotation)*arm[i][0]);
			#if(i != dimension_size(arm, 1)-1)
				#declare endloc_sum = endloc_sum + endloc.y;
			#end
			#if((arm[i][1].x+limbrotation)<arm[i][3].x & arm[i][3].y=1)
				#declare armerr = armerr + abs(abs(arm[i][1].x+limbrotation)-abs(arm[i][3].x))*arm[i][4].x;
			#else 
				#if ((arm[i][1].x+limbrotation)>arm[i][5].x & arm[i][5].y = 1)
					#declare armerr = armerr + abs(abs(arm[i][1].x+limbrotation)-abs(arm[i][5].x))*arm[i][6].x;
				#end
			#end
		#else
			#declare endloc =  vrotate(endloc + arm[i][2], arm[i][1]*arm[i][0]); 
			#if(i != dimension_size(arm, 1)-1)
				#declare endloc_sum = endloc_sum + endloc.y;
			#end
			#if(arm[i][1].x<arm[i][3].x & arm[i][3].y=1)
				#declare armerr = armerr + abs(abs(arm[i][1].x)-abs(arm[i][3].x))*arm[i][4].x;
			#else
				#if (arm[i][1].x>arm[i][5].x & arm[i][5].y = 1)
					#declare armerr = armerr + abs(abs(arm[i][1].x)-abs(arm[i][5].x))*arm[i][6].x;
				#end
			#end
		#end
		#declare i=i-1;
	#end
	vlength(goal-endloc)+armerr*5-endloc_sum*.0
#end



#macro curdistance_arm(arm, goal)
	//This performs the same function without moving a joint.

	#declare i = dimension_size(arm, 1)-1;
	#declare endloc = <0, 0, 0>;
	#declare endloc_sum = 0;
	#declare armerr =0;
	#while(i>=0)
		/*
		#debug "Limb "
		#debug str(i, 0, 0)
		#debug " is rotated "
		#debug str(arm[i][1].x, 0, 2)
		#debug ". The added error is "
		*/
		#declare endloc =  vrotate(endloc + arm[i][2], arm[i][1]*arm[i][0]);
		#if(i != dimension_size(arm, 1)-1)
		#declare endloc_sum = endloc_sum + endloc.y;
		#end
		#if(arm[i][1].x<arm[i][3].x & arm[i][3].y=1)
			//#debug str(abs(abs(arm[i][1].x)-abs(arm[i][3].x))*arm[i][4].x, 0, 2)
			#declare armerr = armerr + abs(abs(arm[i][1].x)-abs(arm[i][3].x))*arm[i][4].x;
		#else
			#if (arm[i][1].x>arm[i][5].x & arm[i][5].y = 1)
				#declare armerr = armerr + abs(abs(arm[i][1].x)-abs(arm[i][5].x))*arm[i][6].x;
				//#debug str(abs(abs(arm[i][1].x)-abs(arm[i][5].x))*arm[i][6].x, 0, 2)
			#end
		#end
		//#debug "\n"
		#declare i=i-1;
	#end
	//#debug "ENDLOC: "
	//#debug vstr(3, endloc, ",", 0, 2)
	vlength(goal-endloc)+armerr*5-endloc_sum*.0
#end

#macro output_arm(arm, name)
//Saves arm position data. Useful for animations.
#fopen out concat("inverse_",name, ".txt") write
#declare i=0;
#while(i<dimension_size(arm, 1))
#declare j=0;
#while(j<dimension_size(arm, 2))
#write(out, arm[i][j], ", ")
#declare j=j+1;
#end
#write(out, "\n")
#declare i=i+1;
#end
#fclose out
#end

#macro input_arm(arm, name)
//Loads arm position data.
#fopen infile concat("inverse_", name, ".txt") read
#declare i=0; 
#while(i<dimension_size(arm, 1))
#read(infile, arm[i][0], arm[i][1], arm[i][2], arm[i][3], arm[i][4], arm[i][5], arm[i][6])#declare i= i+1;
#end
#fclose infile
arm
#end

#macro move_arm(arm, goal, name, in)
//This is the meat.

//#debug vstr(3, goal, ",",0, 2)
#if(in)
#declare arm = input_arm(arm, name)
#end

	#declare dist = curdistance_arm(arm, goal);
	#declare overflow = 0;
	#while(dist>.01 & overflow<100)
		#declare k=0;
		#while(k<dimension_size(arm, 1))
			#declare grada = chdistance_arm(arm, k, 1, goal);
			#declare gradb = chdistance_arm(arm, k, -1, goal);

			/*#debug "\n"
			#debug str(grada, 0, 2)
			#debug "<-- grada | gradb -->"
			#debug str(gradb, 0, 2)
			#debug "\n\n"
			*/

			//#if (grada > dist & gradb > dist)
			//#declare overflow=overflow+1;
			//#else
			#if(dist>1)
			#declare dist = sqrt(dist);
			#end
			#declare grad = (grada-gradb)*.5*dist;
			//#debug str(grad, 0, 2)
			//#debug "\n\n"
			#declare arm[k][1] = arm[k][1] - grad;
			//#end
			#declare k=k+1;
		
		        #declare dist = curdistance_arm(arm, goal);
		#end
		output_arm(arm, name)
		/*
		#debug name
		#debug ": "
		#debug str(dist, 0, 2)
		#debug "\n\n"
		*/
		#declare overflow = overflow + 1;
	#end
#end

#macro endpoint_arm(arm)
	//Finds the endpoint of the arm.
	#declare i = dimension_size(arm, 1)-1;
	#declare endloc = <0, 0, 0>;
	#while(i>=0)
		#declare endloc =  vrotate(endloc + arm[i][2], arm[i][1]*arm[i][0]);
		#declare i=i-1;
	#end
	endloc
#end


#macro make_step(step_a, step_b, step_h, timer)
	//Associated macro fro making a spline.
 #declare step = spline {
   natural_spline
  -0.5, step_a-<0, 0, 0>,
   0.0, step_a,
  0.5,  ((step_b+step_a)/2)*<1, 0, 1>+<0, step_h, 0>,
   1.0, step_b,
   1.5, step_b-<0,  0, 0>
  }
step(timer)
#end