// Ben Sibelman, 1/21/2008
// bird-sample.cpp: Code sample demonstrating Runge-Kutta method applied to output
// from a physics function, as well as sophisticated use of 3D vector operations.

// Note: "bird coordinates" means a reference frame rotated around the Y axis so the
// bird's current velocity has a positive X component and no Z component.


// Move the bird based on the given length of timestep (seconds).
// Return false if bird has hit the ground, true otherwise.
bool Bird::Move(double timeStep)
{
	// get next animation step
	if (m_flapMode == FLAPPING || m_flapMode == BOUNDING) 
	{
		MoveFlapping(timeStep);

		// move wings ahead in flapping animation
		m_leftWing->flap(timeStep); 
		m_rightWing->flap(timeStep);
	} 
	else if (m_flapMode == GLIDING) 
	{
		MoveGliding(timeStep);
	} 
	else // m_flapMode == FALLING 
	{ 
		double* deltaV = new double[3];
		VecInit(deltaV, 0.0, -m_gravity * timeStep, 0.0); // just accelerate downward
		DoSimpleMove(deltaV, timeStep);
		delete [] deltaV;
	}

	// rotate bird based on user control
	UserControlPitchAndRoll();

	if (m_pos[1] <= 0) // crashed into the ground
	{
		m_pos[1] = m_scaleFactor; // set bird on top of ground
		return false; 
	} 
	else 
	{
		return true;
	}
}

// Accelerate the bird at a predefined rate toward a predefined speed in the direction of flight.
void Bird::MoveFlapping(double timeStep)
{
	// get the vector for where the bird wants to be heading based on facing direction
	double* desiredVel = new double[3];
	double pitchRadians = Deg2Rad(m_pitch);
	double cosPitchRadians = cos(pitchRadians);
	VecInit(desiredVel, c_flapSpeed * cosPitchRadians, c_flapSpeed * sin(pitchRadians), 0.0);
	double* deltaV = Subtract(desiredVel, m_vel);

	// scale to an acceleration increment per frame
	double totalChange = Length(deltaV);
	double currentAccel = (m_flapMode == FLAPPING ? c_flapAccel : c_boundAccel) * currentFreq;
	if (totalChange > currentAccel) 
	{
		// only scale it if the increment is less than the total remaining acceleration we need to do
		deltaV = scale(deltaV, currentAccel / totalChange);
	}

	// account for banking by adding a sideways component to the motion due to lift
	// (making the assumption for simplicity that lift just balances gravity)
	double birdUpVectorLift = m_gravity * cosPitchRadians;
	deltaV[2] = timeStep * birdUpVectorLift * -tan(m_roll); 

	// update the bird's velocity and position
	DoSimpleMove(deltaV, timeStep);

	// clean up
	delete [] desiredVel, deltaV;
}

