// APPENDIX B & EXTRA CREDIT PROJECT, COMPUTATIONAL OPTION
// Source Code
// upgraded to Embarcadero 29 March 2011 by NG

//===========================================================================
//===========================================================================
//                                      PATH FOLLOWING AND COLLISION AVOIDANCE
//                                      v. 0.5.5
//                                      	by Rafael DeAquino Villar
//                                          4/29/2008
//                                 For:     HNRS 150 / Dr. Nardi
//
//                            Based on:         CONSTRAINED VECTOR FLOCKING
//                                                v. 0.9.2
//													by Rafael DeAquino Villar
//													12/1/2007 rev. 12/6/2007
//                                                  HNRS 69 / Dr. Gessler
//
// Description: A simple implementation of pedestrian traffic including basic
// path following and collision avoidance, using "CONSTRAINED VECTOR FLOCKING"
// engine developed for HNRS 69 / ALiCE.
//
//
// This code was built for modularity, and should be broadly applicable. Feel
// free to  modify, tweak, steal, distribute or otherwise make use of it.
//
//===========================================================================
//===========================================================================

//$$---- Form CPP ----
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------

#include <vcl.h>
#include <math.h>
#pragma hdrstop
#include "Unit1.h"
#include "stdlib.h"
#include "time.h"
#include <mmsystem.h>
#include "vector.h"

//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma resource "*.dfm"
TForm1 *Form1;
//---------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner)
	: TForm(Owner)
{
   DoubleBuffered = true;
}

//===========================================================================
//															 GLOBAL CONSTANTS
//===========================================================================

#define LEFT 0
#define RIGHT 1
#define TOP 2
#define BOTTOM 3
#define OFF_VERTICAL 0
#define OFF_HORIZONTAL 1
#define NUMBER_OF_AGENTS 40 // Can/should be defined dynamically by user.
#define AGENT_DIAMETER 10

//===========================================================================
//													DATA STRUCTURES & CLASSES
//===========================================================================

using namespace std;

//-----------------------------------------------------------------------Vect
// Vector structure: used for positions, velocities, forces, etc.
// Basic unit of data manipulation.

typedef struct vect
	{
		float x, y;
	} Vect;

// This approach has several advantages: for one, it almost completely
// eliminates the need for trigonometry and angles. Rather, we can think of
// velocities, forces, positions, accelerations, and so forth, as components
// along the x and y axes.

//----------------------------------------------------------------Coordinator
// Coordinator class: coordinates actions, stores global variables

class coordinator

{
	public:

		coordinator(): increment(1), stop(true), wasStopped(false),
						agentWasJustChosen(false), behaviorRange(30),
						sysEnergy(2000), iterations(0) {}
						//No argument contructor

		~coordinator() {} // Destructor: deallocates memory after use.

		int iterations, chosenAgent, shapeDownX, shapeDownY, behaviorRange,
			sysEnergy;
		float increment, tempFloat, maxMagVel, totalKE;
		bool stop, wasStopped, agentWasJustChosen; // Get rid of temps
		Vect pointA, pointB, ctrOfMass, agentPosition, agentLastPosition,
			ctrPoint, boundsCtr, tlCorner, ctrSpace, posRelCtr;

};

//---------------------------------------------------------------------Mobile
// Agent class: self-contained mobile agent

class mobile
{
	public:
			mobile(): alive(true), startAtPtA(true), energy(0.0), mass(1.0),
			avoidingPOCA(false), avd(NULL) {}
			 // No argument constructor

			~mobile() {}  // Destructor

			bool alive, startAtPtA, avoidingPOCA;
			bool *avd;
			int affinity, laps, age, forceRange;
			float mass, energy, distanceToPOCA, distanceToNNatPOCA;
			Vect position, velocity, acceleration, force, positionLast,
			velocityLast, positionObjective, POCA, losToPOCA;

// Classes used in this way are essentially glorified structures.
// To increase modularitydand data protection, functions should be incorporated
// into the classes, and variables should be made private. I would have done so,
// and still may, but didn't have the time to deal with the associated
// complications.

};

//===========================================================================
//												   GLOBAL OBJECTS & VARIABLES
//===========================================================================

// Should be moved to "On Form Create" ; no time to implement

randomize();                                  // Initializes random number gen
vector<TShape*> shape(NUMBER_OF_AGENTS);	  // Initializes vector of shape pointers
vector<mobile> agent(NUMBER_OF_AGENTS);       // Initializes vector of agents
coordinator daemon;						      // Initializes coordinator

//Above: I used a C++ standard library container "vector". While sharing a similar
//name, this has nothing to do with the "Vect" structure I defined previously--
//it is merely a flexible data container that uses dynamic memory allocation,
//and to which several useful standard library algorithms can be applied.

//Since allocation is dynamic, this allows for runtime definition of the number of
//agents, and deletion based on specified criteria (meets some value, etc)
//one need simply move the initialization to "On Form Create", adjust the rest
//of the code accordingly, and use the STL appropriate algorithms.


//===========================================================================
//													     OVERLOADED OPERATORS
//===========================================================================

