/* pyr.c - implementation of Pitch-Yaw-Roll
 * (c)opyright Benjamin 'blindcoder' Schieder <blindcoder@scavenger.homeip.net>
 * Copyright (C) 2006 Benjamin Schieder
 * 
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; version 2 of the License.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
#include <stdio.h>
#include <unistd.h>
#include <math.h>

#define deg2rad M_PI/180
#define rad2deg 180/M_PI

typedef struct Vector {
	float x;
	float y;
	float z;
} Vector;

Vector Cross(Vector v1, Vector v2){
	/* Using the Cross product of two vectors you get the third one to
	 * create a carthesian coordinate system
	 * Cross(right, up) = forward	Cross(up, right) = backward
	 * Cross(forward, right) = up	Cross(right, forward) = down
	 * Cross(up, forward) = right	Cross(forward, up) = left
	 */
	Vector r;

	r.x = ((v1.y * v2.z) - (v1.z * v2.y));
	r.y = ((v1.z * v2.x) - (v1.x * v2.z)); 
	r.z = ((v1.x * v2.y) - (v1.y * v2.x));

	return r;
}

void UpDown(Vector *forward, Vector *up, float degree){
	/* here we move the ships nose up or down depending on the value of degree.
	 * The only two vectors we change here are forward and up
	 */
	Vector nforward, nup, right, nright;
	float radian;
	float c, s;

	radian=degree*deg2rad;
	c=cos(radian);
	s=sin(radian);

	//right = Cross(*up, *forward);

	nforward.x = forward->x*c-up->x*s;
	nforward.y = forward->y*c-up->y*s;
	nforward.z = forward->z*c-up->z*s;

	nup.x = forward->x*s+up->x*c;
	nup.y = forward->y*s+up->y*c;
	nup.z = forward->z*s+up->z*c;

	//nright = Cross(*up, *forward);

	up->x=nup.x;
	up->y=nup.y;
	up->z=nup.z;
	forward->x=nforward.x;
	forward->y=nforward.y;
	forward->z=nforward.z;
	//right=nright;
}

void LeftRight(Vector *forward, Vector *up, float degree){
	/* here we move the ships nose left or right depending on the value of degree.
	 * The only two vectors we change here are forward and right (and so, only
	 * forward, because we calculate right from forward and up vectors)
	 */
	Vector nforward, nup, right, nright;
	float radian;
	float c, s;

	radian=degree*deg2rad;
	c=cos(radian);
	s=sin(radian);

	right = Cross(*up, *forward);

	nforward.x = forward->x*c-right.x*s;
	nforward.y = forward->y*c-right.y*s;
	nforward.z = forward->z*c-right.z*s;

	nright.x = forward->x*s+right.x*c;
	nright.y = forward->y*s+right.y*c;
	nright.z = forward->z*s+right.z*c;

	right=nright;

	//nup=Cross(*forward, right);

	forward->x=nforward.x;
	forward->y=nforward.y;
	forward->z=nforward.z;
	//up->x=nup.x;
	//up->y=nup.y;
	//up->z=nup.z;
}

void CwCCw(Vector *forward, Vector *up, float degree){
	/* here we rotate the ship clockwise or counter-clockwise.
	 * The only two vectors we change here are up and right (and so, only
	 * up, because we calculate right from forward and up vectors)
	 */
	Vector nforward, nup, right, nright;
	float radian;
	float c, s;

	radian=degree*deg2rad;
	c=cos(radian);
	s=sin(radian);

	right = Cross(*up, *forward);

	nright.x = right.x*c-up->x*s;
	nright.y = right.y*c-up->y*s;
	nright.z = right.z*c-up->z*s;

	nup.x = right.x*s+up->x*c;
	nup.y = right.y*s+up->y*c;
	nup.z = right.z*s+up->z*c;

	//nforward=Cross(right, *up);

	right=nright;
	up->x=nup.x;
	up->y=nup.y;
	up->z=nup.z;
	//forward->x=nforward.x;
	//forward->y=nforward.y;
	//forward->z=nforward.z;
}

int main(){
	Vector up, forward, right, nforward, nup, nright;
	char c;
	int print;

	forward.x=0; forward.y=0; forward.z=1; // our forward vector. Where the ships nose points to.
	up.x=0; up.y=1; up.z=0; // The up vector. What we see when we turn our head up.

	printf("Current forward: x %f y %f z %f\n", forward.x, forward.y, forward.z);
	printf("Current up: x %f y %f z %f\n", up.x, up.y, up.z);
	right = Cross(up, forward);
	printf("Current right: x %f y %f z %f\n", right.x, right.y, right.z);

	while (read(0, &c, 1)){
		print = 1;
		switch (c){
			case 'i': UpDown(&forward, &up, 1); break; // move nose down
			case 'k': UpDown(&forward, &up,-1); break; // move nose up
			case 'j': LeftRight(&forward, &up, 1); break; // move nose left
			case 'l': LeftRight(&forward, &up,-1); break; // move nose right
			case 'u': CwCCw(&forward, &up, 1); break; // rotate ship clockwise
			case 'o': CwCCw(&forward, &up,-1); break; // rotate ship counter-clockwise
			default: print=0; break;
		}
		if (print){
			printf("Current forward: x %f y %f z %f\n", forward.x, forward.y, forward.z);
			printf("Current up: x %f y %f z %f\n", up.x, up.y, up.z);
			right = Cross(up, forward);
			printf("Current right: x %f y %f z %f\n", right.x, right.y, right.z);
		}
	}
	return 0;
}