//Prints the elements of a matrix
void Print(Matrix M)
{
	for (int i=0; i < M.RowNo(); i++)
	{
      for (int j=0; j < M.ColNo(); j++)
      {
         printf("%.6f ", M(i,j));

      }
      printf("\n");
	}
}
void WMRA_park2ready(int ini, int vr, int ml, int arm, Matrix Tiwc, Matrix qiwc, Galil &g){
Matrix zero(1,1);
zero.Null(1,1);
	if (ini==3){
		if (arm==1){
			try {
            WMRA_ARM_Motion(ini, 0, zero, 0, g);
			}
			catch (...) {
				cout << "Exception 1 occurred";
			}
		}
		if (vr==1){
			/*try {
				WMRA_VR_Animation(ini, 0, 0);
			}
			catch (...) {
				cout << "Exception 2 occurred";
			}*/
		}
		if (ml==1){
			/*try {
				WMRA_ML_Animation(ini, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
			}
			catch (...) {
				cout << "Exception 3 occurred";
			}*/
		}
		return;
	}

	// Defining the used conditions:
	float qi2 [7]= {0, PI/2, 0, PI, 0, 0, 0}; // Initial joint angles (Parking Position).
	float qd2 [7]= {PI/2, PI/2, 0, PI/2, PI/2, PI/2, 0}; // Final joint angles (Ready Position).
	Matrix qd(7,1), qi(7,1);
	float ts, dt, ddt;
	int n, j;
	for ( j = 0 ; j < 7 ; j++ ) {
		qi(j,0)=qi2[j];
		qd(j,0)=qd2[j];
	}
	ts=10;  // (5 or 10 or 20) Simulation time to move the arm from any position to the ready position.
	n=100;  // Number of time steps.
	dt=ts/n;  // The time step to move the arm from any position to the ready position.
	Matrix dq(1,1);
	dq=qd-qi;
	dq/=(0.5*n+5);  // Joint angle change at every time step.

	FILE * fil;
	fil = fopen("Park2ready.txt","w");
	fprintf(fil," qi is: \n\n");
	int k;
	for (j=0;j<qi.RowNo();j++){
		for (k=0;k<qi.ColNo();k++){
			fprintf(fil," %4.2f ",  qi(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");
	fprintf(fil," qd is: \n\n");
	for (j=0;j<qd.RowNo();j++){
		for (k=0;k<qd.ColNo();k++){
			fprintf(fil," %4.2f ",  qd(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");
	fprintf(fil," ts is %4.2f ",  ts);
	fprintf(fil," \n\n\n");
	fprintf(fil," n is %d ",  n);
	fprintf(fil," \n\n\n");
	fprintf(fil," dt is %4.2f ",  dt);
	fprintf(fil," \n\n\n");
	fprintf(fil," dq is: \n\n");
	for (j=0;j<dq.RowNo();j++){
		for (k=0;k<dq.ColNo();k++){
			fprintf(fil," %4.2f ",  dq(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");

	// Initializing the physical Arm:
	if (arm==1){
		zero.Null(10,1);
		for ( j = 0 ; j < 7 ; j++ ) {
			zero(j,0)=qi(j,0);
		}
		zero(7,0)=qiwc(0,0);
		zero(8,0)=qiwc(1,0);
		WMRA_ARM_Motion(ini, 2, zero, dt, g);
		ddt=0;
	}

	fprintf(fil," qi is: \n\n");
	for (j=0;j<qi.RowNo();j++){
		for (k=0;k<qi.ColNo();k++){
			fprintf(fil," %4.2f ",  qi(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");
	fprintf(fil," qiwc is: \n\n");
	for (j=0;j<qiwc.RowNo();j++){
		for (k=0;k<qiwc.ColNo();k++){
			fprintf(fil," %4.2f ",  qiwc(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");

	// Initializing Virtual Reality Animation:
	if (vr==1){
		/*zeros.Null(9,1);
		for ( j = 0 ; j < 7 ; j++ ) {
			zeros(j,0)=qi(j,0);
		}
		zeros(7,0)=qiwc(0,0);
		zeros(8,0)=qiwc(1,0);*/
	//	WMRA_VR_Animation(ini, Tiwc, qi);
	}

	// Initializing Robot Animation in Matlab Graphics:
	Matrix DH(1,1);
	Matrix T01(4,4), T12(4,4), T23(4,4), T34(4,4), T45(4,4), T56(4,4), T67(4,4);
	Matrix Ti(4,4), Td(4,4);
		
	if (ml==1){
		// Inputting the D-H Parameters in a Matrix form:
		DH=WMRA_DH(qi);
		
		// Calculating the transformation matrices of each link:		
		T01 = WMRA_rotx(DH(0,0))*WMRA_transl(DH(0,1),0,0)*WMRA_rotz(DH(0,3))*WMRA_transl(0,0,DH(0,2));
		//T2
		T12 = WMRA_rotx(DH(1,0))*WMRA_transl(DH(1,1),0,0)*WMRA_rotz(DH(1,3))*WMRA_transl(0,0,DH(1,2));
		//T3
		T23 = WMRA_rotx(DH(2,0))*WMRA_transl(DH(2,1),0,0)*WMRA_rotz(DH(2,3))*WMRA_transl(0,0,DH(2,2));
		//T4
		T34 = WMRA_rotx(DH(3,0))*WMRA_transl(DH(3,1),0,0)*WMRA_rotz(DH(3,3))*WMRA_transl(0,0,DH(3,2));
		//T5
		T45 = WMRA_rotx(DH(4,0))*WMRA_transl(DH(4,1),0,0)*WMRA_rotz(DH(4,3))*WMRA_transl(0,0,DH(4,2));
		//T6
		T56 = WMRA_rotx(DH(5,0))*WMRA_transl(DH(5,1),0,0)*WMRA_rotz(DH(5,3))*WMRA_transl(0,0,DH(5,2));
		//T7
		T67 = WMRA_rotx(DH(6,0))*WMRA_transl(DH(6,1),0,0)*WMRA_rotz(DH(6,3))*WMRA_transl(0,0,DH(6,2));
		// Calculating the Transformation Matrix of the initial and desired arm positions:
		Ti=Tiwc*T01*T12*T23*T34*T45*T56*T67;
		Td=Tiwc*WMRA_q2T(qd);
		//WMRA_ML_Animation(ini, Ti, Td, Tiwc, T01, T12, T23, T34, T45, T56, T67);
	}

	// Initialization:
	Matrix qo(1,1);
	float tt;
	qo=qi;
	tt=0;

	fprintf(fil," qo is: \n\n");
	for (j=0;j<qo.RowNo();j++){
		for (k=0;k<qo.ColNo();k++){
			fprintf(fil," %4.2f ",  qo(j,k));
		}
		fprintf(fil," \n");
	}
	fprintf(fil," \n\n\n");
	fprintf(fil," tt is %4.2f ",  tt);
	fprintf(fil," \n\n\n");
	
	Matrix qn(1,1);
	clock_t startt, endt, endf;
	double timedif; 
	Matrix T1a(1,1), T2a(1,1), T3a(1,1), T4a(1,1), T5a(1,1), T6a(1,1), T7a(1,1);
	
	qn.Null(7,1);
	while (tt <= ts) {
		// Starting a timer:
		startt=0;
		startt=clock()/ CLOCKS_PER_SEC;
		
		
		// Calculating the new Joint Angles:
		if (tt==0){
			qn=qo;
		}
		else if (tt < (dt*(0.5*n-5))){
			qn(0,0)=qo(0,0)+dq(0,0);
		}
		else if (tt < (dt*(0.5*n+5))){
			qn=qo+dq;
		}
		else if (tt < (dt*(n-1))){
			for (j=1;j<7;j++){
				qn(j,0)=qo(j,0)+dq(j,0);
			}
		}
		fprintf(fil," qn is: \n\n");
		for (j=0;j<qn.RowNo();j++){
			for (k=0;k<qn.ColNo();k++){
				fprintf(fil," %4.2f ",  qn(j,k));
			}
			fprintf(fil," \n");
		}
		fprintf(fil," \n\n\n");
		fprintf(fil," ddt is %4.2f ",  ddt);
		fprintf(fil," \n\n\n");
		// Updating the physical Arm:
		if (arm==1) {
			float ddt2=0;
			ddt2=ddt+dt;
			if (ddt2>=0.5 || tt>=ts){
				zero.Null(10,1);
				for ( j = 0 ; j < 7 ; j++ ) {
					zero(j,0)=qn(j,0);
				}
				zero(7,0)=qiwc(0,0);
				zero(8,0)=qiwc(1,0);
				WMRA_ARM_Motion(2, 1, zero, ddt2, g);
				ddt2=0;
			}
			ddt=0;
			ddt=ddt2;
		}
		
		// Updating Virtual Reality Animation:
		if (vr==1){
			/*zero.Null(9,1);
			for ( j = 0 ; j < 7 ; j++ ) {
				zero(j,0)=qn(j,0);
			}
			zero(7,0)=qiwc(0,0);
			zero(8,0)=qiwc(1,0);*/
			//WMRA_VR_Animation(2, Tiwc, zero);
		}
		
		// Updating Matlab Animation:
		if (ml==1){
			// Calculating the new Transformation Matrix:
			T1a = WMRA_rotx(DH(0,0))*WMRA_transl(DH(0,1),0,0)*WMRA_rotz(qn(0,0))*WMRA_transl(0,0,DH(0,2));
		//T2
		T2a = WMRA_rotx(DH(1,0))*WMRA_transl(DH(1,1),0,0)*WMRA_rotz(qn(1,0))*WMRA_transl(0,0,DH(1,2));
		//T3
		T3a = WMRA_rotx(DH(2,0))*WMRA_transl(DH(2,1),0,0)*WMRA_rotz(qn(2,0))*WMRA_transl(0,0,DH(2,2));
		//T4
		T4a = WMRA_rotx(DH(3,0))*WMRA_transl(DH(3,1),0,0)*WMRA_rotz(qn(3,0))*WMRA_transl(0,0,DH(3,2));
		//T5
		T5a = WMRA_rotx(DH(4,0))*WMRA_transl(DH(4,1),0,0)*WMRA_rotz(qn(4,0))*WMRA_transl(0,0,DH(4,2));
		//T6
		T6a = WMRA_rotx(DH(5,0))*WMRA_transl(DH(5,1),0,0)*WMRA_rotz(qn(5,0))*WMRA_transl(0,0,DH(5,2));
		//T7
		T7a = WMRA_rotx(DH(6,0))*WMRA_transl(DH(6,1),0,0)*WMRA_rotz(qn(6,0))*WMRA_transl(0,0,DH(6,2));
		
		//WMRA_ML_Animation(2, Ti, Td, Tiwc, T1a, T2a, T3a, T4a, T5a, T6a, T7a);
		}

		// Updating the old values with the new values for the next iteration:
		
		qo.Null(7,1);
		qo=qn;
		tt=tt+dt;

		fprintf(fil," qo is: \n\n");
		for (j=0;j<qo.RowNo();j++){
			for (k=0;k<qo.ColNo();k++){
				fprintf(fil," %4.2f ",  qo(j,k));
			}
			fprintf(fil," \n");
		}
		fprintf(fil," \n\n\n");
		fprintf(fil," tt is %4.2f ",  tt);
		fprintf(fil," \n\n\n");
		
		// Pausing for the speed sync:
		endt=0;
		endt=clock()/ CLOCKS_PER_SEC;
		timedif=0;
		timedif=endt-startt;
		wait((dt-timedif));
		
	}
	fclose(fil);
}
void WMRA_any2ready(int ini, int vr, int ml, int arm, Matrix Tiwc, Matrix qi, Galil &g){
Matrix zero(1,1);
zero.Null(1,1);
	if (ini==3){
		if (arm==1){
			try {
            WMRA_ARM_Motion(ini, 0, zero, 0, g);
			cout<< "any ini "<<endl;
			}
			catch (...) {
				cout << "Exception 1 occurred";
			}
		}
		if (vr==1){
			/*try {
				WMRA_VR_Animation(ini, 0, 0);
			}
			catch (...) {
				cout << "Exception 2 occurred";
			}*/
		}
		if (ml==1){
			/*try {
				WMRA_ML_Animation(ini, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
			}
			catch (...) {
				cout << "Exception 3 occurred";
			}*/
		}
		return;
	}

	// Defining the used conditions:
	Matrix qd(7,1);
	float ts, dt, ddt;
	int n, j;
	//float qd2 [7]= {90, 90, 0, 90, 90, 90, 0}; // Final joint angles (Ready Position) //#1
	//float qd2 [7]= {90, 90, -90, 180, 0, 60, 0}; // Final joint angles (Ready Position) #3
	//float qd2 [7]= {90, 90, -90, 180, 0, 60, 0}; //#4
	float qd2 [7]= {90, 90, 0, 90, 90, 60, 0}; // (Ready Position) //#2
	//float qd2 [7]= {90, 90, 0, 90, 120, 60, 0}; // (Ready Position) //#5
	//float qd2 [7]= {90, 90, 0, 90, 60, 60, 0}; // (Ready Position) //#6
	//float qd2 [7]= {90, 60, 0, 90, 60, 60, 0};// ready position joint angles #7
	//float qd2 [7]= {90, 90, 30, 90, 90, 60, 0};// ready position joint angles #8

	for ( j = 0 ; j < 7 ; j++ ) {
		qd(j,0)=qd2[j]*d2r;
	}
	ts=10;  // (5 or 10 or 20) Simulation time to move the arm from any position to the ready position.
	n=100;  // Number of time steps.
	dt=ts/n;  // The time step to move the arm from any position to the ready position.

	FILE * fel;
	fel = fopen("Any2ready.txt","w");
	fprintf(fel," qd is: \n\n");
	int k;
	for (j=0;j<qd.RowNo();j++){
		for (k=0;k<qd.ColNo();k++){
			fprintf(fel," %4.2f ",  qd(j,k));
		}
		fprintf(fel," \n");
	}
	fprintf(fel," \n\n\n");
	fprintf(fel," ts is %4.2f ",  ts);
	fprintf(fel," \n\n\n");
	fprintf(fel," n is %d ",  n);
	fprintf(fel," \n\n\n");
	fprintf(fel," dt is %4.2f ",  dt);
	fprintf(fel," \n\n\n");
	
	// Initializing the physical Arm:
	if (arm==1){
		zero.Null(10,1);
		for ( j = 0 ; j < 9 ; j++ ) {
			zero(j,0)=qi(j,0);
		}
		cout<<"0";
		WMRA_ARM_Motion(ini, 2, zero, dt, g);
		ddt=0;
		cout<<"1"<<endl;		
	}
	fprintf(fel," qi is: \n\n");
	for (j=0;j<qi.RowNo();j++){
		for (k=0;k<qi.ColNo();k++){
			fprintf(fel," %4.2f ",  qi(j,k));
		}
		fprintf(fel," \n");
	}	
	fprintf(fel," \n\n\n");

	// Initializing Virtual Reality Animation:
	if (vr==1){
	//	WMRA_VR_Animation(ini, Tiwc, qi);
	}

	// Initializing Robot Animation in Matlab Graphics:
	Matrix DH(1,1);
	Matrix T01(4,4), T12(4,4), T23(4,4), T34(4,4), T45(4,4), T56(4,4), T67(4,4);
	Matrix Ti(4,4), Td(4,4);
		
	if (ml==1){
		// Inputting the D-H Parameters in a Matrix form:
		DH=WMRA_DH(qi);
		
		// Calculating the transformation matrices of each link:		
		T01 = WMRA_rotx(DH(0,0))*WMRA_transl(DH(0,1),0,0)*WMRA_rotz(DH(0,3))*WMRA_transl(0,0,DH(0,2));
		//T2
		T12 = WMRA_rotx(DH(1,0))*WMRA_transl(DH(1,1),0,0)*WMRA_rotz(DH(1,3))*WMRA_transl(0,0,DH(1,2));
		//T3
		T23 = WMRA_rotx(DH(2,0))*WMRA_transl(DH(2,1),0,0)*WMRA_rotz(DH(2,3))*WMRA_transl(0,0,DH(2,2));
		//T4
		T34 = WMRA_rotx(DH(3,0))*WMRA_transl(DH(3,1),0,0)*WMRA_rotz(DH(3,3))*WMRA_transl(0,0,DH(3,2));
		//T5
		T45 = WMRA_rotx(DH(4,0))*WMRA_transl(DH(4,1),0,0)*WMRA_rotz(DH(4,3))*WMRA_transl(0,0,DH(4,2));
		//T6
		T56 = WMRA_rotx(DH(5,0))*WMRA_transl(DH(5,1),0,0)*WMRA_rotz(DH(5,3))*WMRA_transl(0,0,DH(5,2));
		//T7
		T67 = WMRA_rotx(DH(6,0))*WMRA_transl(DH(6,1),0,0)*WMRA_rotz(DH(6,3))*WMRA_transl(0,0,DH(6,2));
		// Calculating the Transformation Matrix of the initial and desired arm positions:
		Ti=Tiwc*T01*T12*T23*T34*T45*T56*T67;
		Td=Tiwc*WMRA_q2T(qd);
		//WMRA_ML_Animation(ini, Ti, Td, Tiwc, T01, T12, T23, T34, T45, T56, T67);
	}

	// Check for the shortest route:
	Matrix diff(7,1);
	for (j=0;j<7;j++){
		diff(j,0)=qd(j,0)-qi(j,0);
	}
	for (j=0;j<7;j++){
		if (diff(j,0) > PI) {
			diff(j,0)=diff(j,0)-2*PI;
		}
		else if (diff(j,0) < (-PI)) {
			diff(j,0)=diff(j,0)+2*PI;
		}
	}

	fprintf(fel," diff is: \n\n");
	for (j=0;j<diff.RowNo();j++){
		for (k=0;k<diff.ColNo();k++){
			fprintf(fel," %4.2f ",  diff(j,k));
		}
		fprintf(fel," \n");
	}
	fprintf(fel," \n\n\n");

	// Joint angle change at every time step.
	diff/=(n);
	Matrix dq(1,1);
	dq.Null(9,1);
	for ( j = 0 ; j < 7 ; j++ ) {
		dq(j,0)=diff(j,0);
	}

	fprintf(fel," dq is: \n\n");
	for (j=0;j<dq.RowNo();j++){
		for (k=0;k<dq.ColNo();k++){
			fprintf(fel," %4.2f ",  dq(j,k));
		}
		fprintf(fel," \n");
	}
	fprintf(fel," \n\n\n");
	
	// Initialization:
	Matrix qo(1,1);
	float tt;
	qo=qi;
	tt=0;
	fprintf(fel," qo is: \n\n");
	for (j=0;j<qo.RowNo();j++){
		for (k=0;k<qo.ColNo();k++){
			fprintf(fel," %4.2f ",  qo(j,k));
		}
		fprintf(fel," \n");
	}
	fprintf(fel," \n\n\n");
	fprintf(fel," tt is %4.2f ",  tt);
	fprintf(fel," \n\n\n");
	
	Matrix qn(1,1);
	clock_t startt, endt, endf;
	double timedif; 
	Matrix T1a(1,1), T2a(1,1), T3a(1,1), T4a(1,1), T5a(1,1), T6a(1,1), T7a(1,1);

	while (tt <= (ts-dt)) {
		// Starting a timer:
		startt=0;
		startt=clock()/ CLOCKS_PER_SEC;
			
		// Calculating the new Joint Angles:
		qn.Null(9,1);
		qn=qo+dq;

		fprintf(fel," qn is: \n\n");
		for (j=0;j<qn.RowNo();j++){
			for (k=0;k<qn.ColNo();k++){
				fprintf(fel," %4.2f ",  qn(j,k));
			}
			fprintf(fel," \n");
		}
		fprintf(fel," \n\n\n");		
		fprintf(fel," ddt is %4.2f ",  ddt);
		fprintf(fel," \n\n\n");
		
		// Updating the physical Arm:
		if (arm==1) {
			float ddt2=0;
			ddt2=ddt+dt;
			if (ddt2>=0.5 || tt>=(ts-dt)){
				zero.Null(10,1);
				for ( j = 0 ; j < 9 ; j++ ) {
					zero(j,0)=qn(j,0);
				}
				cout<<"2";
				WMRA_ARM_Motion(2, 1, zero, ddt2, g);
				cout<<"3";
				ddt2=0;
			}
			ddt=0;
			ddt=ddt2;
		}
		
		// Updating Virtual Reality Animation:
		if (vr==1){
			//WMRA_VR_Animation(2, Tiwc, qn);
		}
		
		// Updating Matlab Animation:
		if (ml==1){
			// Calculating the new Transformation Matrix:
			T1a = WMRA_rotx(DH(0,0))*WMRA_transl(DH(0,1),0,0)*WMRA_rotz(qn(0,0))*WMRA_transl(0,0,DH(0,2));
		//T2
		T2a = WMRA_rotx(DH(1,0))*WMRA_transl(DH(1,1),0,0)*WMRA_rotz(qn(1,0))*WMRA_transl(0,0,DH(1,2));
		//T3
		T3a = WMRA_rotx(DH(2,0))*WMRA_transl(DH(2,1),0,0)*WMRA_rotz(qn(2,0))*WMRA_transl(0,0,DH(2,2));
		//T4
		T4a = WMRA_rotx(DH(3,0))*WMRA_transl(DH(3,1),0,0)*WMRA_rotz(qn(3,0))*WMRA_transl(0,0,DH(3,2));
		//T5
		T5a = WMRA_rotx(DH(4,0))*WMRA_transl(DH(4,1),0,0)*WMRA_rotz(qn(4,0))*WMRA_transl(0,0,DH(4,2));
		//T6
		T6a = WMRA_rotx(DH(5,0))*WMRA_transl(DH(5,1),0,0)*WMRA_rotz(qn(5,0))*WMRA_transl(0,0,DH(5,2));
		//T7
		T7a = WMRA_rotx(DH(6,0))*WMRA_transl(DH(6,1),0,0)*WMRA_rotz(qn(6,0))*WMRA_transl(0,0,DH(6,2));
		
		//WMRA_ML_Animation(2, Ti, Td, Tiwc, T1a, T2a, T3a, T4a, T5a, T6a, T7a);
		}

		// Updating the old values with the new values for the next iteration:
		
		qo.Null(9,1);
		qo=qn;
		tt=tt+dt;

		fprintf(fel," qo is: \n\n");
		for (j=0;j<qo.RowNo();j++){
			for (k=0;k<qo.ColNo();k++){
				fprintf(fel," %4.2f ",  qo(j,k));
			}
			fprintf(fel," \n");
		}
		fprintf(fel," \n\n\n");
		fprintf(fel," tt is %4.2f ",  tt);
		fprintf(fel," \n\n\n");
		
		// Pausing for the speed sync:
		endt=0;
		endt=clock()/ CLOCKS_PER_SEC;
		timedif=0;
		timedif=endt-startt;
		wait((dt-timedif));
	}
	fclose(fel);
}