//Below are useful operators that I overloaded for use with the Vect data
//structure.

//---------------------------------------------------------------------------
// Vector addition
Vect operator +(const Vect& vector1, const Vect& vector2)
{
	Vect tmpDump;
	tmpDump.x = vector1.x + vector2.x;
	tmpDump.y = vector1.y + vector2.y;
	return tmpDump;
}

Vect resultant(Vect& vectA, Vect& vectB) //temporary alternative to overloaded -
{
	Vect tmpDump;
	tmpDump.x=vectB.x - vectA.x;
	tmpDump.y=vectB.y - vectA.y;
	return tmpDump;
}

//---------------------------------------------------------------------------
// Vector subtraction
Vect operator -(const Vect& vector1, const Vect& vector2)
{
	Vect tmpDump;
	tmpDump.x = vector1.x - vector2.x; //ugly fix to AccsViol
	tmpDump.y = vector1.y - vector2.y;
	return tmpDump;
}

//---------------------------------------------------------------------------
// Scalar Multiplication
Vect operator *(const Vect& vector1, float factor)
{
	Vect tmpDump;
	tmpDump.x = vector1.x * factor;
	tmpDump.y = vector1.y * factor;
	return tmpDump;
}

Vect operator *(float factor, const Vect& vector1)
{
	Vect tmpDump;
	tmpDump.x = vector1.x * factor;
	tmpDump.y = vector1.y * factor;
	return tmpDump;
}


//---------------------------------------------------------------------------
// Dot Product
float operator *(const Vect& vector1, const Vect& vector2)
{
	float temp;
	temp = vector1.x * vector2.x + vector1.y * vector2.y;
	return temp;
}

//---------------------------------------------------------------------------
// Scalar Division
Vect operator /(const Vect& vector1, float divisor)
{
	Vect tmpDump;
	tmpDump.x = vector1.x / divisor;
	tmpDump.y = vector1.y / divisor;
	return tmpDump;
}

//---------------------------------------------------------------------------
// == Operator for Vect
bool operator ==(const Vect& vector1, const Vect& vector2)
{
	if(vector1.x == vector2.x && vector1.y == vector2.y)
		return true;
		else return false;
}

//---------------------------------------------------------------------------
// != Operator for Vect
bool operator !=(const Vect& vector1, const Vect& vector2)
{
	if(vector1.x != vector2.x || vector1.y != vector2.y)
		return true;
		else return false;
}


//===========================================================================
//																	FUNCTIONS
//===========================================================================


//----------------------------------------------------------------Color Ramp
// Color Ramp function: creates color gradient corresponding to a range of
// values

int colorRamp(int range, int value)
	{
	int pixelDistanceAlongPath = (value * 1792) / range;
	int red, green, blue;
	// Which edge of the color cube are we on?
	if (pixelDistanceAlongPath < 256) {
		// Edge 1 from BLACK to BLUE
		red=0; green=0; blue=pixelDistanceAlongPath;
	}
	else if (pixelDistanceAlongPath < 512) {
		// Edge 2 from BLUE to CYAN
		red =0; green=pixelDistanceAlongPath-256; blue=255;
	}
	else if (pixelDistanceAlongPath < 768) {
		// Edge 3 from CYAN to GREEN
		red =0; green =255; blue= 255-(pixelDistanceAlongPath-512);
	}
	else if (pixelDistanceAlongPath < 1024) {
		// Edge 4 from GREEN to YELLOW
		red= (pixelDistanceAlongPath-768); green =255; blue =0;
	}
	else if (pixelDistanceAlongPath <1280) {
		// Edge 5 from YELLOW to RED
		red =255; green=255-(pixelDistanceAlongPath-1024); blue =0;
	}
	else if (pixelDistanceAlongPath < 1536) {
		// Edge 6 from RED to MAGENTA
		red =255; green=0; blue=pixelDistanceAlongPath -1280;
	}
	else {
		// Edge 7 from MAGENTA to WHITE
		red =255; green=pixelDistanceAlongPath-1536; blue =255;
	}
	return (RGB(red, green, blue));
}

//------------------------------------Plus or Minus for Float
float floatPlusOrMinus()		   	 // Generates either a -1.0 or 1.0
{                                    // for various uses.
	bool plus=(bool)random(2);
	float temp = plus ? 1.0 : -1.0;
	return temp;
}

//-------------------------------------------------------------Random Velocity
Vect randVelocity()               // Assigns random velocity vectors to agents
{
	Vect vel;
	vel.x=floatPlusOrMinus()* ((float)random(daemon.sysEnergy))/30000;
	vel.y=floatPlusOrMinus()* ((float)random(daemon.sysEnergy))/30000;
	return vel;
}

