import static java.lang.Math.*;
import de.grogra.vecmath.Math2;

module Boid(double speed, int num) extends Cone 
{
	TMatrix4d t = new TMatrix4d();
	{ setLength(3); setShader(GREEN); setTransform(new TMatrix4d()); }
	
	public void setLocation(Point3d location)
	{
		((TMatrix4d) this[transform])[m03] = location.x; 
		((TMatrix4d) this[transform])[m13] = location.y; 
		((TMatrix4d) this[transform])[m23] = location.z; 
	}
	
	public void setDirection(Vector3d direction)
	{
		Matrix3d m = new Matrix3d();
		Math2.getOrthogonalBasis(direction, m, true);
		((TMatrix4d) this[transform])[m00] = m.m00; 
		((TMatrix4d) this[transform])[m01] = m.m01; 
		((TMatrix4d) this[transform])[m02] = m.m02; 
		((TMatrix4d) this[transform])[m10] = m.m10; 
		((TMatrix4d) this[transform])[m11] = m.m11; 
		((TMatrix4d) this[transform])[m12] = m.m12; 
		((TMatrix4d) this[transform])[m20] = m.m20; 
		((TMatrix4d) this[transform])[m21] = m.m21; 
		((TMatrix4d) this[transform])[m22] = m.m22; 
	}
}
module Master(super.speed) extends Boid(speed, -1) {{ setShader(RED); }};
module Master2(super.speed) extends Boid(speed, -1) {{ setShader(BLUE); }};
module Raptor(super.speed) extends Boid(speed, -1) {{ setShader(BLACK); }};


// current system time in milliseconds
long t;

const double V_MIN = 10;
const double V_MAX = 20;
const double D_MIN = 1;
const double D_MAX = 10;

protected void init ()
[
	{ t = System.currentTimeMillis(); }
	==>> ^[ Master(15)]  [Master2(15)]  Raptor(30);
	{ for(int i:(1:50)) [ ==>> ^ Boid(17, i).(setLocation(new Point3d(0, 0, -3*i))); ] }
]

public void run ()
[
	{
		// calculate time delta
		long delta = System.currentTimeMillis() - t;
		double dt = (double)delta / 1000;
		// some data for the clock
		final double r = 30;
		final double phi = (double)t * 10 / 1000 / r;
		final double c = cos(phi);
		final double s = sin(phi);
		// calculate center of mass of all boids
		Point3d center = mean(location((* Boid *)));
		// calculate alignment of group
		Vector3d alignment = mean(direction((* Boid *)));
	}

	m:Master(speed) ::> {
		// obtain current location and orientation
		Vector3d dir = direction(m);
		Point3d pos = location(m);
		// update location
		pos.scaleAdd(dt * speed, dir, pos);
		m.setLocation(pos);
		// calculate new heading
		Point3d p = new Point3d(r * c, r * s, 0);
		Vector3d d = new Vector3d(p);
		d.sub(pos);
		d.scale(0.1 * dt);
		dir.add(d);
		dir.normalize();
		m.setDirection(dir);
	}
	m:Master2(speed) ::> {
		// obtain current location and orientation
		Vector3d dir = direction(m);
		Point3d pos = location(m);
		// update location
		pos.scaleAdd(dt * speed, dir, pos);
		m.setLocation(pos);
		// calculate new heading
		Point3d p = new Point3d(r * c / 3, r * s / 3, 0);
		Vector3d d = new Vector3d(p);
		d.sub(pos);
		d.scale(0.1 * dt);
		dir.add(d);
		dir.normalize();
		m.setDirection(dir);
	}

	m:Raptor(speed) ::> {
		// obtain current location and orientation
		Vector3d dir = direction(m);
		Point3d pos = location(m);
		// update location
		pos.scaleAdd(dt * speed, dir, pos);
		m.setLocation(pos);
		// calculate new heading
		Point3d p = new Point3d(r * c / 10, r * s / 10, 0);
		Vector3d d = new Vector3d(p);
		d.sub(pos);
		d.scale(0.1 * dt);
		dir.add(d);
		dir.normalize();
		m.setDirection(dir);
	}	
	
	b:Boid(speed, id), (!(b instanceof Master)), (!(b instanceof Master2)), 
						(!(b instanceof Raptor)) ::> {
		// obtain current location and orientation
		Vector3d dir = direction(b);
		Point3d pos = location(b);
		// update location
		pos.scaleAdd(dt * speed, dir, pos);
		b.setLocation(pos);
		
		// stay together
		// d1 = center - pos, direction to center
		// strength depends on distance to center
		Vector3d d1 = new Vector3d(center);
		d1.sub(pos);
		d1.scale(0.1 * dt);
		
		// align to group
		Vector3d d2 = new Vector3d(alignment);
		d2.normalize();
		d2.scale(0.2 * dt);
		
		// evade neighbour
		// find closest neighbour and try to evade by steering to the
		// opposite direction, strength is greater when closer
		Vector3d d3;
		d3 = selectWhereMin(d3 = (* bb:Boid, (bb != b) *) - b, d3.lengthSquared());
		d3.normalize();
		d3.scale(-1.5 * dt / d3.length());
		
		Vector3d d4;
		// follow master if id mod 2 == 0
		if (id%2 == 0) {
			d4 = new Vector3d(first((* Master *)));
			d4.sub(pos);
			d4.normalize();
			d4.scale(1.5 * dt);
		}
		
		//follow master 2
		else {
			d4 = new Vector3d(first((* Master2 *)));
			d4.sub(pos);
			d4.normalize();
			d4.scale(1.5 * dt);
		}
		
		
		//direction of the Raptor --> d5, position --> rappos
		Raptor rap;
		Tuple3d rappos;
		Vector3d d5 = new Vector3d(rap = first((* Raptor *)));
		d5.sub(pos);
		double dist_rap = d5.length();
		d5.normalize();
		d5.scale(-6.8 * Math.exp(-dist_rap*0.05) * dt);
		rappos = rap.getTranslation();
		
		
		
		// calculate new heading direction
		dir.add(d1);
		dir.add(d2);
		dir.add(d3);
		dir.add(d4);
		dir.add(d5);		
		
		
		dir.normalize();
		b.setDirection(dir);
		
		
		if (id%2 == 0) {
			// change speed to keep up with master
			double d = distance(first((* Master *)), b);
			speed = min(V_MAX, max(V_MIN, 
				(V_MAX - V_MIN) * (d - D_MIN) / (D_MAX - D_MIN)));
		}
		else {
			// change speed to keep up with master2
			double d = distance(first((* Master2 *)), b);
			speed = min(V_MAX, max(V_MIN, 
				(V_MAX - V_MIN) * (d - D_MIN) / (D_MAX - D_MIN)));
		}
	}
	
	{ t += delta; }
]