/* PARTICLES.C */

#include "glut.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define  PI  3.1415926535
#define  TRUE 1
#define  FALSE 0
#define  EPSILON  0.001
#define  RAND  ((rand()*1.0)/(RAND_MAX))

#define  MAX_PRODUCTIONS  1000

typedef struct xyz_struct {
	float	x,y,z;
} xyz_td;

typedef struct rgb_struct {
	float	r,g,b;
} rgb_td;

typedef struct production_struct {
	char	source;
	char	destination[100];
	float	probability;
} production_td;

production_td  productions[MAX_PRODUCTIONS];
int			count;
char		string[2][10000];
int			steps;
int			in=0,out=1;

void display_analinvkin();

float	theta1,theta2,theta1Rad,theta2Rad;
xyz_td	goal;
float	L1,L2;

// ---------------------------------------------------------------------
// INITIALIZE L-System
// ---------------------------------------------------------------------
void initialize_analinvkin()
{
	theta1 = 0.0;
	theta2 = 0.0;
	L1 = 8.0;
	L2 = 6.0;
	
}

// ---------------------------------------------------------------------
// REINITIALIZE L-System
// ---------------------------------------------------------------------
void reinitialize_lsystem()
{
}

// ---------------------------------------------------------------------
// REINITIALIZE L-System
// ---------------------------------------------------------------------
void set_goal(float x,float y,float z)
{
	float	thetaT,thetaTRad;
	
	goal.x = x;
	goal.y = y;
	goal.z = z;
	
	// analytically calculate what it takes to get to the goal position
	// calculations in radians
	
	thetaTRad = acos(y/sqrt(x*x+y*y));
	
	theta1Rad = acos((L1*L1 + x*x + y*y - L2*L2)/(2*L1*sqrt(x*x+y*y)));
	theta1Rad = theta1Rad + thetaTRad;
	if (x>0) theta1Rad = -theta1Rad;

	theta2Rad = acos((L1*L1 + L2*L2 - x*x - y*y)/(2*L1*L2));
	theta2Rad = PI - theta2Rad;
	if (x<=0) theta2Rad = -theta2Rad;

	// form angles in degrees
	thetaT = thetaTRad*180/PI;
	theta1 = theta1Rad*180/PI;
	theta2 = theta2Rad*180/PI;
	
	// printf("x = %f; y = %f\n",x,y);
	// printf(" theta1 = %f; theta1 = %f; theta2 = %f\n",thetaT,theta1,theta2);
}

// ---------------------------------------------------------------------
// DISPLAY PARTICLES
// ---------------------------------------------------------------------
void display_analinvkin()
{
	// base
	glPushMatrix();
	glTranslatef(0.0,-1.0,0.0);
	draw_cube();
	glPopMatrix();
	
	// first segment
	glPushMatrix();
	glRotatef(theta1,0.0,0.0,1.0);
	glScalef(0.25,L1/2,0.25);
	glTranslatef(0.0,1.0,0.0);
	draw_cube();
	glPopMatrix();
	
	// second segment
	glPushMatrix();
	glTranslatef(L1*sin(-theta1Rad),L1*cos(-theta1Rad),0.0);
	glRotatef(theta1+theta2,0.0,0.0,1.0);
	glScalef(0.25,L2/2,0.25);
	glTranslatef(0.0,1.0,0.0);
	draw_cube();
	glPopMatrix();

	// goal
	glPushMatrix();
	glTranslatef(goal.x,goal.y,goal.z);
	draw_tetrahedron();
	glPopMatrix();

}