//-------------------------------------------------------Center on Coordinates
void spawnOnCoordinate(Vect ctr1, Vect ctr2)     //Sets Up Agent Spawnpoints
{
   for (int i = 0; i < NUMBER_OF_AGENTS; i=i+2)
   {
	  agent[i].position = ctr1;
	  shape[i]->Left = agent[i].position.x - shape[i]->Width / 2;
	  shape[i]->Top = agent[i].position.y - shape[i]->Height / 2;
	  agent[i].positionObjective = ctr2;
   }

   for (int i = 1; i < NUMBER_OF_AGENTS; i=i+2)
   {
	  agent[i].position = ctr2;
	  shape[i]->Left = agent[i].position.x - shape[i]->Width / 2;
	  shape[i]->Top = agent[i].position.y - shape[i]->Height / 2;
	  agent[i].positionObjective = ctr1;
   }
}

// -----------------------------------------------------Bounce Off Boundraries
void bounce(int xy, mobile& anAgent) // Bounces off vertical or horizontal walls
{
	if (xy == OFF_VERTICAL)
		anAgent.velocity.y = anAgent.velocity.y * -1.0;
		else if (xy == OFF_HORIZONTAL)
			anAgent.velocity.x = anAgent.velocity.x * -1.0;
}

// Vector Tools:

//------------------------------------------------------------------Set Vector
 void stV(Vect& vector1, float xCmp, float yCmp)  // Initialzes vect
{
	vector1.x=xCmp;
	vector1.y=yCmp;
}

// ---------------------------------------------------------Set Vector to Null
void nullVect(Vect& vector1)
{
	stV(vector1, 0.0, 0.0);
}

// -----------------------------------------------------------Vector Magnitude
inline float magnitudeOfVect(const Vect& a) // Calculates vector a's magnitude:
{                                     // i.e. distance or speed.
	float temp;
	temp = sqrt(pow(a.x,2) + pow(a.y,2));
	return temp;
}

// ------------------------------------------------------------Get Eigenvector
Vect getEigenVect(const Vect& a) // Calculates directional eigenvector of a
{
	Vect temp;
	float mag = magnitudeOfVect(a);
	if(mag>0)
	 {
		temp = a / (magnitudeOfVect(a));
		return temp;
	 }
	 else
		{
		 nullVect(temp);
		 return temp;
		}

}

// ------------------------------------------------------------Add Eigenvector
void addEigen(Vect& a, const Vect& b)
{
	Vect temp = a;
	a = a + getEigenVect(b);
	a = a * (magnitudeOfVect(temp)/magnitudeOfVect(a));// normalizes magnitude
}

// --------------------------------------------------------Steer Toward Object
inline void steerTwdObj(Vect& a, const Vect& b)
{
	Vect temp = a;
	a = a + .01 * getEigenVect(b);
	a = a * (magnitudeOfVect(temp)/magnitudeOfVect(a));// normalizes magnitude
}

// -----------------------------------------------------------Switch Objective
void switchObj(Vect& oldObj, const Vect& newObj)
{
	oldObj = newObj;
}


// -------------------------------------------------------Subtract Eigenvector
void subEigen(Vect& a, const Vect& b)
{
	Vect temp = a;
	a = a - getEigenVect(b);
	a = a * (magnitudeOfVect(temp)/magnitudeOfVect(a));
}

// ---------------------------------------Randomly Add or Subtract Eigenvector
void randEigen(Vect& a, const Vect& b)
{
	Vect temp = a;
	a = a + getEigenVect(b)* floatPlusOrMinus();
	a = a * (magnitudeOfVect(temp)/magnitudeOfVect(a));
}

// --------------------------------------------------------------Cross Product
Vect crossProd(const Vect& a) // Not strictly a crossp. Calculates orthogonal
{                             // vector to a.
	Vect temp;
	temp.x=a.y;
	temp.y= floatPlusOrMinus() * a.x;//-a.x;
	return temp;
}

// ------------------------------------------------------------Cross Product 2
float crossProd(const Vect& a, const Vect& b) // Not strictly a crossp. Calc
{                          			// area of parallelogram between a and b.
	float temp=(a.x * b.y) - (a.y * b.x);
	return temp;
}

// -----------------------------------------------------------Scalar Projection
float compBontoA(const Vect& b, const Vect& a) // Finds component of b along a
{
	float temp=(a * b) / magnitudeOfVect(a);
	return temp;
}

// ----------------------------------------------------------Raise Vect to Pow
Vect powVect(const Vect& baseVect, float exp)
{
	Vect temp;
	temp.x=pow(baseVect.x,exp);
	temp.y=pow(baseVect.y,exp);
	return temp;
}

//---------------------------------------------------------------Pure Pursuit
Vect purePursuitDir(const Vect& positionA, const Vect& positionB)
{                     // returns an Eigen direction vector to nearest neighbor
	Vect temp = positionB - positionA;
	return getEigenVect(temp);
}

// ------------------------------------------------------------------Collide
void collide(mobile& agentA, const mobile& agentB)// Collisions between agents
{
	Vect temp = agentA.velocity;
	agentA.velocity = agentA.velocity -
	purePursuitDir(agentA.position,agentB.position);
	agentA.velocity = agentA.velocity *
		(magnitudeOfVect(temp)/magnitudeOfVect(agentA.velocity));
} //Simple collisions; improved version in the works. See below.