// Apply Runge-Kutta method to gliding physics calculations and "AI" for setting wings' angle of attack
// (this smooths out the acceleration curve so it doesn't behave unpredictably).
void Bird::MoveGliding(double timeStep)
{
	// k0 = f(t0, x0) where k0 is our first approximation of the acceleration or wing angle change, 
	// f is the physics or "AI" function, t0 is the beginning of the timestep, and x0 is the current velocity
	// or wing angle.
	// (Actually we don't pass in the current position within the timestep because acceleration and angle 
	// adjustment are always the same functions of the velocity at any given instant.)
	double* accel0 = DoPhysics(m_vel, wingTilt, timeStep);
	double tiltAdjust0 = getTiltAdjust(m_vel);

	// based on this first approximation, find the velocity halfway through the timestep
	double* halfVel0 = add(m_vel, GetScaled(accel0, timeStep * 0.5)); 

	// k1 = f(t0 + h/2, x0 + h*k0/2) where h is the length of the timestep
	double* accel1 = DoPhysics(halfVel0, wingTilt + (tiltAdjust0 * 0.5), timeStep);
	double tiltAdjust1 = getTiltAdjust(halfVel0);

	// based on this second approximation, find the velocity halfway through the timestep again
	double* halfVel1 = add(m_vel, GetScaled(accel1, timeStep * 0.5));

	// k2 = f(t0 + h/2, x0 + h*k1/2)
	double* accel2 = DoPhysics(halfVel1, wingTilt + (tiltAdjust1 * 0.5), timeStep);
	double tiltAdjust2 = getTiltAdjust(halfVel1);

	// based on this third approximation, find the velocity at the end of the timestep
	double* fullVel = add(m_vel, GetScaled(accel2, timeStep));

	// k3 = f(t0 + h, x0 + h*k2)
	double* accel3 = DoPhysics(fullVel, wingTilt + tiltAdjust2, timeStep);
	double tiltAdjust3 = getTiltAdjust(fullVel);

	// calculate (k0 + 2*k1 + 2*k2 + k3)/6 for final acceleration, then multiply by timestep to get delta V
	double timeStepOverSix = timeStep / 6.0;

	double* deltaV = add(accel1, accel2);
	scale(deltaV, 2.0);
	deltaV = add(deltaV, add(accel0, accel3));
	scale(deltaV, timeStepOverSix);

	// calculate (k0 + 2*k1 + 2*k2 + k3)/6 for final wing angle adjustment value
	double tiltAdjust = (tiltAdjust0 + 2.0 * (tiltAdjust1 + tiltAdjust2) + tiltAdjust3) / 6.0;

	// based on current average attack angle, try to adjust the wing tilt for next timestep
	m_rightWing->tiltWing(tiltAdjust);
	m_leftWing->tiltWing(tiltAdjust);
	m_wingTilt += tiltAdjust;

	// now for the position: calculate (k0 + 2*k1 + 2*k2 + k3)/6 for smoothed velocity,
	// where k0 = current velocity, k1 = halfVel1, k2 = halfVel2, and k3 = fullVel,
	// then multiply by timestep to get change in position
	double* deltaPos = add(halfVel0, halfVel1);
	Scale(deltaPos, 2.0);
	deltaPos = add(deltaPos, Add(m_vel, fullVel));
	Scale(deltaPos, timeStepOverSix);

	// rotate position change into world coordinates
	deltaPos = RotateY(deltaPos, Deg2Rad(m_yaw)); 

	// update bird's stored position and velocity
	m_pos = add(m_pos, deltaPos);
	YawAndUpdateVelocity(deltaV);

	// clean up
	delete [] accel0, accel1, accel2, accel3, halfVel0, halfVel1, fullVel, deltaV, deltaPos;
}

// Given a change in velocity for a timestep, just apply it directly to the stored velocity and position.
void Bird::DoSimpleMove(double* deltaV, double timeStep)
{
	// update bird's stored velocity
	YawAndUpdateVelocity(deltaV);

	// just multiply new velocity by timestep length
	double* deltaPos = new double[3];
	VecInit(deltaPos, m_vel);
	deltaPos = scale(deltaPos, timeStep);

	// rotate position change into world coordinates
	deltaPos = RotateY(deltaPos, Deg2Rad(m_yaw)); 

	// update bird's stored position
	m_pos = add(m_pos, deltaPos);

	// clean up
	delete [] deltaPos;
}

// Take a delta V with a possible sideways component and translate it into a new velocity
// in bird coordinates (including pitch but not yaw).
void Bird::YawAndUpdateVelocity(double *deltaV)
{
	AddOn(m_vel, deltaV); // add the acceleration to the rotated velocity

	m_speed = Length(m_vel); // recalculate bird's stored speed

	// update bird's yaw (left-right rotation) according to the roll
	if (deltaV[2] != 0.0) 
	{
		// centripetal acceleration / velocity = angular velocity
		double turn = deltaV[2] / Length(m_vel); 

		// artificially allow turns twice as fast as that (small birds can turn FAST!)
		m_yaw -= (Rad2Deg(turn) * 2.0);
		if (m_yaw >= 360.0) 
		{
			m_yaw -= 360.0;
		} 
		else if (m_yaw < 0.0) 
		{
			m_yaw += 360.0;
		}
		if (m_vel[2] != 0.0) 
		{
			// keep velocity in bird's coordinates
			if (m_flapMode != FLAPPING) 
			{
				m_vel[0] = sqrt(sqr(m_vel[0]) + sqr(m_vel[2]));
			}
			m_vel[2] = 0.0;
		}
	}
}