/*void collideProper(mobile& agentA, mobile& agentB,int i, int iterations)
{
	if(*agentA.collided<1 && !(agentA.collided==&agentB.collided))
		{
			Vect linearMomentum, normalI, normalJ, los;
			los=purePursuitDir(agentA.position,agentB.position);
			linearMomentum=(agentA.mass*agentA.velocity)+(agentB.mass*agentB.velocity);
			normalI=agentA.velocity-(compBontoA(agentA.velocity, los)*los);
			normalJ=agentB.velocity-(compBontoA(agentB.velocity, los)*los);
			agentA.velocity=normalI+linearMomentum;
			agentB.velocity=normalJ+linearMomentum;
			++(*agentA.collided);
			agentA.collided=agentB.collided;
		 }
} // Not working yet. The problem it seems, is that due to cinematic scheduling,
  // the agents collide multiple times, and initiate a positive feedback loops.
  // My implementation of flags didn't work, nor did my first try with pointers.
  // I'm confident this could eventually be resolved. I believe the mathematics
  // and physics for the colision algorithm above is correct, however.
  // NEW: The logic used for collision avoidance will probably work for this.
*/


// --------------------------------------------------------------Radial Force
Vect radialForce(const Vect& positionA, const Vect& positionB, float coefficient, float power)
{
	Vect force;
	force=purePursuitDir(positionA,positionB)*
		(coefficient/pow(magnitudeOfVect(positionB-positionA),power));
	return force;
}

// --------------------------------------------------------Steer Away From POCA
// Calculates and adds steering vector required to avoid POCA
void steerAwayFromPOCA(mobile& agentA)
{
	float criticalDistance = static_cast<float>(4 * AGENT_DIAMETER);
	float dCDdT = compBontoA(agentA.losToPOCA, agentA.velocity); // calculate rate of change of distance to POCA
	if( agentA.distanceToPOCA < criticalDistance) //If within critical radius
		{

			Vect sftyVect = -1.0 * getEigenVect(agentA.losToPOCA)*.5*criticalDistance; // calculate safe radius
			Vect avdVect = agentA.losToPOCA + sftyVect;  // calculate vector to safe radius
			float distanceToSftyRad = magnitudeOfVect(avdVect);
			if(dCDdT > 0) // If getting closer to POCA
			  if(agentA.distanceToNNatPOCA <= 2.0*magnitudeOfVect(sftyVect)) // new condition, investigate
				{
					Vect steeringVect = (criticalDistance-distanceToSftyRad)*crossProd(sftyVect); // calculate steering vector
					Vect temp = agentA.velocity;
					agentA.velocity = agentA.velocity + steeringVect;
					agentA.velocity = agentA.velocity * (magnitudeOfVect(temp)/magnitudeOfVect(agentA.velocity)); // steer
				}

		}  // Might be interesting to implement with steering force or acceleration
}          // rather than direct manipulation of agent velocity.

// ------------------------------------------------------------------Avoid POCA
// Calculates and directs agent away from point of closest approach (POCA)
void avoidPOCA(mobile& agentA, mobile& agentB, Vect& relPos, Vect& relVel)
{
	float time;
	float dotPRelV = relVel * relVel;
	if (dotPRelV > 0) // if absolute value of relative velocity squared > 0 to avoid division by 0;
		if (agentA.POCA != agentA.position) // if there actually is a POCA
		   if (agentB.avoidingPOCA == false) // if not pointing at agent B
			{
				time =(-(relVel.x*relVel.x)-(relPos.y*relVel.y))/(dotPRelV); // calculate POCA wrt. agent B
				agentA.POCA = agentA.position + (agentA.velocity * time); // see documentation for math
				agentA.losToPOCA = agentA.POCA-agentA.position;
				agentA.avoidingPOCA = true;
				// agentB.avoidingPOCA = true;
				agentA.avd = &agentB.avoidingPOCA; // point current agent at objective agent
				agentA.distanceToPOCA = magnitudeOfVect(agentA.losToPOCA);
				steerAwayFromPOCA(agentA);
			}
		   else if (agentA.avd == &agentB.avoidingPOCA) // if an avoidance action has already occured, and
				 { // it was agent B
					agentA.avoidingPOCA = true; // continue on present course
				 }
				else // if avoided but not agent B
				  {
					time =(-(relVel.x*relVel.x)-(relPos.y*relVel.y))/(dotPRelV); // calculate POCA wrt. agent B
					agentA.POCA = agentA.position + (agentA.velocity * time);
					agentA.losToPOCA = agentA.POCA-agentA.position;
					agentA.avoidingPOCA = true;
					agentA.avd = &agentB.avoidingPOCA; // point current agent at objective agent
					agentA.distanceToPOCA = magnitudeOfVect(agentA.losToPOCA);
					agentA.distanceToNNatPOCA = magnitudeOfVect(agentB.position + (agentB.velocity * time) - (agentA.position + (agentA.velocity * time)));
					steerAwayFromPOCA(agentA);
				  }

}

// -------------------------------------------------------Update Center of Mass
inline void updateCtrOfMass()
{
   Form1->ShapeCtrOfMass->Left = daemon.ctrOfMass.x - Form1->ShapeCtrOfMass->Width / 2;
   Form1->ShapeCtrOfMass->Top = daemon.ctrOfMass.y - Form1->ShapeCtrOfMass->Height / 2;
}

// ---------------------------------------------------------------Update Shape
// Graphically updates agent position
inline void updateShape(bool render, int i)
{
	  if(render)
	  {
		shape[i]->Left = agent[i].position.x - shape[i]->Width / 2;
		shape[i]->Top =  agent[i].position.y - shape[i]->Height / 2;
		shape[i]->Brush->Color =
			static_cast<TColor>(colorRamp(100, static_cast<int>(agent[i].energy*5000)));
	  }
}

// -----------------------------------------------------------------To Render?
// Determines whether to render on present iteration
inline bool toRender()
{
	bool temp;
	((daemon.iterations % 20) == 0) ? temp=true : temp=false;
	return temp;
}

// --------------------------------------------------------------Check Bounds
void checkBounds() //no argument version for updating walls
{
	  switch(2)//Form1->RadioGroupBounds->ItemIndex)
	  {
		case 0:
			{
				Form1->LeftWall->Visible=true;
				Form1->RightWall->Visible=true;
				Form1->UpperWall->Visible=false;
				Form1->LowerWall->Visible=false;
			}
		 break;
		case 1:
			{
				Form1->LeftWall->Visible=true;
				Form1->RightWall->Visible=true;
				Form1->UpperWall->Visible=true;
				Form1->LowerWall->Visible=true;
			}
		 break;
		case 2:
			{
				Form1->LeftWall->Visible=false;
				Form1->RightWall->Visible=false;
				Form1->UpperWall->Visible=false;
				Form1->LowerWall->Visible=false;
			}
		 break;
		default:
		 break;
	  }
}


// --------------------------------------------------------------Check Bounds
void checkBounds(mobile& anAgent)
{
	  daemon.posRelCtr=anAgent.position-daemon.boundsCtr;
	  switch(2)//Form1->RadioGroupBounds->ItemIndex)
	  {

		case 0:
			{
				Form1->LeftWall->Visible=true;
				Form1->RightWall->Visible=true;
				Form1->UpperWall->Visible=false;
				Form1->LowerWall->Visible=false;
				if(abs(long(daemon.posRelCtr.x)) > (long(daemon.ctrSpace.x-AGENT_DIAMETER)/2))
					bounce(OFF_HORIZONTAL, anAgent);
				if(anAgent.position.y < 0)
					anAgent.position.y = 536;
				  else if(anAgent.position.y > 536)
						anAgent.position.y = 0;
			}
			break;

		case 1:
			{
				Form1->LeftWall->Visible=true;
				Form1->RightWall->Visible=true;
				Form1->UpperWall->Visible=true;
				Form1->LowerWall->Visible=true;
				if(abs(long(daemon.posRelCtr.x)) >
					(long(daemon.ctrSpace.x-AGENT_DIAMETER)/2))
						bounce(OFF_HORIZONTAL, anAgent);
				if(abs(long(daemon.posRelCtr.y)) >
					(long(daemon.ctrSpace.y-AGENT_DIAMETER)/2))
						bounce(OFF_VERTICAL, anAgent);
			}
			break;

		case 2:
			{
				Form1->LeftWall->Visible=false;
				Form1->RightWall->Visible=false;
				Form1->UpperWall->Visible=false;
				Form1->LowerWall->Visible=false;
				if(anAgent.position.x < 0)
						anAgent.position.x = 530;
					else if(anAgent.position.x > 530)
						anAgent.position.x = 0;
				if(anAgent.position.y < 0)
						anAgent.position.y = 536;
					else if(anAgent.position.y > 536)
						anAgent.position.y = 0;
			}
			break;

		default:
		break;
	  }
} // Enforces boundraries; still needs considerable work to prevent "leakage".
// The problem, it seems, is that the agent can move past the boundrary when
// increment size of iteration is too big. My current work around is to decrease
// increment size and only render every after n frames, where n > increment

//------------------------------------------------------------Nearest Neighbor
int nearestNeighbor (int me) // Finds nearest neighbor, returns index
{
	int myNeighbor;
	Vect temp;
	float dist = 10000,z;
	for (int k = 0; k < NUMBER_OF_AGENTS; k++)
		{
			if (k == me) continue;
			temp = resultant(agent[me].position, agent[k].position);

			if ((z=(magnitudeOfVect(temp))) < dist)
				{
				dist = z;
				myNeighbor = k;
				}
	}
	return myNeighbor;
}