// Calculate acceleration for a given starting velocity (in bird coordinates), 
// wing angle, and timestep using the functions
// lift and drag each = 1/2 * air density * wing area * velocity^2 / mass * attack factor,
// where the factor is different for lift and drag and each one is a hard-coded function 
// of the angle of attack.
double* Bird::DoPhysics(double* startVel, double wingTilt, double timeStep)
{
	double startSpeed = Length(startVel);
	double baseAccel = m_densityAreaMassConstant * sqr(startVel);

	// angle the velocity vector makes with the horizontal: not quite accurate since there may
	// be a small startVel[2] value sometimes, but this way it's harder to stall
	double velAngle = atan(startVel[1] / startVel[0]);

	// angle of attack = angle between the plane of the wings and the bird's velocity vector
	double attack = m_pitch + wingTilt - rad2deg(velAngle);

	// check if airflow is coming from behind (this should happen very rarely :-)
	if (attack > 90.0)
	{
		attack = 180.0 - attack;
	}
	else if (attack < -90.0)
	{
		attack = -180.0 - attack;
	}

	// calculate lift
	double liftFactor = GetLiftFactor(attack);
	if (liftFactor == 0.0) 
	{
		vecinit(m_liftAccel, 0.0, 0.0, 0.0);
	}
	else
	{
		// lift acts perpendicularly to the velocity
		delete [] m_liftAccel;
		m_liftAccel = GetUpVector(false, startVel);
		scale(m_liftAccel, liftFactor * baseAccel);
	}

	// calculate drag
	double dragFactor = GetDragFactor(attack);
	double* dragAccel;
	if (dragFactor == 0.0)
	{
		dragAccel = new double[3];
		vecinit(dragAccel, 0, 0, 0);
	}
	else
	{
		dragAccel = unit(startVel);
		scale(dragAccel, -dragFactor * baseAccel);
	}

	dragAmount = Length(dragAccel);
	if (dragAmount * timeStep > startSpeed) 
	{ 
		// drag can stop bird, not reverse direction
		Scale(dragAccel, startSpeed / timeStep / dragAmount);
	}

	double* accel = add(m_liftAccel, dragAccel);

	accel[1] -= m_gravity; // calculate effects of gravity

	return accel;
}

// "AI" function that just tries to tilt the wings so as to achieve the hard-coded optimum 
// angle of attack based on the current velocity, adjusting up or down if the bird's facing
// direction indicates that it wants to be travelling at a different angle.
double Bird::GetTiltAdjust(double* currentVel)
{
	double velAngle = Rad2Deg(asin(currentVel[1] / Length(currentVel)));
	double pitchDiff = m_pitch - velAngle;

	// current angle of attack: the net angle between the wings and the direction the bird is moving in
	double attack = pitchDiff + m_wingTilt; 

	// if pitch is too far down, try to pull up, and vice versa
	return c_idealAngle - attack + c_adjustFactor * pitchDiff; 
}

// Get the vector perpendicular to the given direction of flight that the bird would see as "up"
// (meaning if the bird is tipped over to the side, so is the up vector).  The given boolean
// determines if the vectors are in bird coordinates (false) or world coordinates (true).
double* Bird::GetUpVector(bool yawed, double* usedVel)
{
	double* unitVel = unit(usedVel);
	double* velRight; // horizontal vector perpendicular to velocity
	double* ret;
	if (yawed) 
	{
		velRight = cross(yAxis, usedVel); // cross product ensures result is perpendicular to both vectors

		// rotate the Y axis around the right vector to get the right pitch
		double yAxis[3] = {0.0, 1.0, 0.0};
		double* velPitched = Rotate(yAxis, velRight, Deg2Rad(pitch)); 

		// then spin the result around velocity vector by roll (already in radians)
		ret = Rotate(velPitched, unitVel, m_roll);

		delete [] velPitched;
	} 
	else 
	{
		// we know the non-yawed velocity will have a right vector along the Z axis
		velRight = new double[3];
		vecinit(velRight, 0.0, 0.0, -1.0);

		// spin the right vector -90 degrees plus the roll value
		ret = Rotate(velRight, unitVel, -M_PI / 2.0 + roll); 
	}

	Normalize(ret);

	delete [] velRight;
	return ret;
}