//----------------------------------------------------------------------Reset
void reset (void) // Resets simulation
{
   daemon.iterations = 0;
   Form1->EditIterations->Text = daemon.iterations;
   daemon.sysEnergy=Form1->TrackBarInitialSystemEnergy->Position;
   daemon.totalKE = 0;
   stV(daemon.tlCorner, Form1->LeftWall->Width, (Form1->UpperWall->Height)
		- (Form1->LeftWall->Top));
   stV(daemon.ctrSpace, (Form1->RightWall->Left) - ((Form1->LeftWall->Left)
		+ (Form1->LeftWall->Width)), (Form1->LowerWall->Top)-(Form1->UpperWall->Top+Form1->UpperWall->Height));
   daemon.boundsCtr = daemon.tlCorner + daemon.ctrSpace/2;
   for (int i = 0; i < NUMBER_OF_AGENTS; i++)
   {
	  agent[i].velocity = randVelocity();
	  nullVect(agent[i].force);
	  nullVect(agent[i].acceleration);
	  agent[i].energy = .5*agent[i].mass*(agent[i].velocity)*(agent[i].velocity);
	  daemon.totalKE += agent[i].energy;
	  agent[i].affinity = (int)random(5);
	  shape[i]->Brush->Color =
			static_cast<TColor>(colorRamp(100, static_cast<int>(agent[i].energy*100)));
	  updateShape(toRender, i);
   }
   spawnOnCoordinate(daemon.pointA, daemon.pointB);
   daemon.ctrOfMass=(daemon.pointA+daemon.pointB)/2;
   updateCtrOfMass();
   checkBounds();
   Form1->EditTotalKE->Text=daemon.totalKE;
}

//-----------------------------------------------------------------------Step
void step()  // The "engine". Updates positions and performs calculations
 {           // using cinematic scheduling.
   daemon.iterations++;
   Form1->EditIterations->Text = daemon.iterations;
   Vect nearResultant, relV, aggregate, dstToObj;
   nullVect(aggregate);
   int j, k;
   daemon.behaviorRange = Form1->TrackBarBehaviorRange->Position;
   float newKE = 0;
   for (int i=0;i<NUMBER_OF_AGENTS; i++)
   {
	  // update or aggregate positions, store previous values.
	  aggregate=aggregate+agent[i].position;  //assumes mass of j = mass of n = 1
	  agent[i].positionLast = agent[i].position;
	  agent[i].velocityLast = agent[i].velocity;
	  daemon.agentPosition = agent[i].position;        // Clean up
	  daemon.agentLastPosition = agent[i].positionLast;
	  // calculate agent's new position based on velocity and acceleration
	  agent[i].acceleration = agent[i].force / agent[i].mass;
	  nullVect(agent[i].force);
	  agent[i].velocity = agent[i].velocity + agent[i].acceleration * daemon.increment;
	  agent[i].position = agent[i].position + agent[i].velocity * daemon.increment;
	  nullVect(agent[i].acceleration);
	  // move the visualization on the screen accordingly
	  updateShape(toRender(),i);
	  // calculate nearest neighbor and resultant vector
	  dstToObj = agent[i].positionObjective - agent[i].position;
	  j=nearestNeighbor(i);
	  nearResultant=resultant(agent[i].position, agent[j].position);
	  relV=resultant(agent[i].velocity, agent[j].velocity);
	  // collisions toggle
	  if(Form1->CheckBoxCollisions->Checked)
		if ((magnitudeOfVect(nearResultant)) < AGENT_DIAMETER)
			{
			collide(agent[i],agent[j]);
			//Beep(400, 400);
			}
	  // gravity toggle: deprecated for now, as it would only cause problems
	 // and serves no purpose in this model
	 /*if (Form1->CheckBoxGravity->Checked)
		if (Form1->CheckBoxRepulsive->Checked)
			agent[i].force=-1*radialForce(agent[i].position, daemon.ctrOfMass,
				 .5*(Form1->TrackBarGravityStrength->Position),2.0);
		else agent[i].force=radialForce(agent[i].position, daemon.ctrOfMass,
				 .5*(Form1->TrackBarGravityStrength->Position),2.0);  */
	 // Flocking Algorithms
	  switch(Form1->RadioGroupBehavior->ItemIndex)
	  {
		case 0:
		 break;
		case 1:
		   /*	if(magnitudeOfVect(dstToObj) <= 10)
				{
				 if(agent[i].positionObjective==daemon.pointA )
					agent[i].positionObjective=daemon.pointB;
					dstToObj = agent[i].positionObjective - agent[i].position;
				 if(agent[i].positionObjective!=daemon.pointA)
					agent[i].positionObjective=daemon.pointA;
					dstToObj = agent[i].positionObjective - agent[i].position;
				}
			if(magnitudeOfVect(dstToObj) > 10)  */
			if (magnitudeOfVect(dstToObj) < daemon.behaviorRange) // Basic path following
				if(agent[i].positionObjective == daemon.pointA)
						switchObj(agent[i].positionObjective, daemon.pointB);
					else if(agent[i].positionObjective == daemon.pointB)
						switchObj(agent[i].positionObjective, daemon.pointA);
						 //	else if(agent[i].positionObjective == daemon.pointC)
						 //	   switchObj(agent[i].positionObjective, daemon.pointA);
			steerTwdObj(agent[i].velocity, dstToObj);
		 break;
		case 2:
			if (magnitudeOfVect(dstToObj) < daemon.behaviorRange) // rule 1: follow path
				if(agent[i].positionObjective == daemon.pointA)
						switchObj(agent[i].positionObjective, daemon.pointB);
					else if(agent[i].positionObjective == daemon.pointB)
						switchObj(agent[i].positionObjective, daemon.pointA);
			steerTwdObj(agent[i].velocity, dstToObj);
			if(magnitudeOfVect(nearResultant) < static_cast<float>(2*AGENT_DIAMETER)) // rule 2: avoid collisions
				avoidPOCA(agent[i], agent[j], nearResultant, relV);
		 break;
		   /*	case 1:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				addEigen(agent[i].velocity,agent[j].velocity);
				// Add nearest neighbor's eigenvector, and normalize magnitude.
		 break;
		case 2:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				subEigen(agent[i].velocity,agent[j].velocity);
				// Subtract nearest neighbor's eigenvector, and normalize mag.
		 break;
		case 3:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				randEigen(agent[i].velocity,agent[j].velocity);
		 break; // Randomly add or subtract NN's eigenvector.
		case 4:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
			{
				addEigen(agent[i].velocity,agent[j].velocity);
				if(magnitudeOfVect(nearResultant) < (daemon.behaviorRange/2))
					subEigen(agent[i].velocity,agent[j].velocity);
			}    // Add or Subtract nearest neighbor's eigenvector depending
				 // on range, and normalize magnitude.
		 break;
		case 5:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				addEigen(agent[i].velocity,crossProd(agent[j].velocity));
		 break;  // Add NN's normal eigenvector
		case 6:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				addEigen(agent[i].velocity,crossProd(agent[j].velocity));
				if(magnitudeOfVect(nearResultant) < (daemon.behaviorRange/3))
					agent[i].force=radialForce(agent[i].position, daemon.ctrOfMass, 1.0,1.0);
		 break; // Normal eigenvector + attraction
		case 7:
			if(magnitudeOfVect(nearResultant) < daemon.behaviorRange)
				agent[i].force=radialForce(agent[i].position, daemon.ctrOfMass, .0001,-1.0);
				if(magnitudeOfVect(nearResultant) < (daemon.behaviorRange/4))
					agent[i].force=radialForce(agent[i].position, daemon.ctrOfMass,-1.0,3.0);
		 break;
		 case 8:
			agent[i].force=radialForce(agent[i].position, daemon.ctrOfMass, .0001,-1.0);
		 break;  */ // None of these are relevant, and have hence been depricated
		default:
		 break;
		}

	  // Improved Bounds Checking
	  checkBounds(agent[i]);
	  // Calculate KE
	  agent[i].energy = .5*agent[i].mass*(agent[i].velocity)*(agent[i].velocity);
	  newKE += agent[i].energy;
	 }
   daemon.ctrOfMass=aggregate/NUMBER_OF_AGENTS;
   updateCtrOfMass();
   daemon.totalKE -= (daemon.totalKE)-(newKE);
   Form1->EditTotalKE->Text= daemon.totalKE;
   for(int i = 0; i < NUMBER_OF_AGENTS; i++) // resets POCA flags, links
	{
		agent[i].avd = NULL;
		agent[i].avoidingPOCA = false;
	}
   }


//---------------------------------------------------------------------------
void run()
{
   daemon.stop = false;
   while (daemon.stop == false)
   {
	  step();
	  Application->ProcessMessages();
   }
}

//===========================================================================
//															   EVENT HANDLERS
//===========================================================================

//-------------------------------------------------------------On Form Create
void __fastcall TForm1::FormCreate(TObject *Sender)
{
  stV(daemon.ctrPoint,265,268);
  stV(daemon.pointA, 420.0,90.0);  // spawnpoints
  stV(daemon.pointB, 90.0,420.0);
  //stV(daemon.pointC, 490.0,370.0);
  daemon.ctrOfMass=(daemon.pointA+daemon.pointB)/2;
  ShapeCtrOfMass->Left = daemon.ctrOfMass.x - ShapeCtrOfMass->Width / 2;
  ShapeCtrOfMass->Top = daemon.ctrOfMass.y - ShapeCtrOfMass->Height / 2;
  for (int i=0;i<NUMBER_OF_AGENTS; i++) // Tried to define as function, but was
	{                                   // not member of TShape class.
	  agent[i].affinity=(int)random(5);  // Ideally, this would be a member of
	  shape[i] = new TShape(this);  // the coordinator class, allowing multiple
	  shape[i]->Parent = Form1;  // coordinators to run an arbitrary number of
	  shape[i]->Visible = true;    // simulations, which might be useful for
	  shape[i]->OnMouseDown = ShapeMouseDown; // injecting fit individuals from
	  shape[i]->OnMouseUp = ShapeMouseUp;  // one population into another.
	  shape[i]->OnMouseMove = ShapeMouseMove;
	  shape[i]->Width = AGENT_DIAMETER;
	  shape[i]->Height = AGENT_DIAMETER;
	  shape[i]->Left = 0;
	  shape[i]->Top = 0;
	  shape[i]->Shape = stCircle;
	  shape[i]->Tag = i;
	  shape[i]->Pen->Width = 2;
	  shape[i]->Brush->Color = clBlack;
	  shape[i]->Pen->Color = clBlack;
		 //static_cast<TColor>(colorRamp(5, agent[i].affinity));
   }
   reset();
}

//------------------------------------------------------- On Shape Mouse Down
// An exception to the rule which says "Let Borland write event handlers."
// This one you must type in yourself in addition to a reference to it
// in Unit1.h (look at the source code in that unit).
void __fastcall TForm1::ShapeMouseDown(TObject *Sender,
	  TMouseButton Button, TShiftState Shift, int X, int Y)
{
   TShape *shape = dynamic_cast<TShape*>(Sender);
   // The following will be used for mouseMove and mouseUp events...
   // Captures the index number of the shape that was clicked
   daemon.chosenAgent = shape->Tag;
   // remembers is we were stopped or running
   daemon.wasStopped = daemon.stop;
   daemon.stop = true;
   if (Button == 0) {                              // Begin drag object
	   // Remembers that an shape was chosen for mouseMove and mouseUp
	   daemon.agentWasJustChosen = true;
	   // Remembers where on the shape the mouse was downed
	   daemon.shapeDownX = X;
	   daemon.shapeDownY = Y;
   }
   else {                                         // Show nearest neighbor
	   Form1->Canvas->MoveTo(agent[daemon.chosenAgent].position.x,
	   agent[daemon.chosenAgent].position.y);
	   Form1->Canvas->LineTo(agent[nearestNeighbor(daemon.chosenAgent)].position.x,
			agent[nearestNeighbor(daemon.chosenAgent)].position.y);
   }
}

//------------------------------------------------------- On Shape Mouse Move
// An exception to the rule which says "Let Borland write event handlers."
// This one you must type in yourself in addition to a reference to it
// in Unit1.h (look at the source code in that unit).
void __fastcall TForm1::ShapeMouseMove(TObject *Sender,
	  TShiftState Shift, int X, int Y)
{
   TShape *shape = dynamic_cast<TShape*>(Sender);
   daemon.chosenAgent = shape->Tag;
   if (daemon.agentWasJustChosen) {
	  // this drags the shape along...
	  shape->Left = shape->Left + X - daemon.shapeDownX;
	  shape->Top = shape->Top + Y - daemon.shapeDownY;
	  agent[daemon.chosenAgent].position.x = shape->Left + shape->Width / 2;;
	  agent[daemon.chosenAgent].position.y = shape->Top + shape->Height / 2;;
   }
   // When the mouse moves over an shape its values are displayed
   EditAgent->Text = shape->Tag;
   EditX->Text = int(agent[daemon.chosenAgent].position.x);
   EditY->Text = int(agent[daemon.chosenAgent].position.y);
   EditVx->Text = agent[shape->Tag].velocity.x;
   EditVy->Text = agent[shape->Tag].velocity.y;
   EditAgentKE->Text = agent[shape->Tag].energy;
   EditPOCAx->Text = int(agent[daemon.chosenAgent].POCA.x);
   EditPOCAy->Text = int(agent[daemon.chosenAgent].POCA.y);
}

//--------------------------------------------------------- On Shape Mouse Up
void __fastcall TForm1::ShapeMouseUp(TObject *Sender,
	  TMouseButton Button, TShiftState Shift, int X, int Y)
{
   TShape *shape = dynamic_cast<TShape*>(Sender);
   // this is the end of the drag...
   daemon.agentWasJustChosen = false;
   EditAgent->Text = shape->Tag;
   // When the mouse moves over an shape its values are displayed
   EditX->Text = int(agent[daemon.chosenAgent].position.x);
   EditY->Text = int(agent[daemon.chosenAgent].position.y);
   EditVx->Text = agent[shape->Tag].velocity.x;
   EditVy->Text = agent[shape->Tag].velocity.y;
   EditAgentKE->Text = agent[shape->Tag].energy;
   EditPOCAx->Text = int(agent[daemon.chosenAgent].POCA.x);
   EditPOCAy->Text = int(agent[daemon.chosenAgent].POCA.y);
   // if we were running before the drag, then run...
   Form1->Refresh();
   if (!daemon.wasStopped) {
	   run();
   }
}

//-----------------------------------------------------------------Run Button
void __fastcall TForm1::ButtonRunClick(TObject *Sender)
{
	run();
}

//----------------------------------------------------------------Stop Button
void __fastcall TForm1::ButtonStopClick(TObject *Sender)
{
	daemon.stop=true;
}

//---------------------------------------------------------------Reset Button
void __fastcall TForm1::ButtonResetClick(TObject *Sender)
{
	reset();
}
//----------------------------------------------------------------Step Button
void __fastcall TForm1::ButtonStepClick(TObject *Sender)
{
   daemon.stop = true;
   step();
}

//////////////////////////////////////////////////////////////////END PROGRAM