Vector3 UnitAiMoonGuard::avoidShips(const WorldInterface& world, const Vector3& originalVelocity)
{
    Vector3 agentPosition = getShip().getPosition();
    vector<PhysicsObjectId> shipIDs = world.getShipIds(agentPosition, SAFE_DISTANCE);
    unsigned int amount = shipIDs.size();

    if (amount == 1) {
        return originalVelocity;
    }

    PhysicsObjectId closestID = PhysicsObjectId::ID_NOTHING;
    double closestDistance = -1;
    PhysicsObjectId agentID = getShip().getId();
    for (unsigned int i = 0; i < amount; i++) {
        PhysicsObjectId shipID = shipIDs[i];
        if (agentID != shipID) {
            Vector3 shipPosition = world.getPosition(shipIDs[i]);
            double distance = agentPosition.getDistanceSquared(shipPosition);
            if (closestDistance < 0) {
                closestDistance = distance;
                closestID = shipIDs[i];
            }
            else if (distance <= closestDistance) {
                closestDistance = distance;
                closestID = shipIDs[i];
            }
        }
    }
    if (closestDistance == -1) {
        return originalVelocity;
    }
    return 	m_steering_behaviour.evade(world, closestID);
}

void UnitAiMoonGuard :: run (const WorldInterface& world)
{
    assert(world.isAlive(getShipId()));

    scan(world);

    Vector3 v = getShip().getVelocity();
    if (nearestEnemyShip != PhysicsObjectId::ID_NOTHING)
    {
        v = chargeAtTarget(world, nearestEnemyShip, v);
        shootAtShip(world, nearestEnemyShip);
    }
    else
    {
        Vector3 moon_pos = world.getPosition(moon);
        double moon_radius = world.getRadius(moon);
        v = steeringBehaviour->patrolSphere(world,
                                            moon_pos,
                                            moon_radius,
                                            PLANETOID_AVOID_DISTANCE);
        //printf("Patrolling...\n");
    }
    v = avoidShips(world, v);
    v = avoidRingParticles(world, v);

    getShipAi().setDesiredVelocity(v);
}

void Ship::update(WorldInterface& world)
{
	if (m_health <= 0 && m_status == dying){
		m_status = dead;
		world.addExplosion(getPositionPrevious(), EXPLOSION_SIZE, 0);
	}

	if (m_status == alive){
		if (isUnitAiSet()){
			double duration = TimeSystem::getFrameDuration();
			m_coordinate.rotateToVector(m_desired_velocity, duration*m_rotation_rate);
			changeSpeed(m_desired_velocity.getNorm(), duration*m_acceleration);
			if (m_fire_bullet_desired && isBulletReady()){
				world.addBullet(getPosition(), m_coordinate.getForward(), m_id);
				m_fire_bullet_desired = false;
				reloadBullet();
			}
		}
		updateBasic();
		m_reloadTimer.update();
	}


	else if (m_health > 0 && m_status == dead){
		m_status = alive;
		Vector3 position = m_coordinate.getPosition();
		position -= 500 * m_coordinate.getForward();
		m_coordinate.setPosition(position);
	}
}

void UnitAiMoonGuard::chooseTarget(const WorldInterface& world)
{
    const Vector3& ship_position = getShip().getPosition();
    const Vector3& ship_forward = getShip().getForward();
    unsigned int   ship_fleet = getShipId().m_fleet;

    vector<PhysicsObjectId> v_ids = world.getShipIds(ship_position, SCAN_RADIUS);

    m_id_target = PhysicsObjectId::ID_NOTHING;
    double best_angle = VERY_LARGE_ANGLE;

    for (unsigned int i = 0; i < v_ids.size(); i++)
    {
        PhysicsObjectId other_id = v_ids[i];

        if (other_id.m_fleet != ship_fleet)
        {
            // we found an enemy ship

            Vector3 other_position = world.getPosition(other_id);
            Vector3 direction_to_other = other_position - ship_position;
            double angle_to_other = direction_to_other.getAngleSafe(ship_forward);

            if (angle_to_other < best_angle)
            {
                m_id_target = other_id;
                best_angle = angle_to_other;
            }
        }
        // else ship is part of our fleet, so ignore it
    }
}

void Ship :: update (WorldInterface& r_world)
{
    if (isUnitAiSet())
    {
        if (!desired_velocity.isZero())
        {
            double theta = max_rotation_rate * TimeSystem::getFrameDuration();
            if(theta > 0.0)
            {
                rotateTowards(desired_velocity.getNormalized(), theta);
            }
            
            double delta = max_acceleration * TimeSystem::getFrameDuration();
            if(delta > 0.0)
            {
                double currentSpeed = getSpeed();
                double desiredSpeed = desired_velocity.getNorm();
                
                if (currentSpeed < desiredSpeed)
                {
                    currentSpeed += delta;
                    if (currentSpeed > desiredSpeed) currentSpeed = desiredSpeed;
                    setSpeed(currentSpeed);
                }
                else if (currentSpeed > desiredSpeed)
                {
                    currentSpeed -= delta;
                    if (currentSpeed < desiredSpeed) currentSpeed = desiredSpeed;
                    setSpeed(currentSpeed);
                }
            }
        }
        
        if (wantsToFire && isReloaded())
        {
            r_world.addBullet(getPosition(), getForward(), getId());
            wantsToFire = false;
            markReloading();
        }
    }
    
	if(isAlive() && isDying())
	{
		r_world.addExplosion(getPositionPrevious(), getRadius() * EXPLOSION_SIZE_FACTOR, 0);
		m_is_dead = true;

		// no further updates
		assert(invariant());
		return;
	}

	if(isAlive())
	{
		updateBasic();  // moves the Ship

		m_reload_timer += TimeSystem::getFrameDuration();
	}

	assert(invariant());
}

Vector3 UnitAiMoonGuard::avoidParticles(const WorldInterface& world, const Vector3& originalVelocity) {
    Vector3 agentPosition = getShip().getPosition();
    if (world.getRingDensity(agentPosition) <= 0) {
        return originalVelocity;
    }
    vector<WorldInterface::RingParticleData> particles = world.getRingParticles(agentPosition, SAFE_DISTANCE);
    unsigned int amount = particles.size();
    if (amount == 0) {
        return originalVelocity;
    }

    WorldInterface::RingParticleData cloestParticle = particles[0];
    double closestDistance = cloestParticle.m_position.getDistanceSquared(agentPosition);
    for (unsigned int i = 1; i < amount; i++) {
        WorldInterface::RingParticleData particle = particles[i];
        double distance = particle.m_position.getDistanceSquared(agentPosition);
        if (distance < closestDistance) {
            cloestParticle = particle;
            closestDistance = distance;
        }
    }

    Vector3 avoidVelocity = m_steering_behaviour.avoid(world, avoidVelocity,
                            cloestParticle.m_position,
                            cloestParticle.m_radius,
                            cloestParticle.m_radius,
                            SAFE_DISTANCE);
    double speed = getShip().getSpeedMax() / amount * SAFE_DISTANCE / 100;

    return avoidVelocity.getCopyWithNorm(speed);
}

UnitAiMoonGuard::UnitAiMoonGuard(const AiShipReference& ship,
                                 const WorldInterface& world,
                                 const PhysicsObjectId& id_moon)
    : UnitAiSuperclass(ship), m_id_target(PhysicsObjectId::ID_NOTHING),
      m_steering_behaviour(ship.getId()), m_scan_counter(0),
      m_moon_id(id_moon), mp_world(&world)
{
    assert(ship.isShip());
    assert(id_moon.m_type == PhysicsObjectId::TYPE_PLANETOID);
    assert(world.isAlive(id_moon));
    assert(world.isPlanetoidMoon(id_moon));
}

void UnitAiMoonGuard::patrolMoon(const WorldInterface& world, const PhysicsObjectId& moonID)
{
    Vector3         planetoid_center = world.getPosition(moonID);
    double          planetoid_radius = world.getRadius(moonID);
    Vector3 desired_velocity = m_steering_behaviour.patrolSphere(world, planetoid_center,
                               planetoid_radius + PLANETOID_CLEARANCE,
                               planetoid_radius + Const::SHELL_THICKNESS);
    Vector3 avoid_velocity = avoidPlanets(world, desired_velocity);
    avoid_velocity = avoidParticles(world, avoid_velocity);
    avoid_velocity = avoidShips(world, avoid_velocity);
    getShipAi().setDesiredVelocity(avoid_velocity);
}

Vector3 UnitAiMoonGuard::avoidPlanets(const WorldInterface& world, const Vector3& originalVelocity)
{
    PhysicsObjectId planetoid_id = world.getNearestPlanetoidId(getShip().getPosition());
    Vector3         planetoid_center = world.getPosition(planetoid_id);
    double          planetoid_radius = world.getRadius(planetoid_id);
    return m_steering_behaviour.avoid(world,
                                      originalVelocity,
                                      planetoid_center,
                                      planetoid_radius,
                                      PLANETOID_CLEARANCE,
                                      PLANETOID_AVOID_DISTANCE);
}

Vector3 UnitAiMoonGuard::avoidPlanetoids(const WorldInterface &world, const Vector3& orig_velocity)
{
    Vector3 planetoid_pos = world.getPosition(nearestPlanetoid);
    double planetoid_radius = world.getRadius(nearestPlanetoid);

    Vector3 v = steeringBehaviour->avoid(world,
                                         orig_velocity,
                                         planetoid_pos,
                                         planetoid_radius,
                                         PLANETOID_CLEARANCE,
                                         PLANETOID_AVOID_DISTANCE);
    //printf("Avoiding Planetoid\n");
    return v;
}

UnitAiMoonGuard :: UnitAiMoonGuard (const AiShipReference& ship,
                                    const WorldInterface& world,
                                    const PhysicsObjectId& id_moon)
    : UnitAiSuperclass(ship)
{
    assert(ship.isShip());
    assert(id_moon.m_type == PhysicsObjectId::TYPE_PLANETOID);
    assert(world.isAlive(id_moon));
    assert(world.isPlanetoidMoon(id_moon));

    steeringBehaviour = new FleetName::SteeringBehaviour(ship.getId());
    moon = id_moon;
    scanCount = rand() % SCAN_COUNT_MAX;

}

void UnitAiMoonGuard::run(const WorldInterface& world)
{
    assert(world.isAlive(getShipId()));

    //
    //  Scan for enemies evey few frames.  We use a counter to
    //    know which frames to scan on.
    //

    m_scan_counter++;
    if (m_scan_counter >= SCAN_COUNT_MAX)
    {
        chooseTarget(world);
        m_scan_counter = 0;
    }

    //
    //  Check to make sure the target is still alive.  If we
    //    didn't scan this frame, it might have died since we
    //    last checked.
    //

    if (m_id_target != PhysicsObjectId::ID_NOTHING &&
            !world.isAlive(m_id_target))
    {
        // give up on dead target
        m_id_target = PhysicsObjectId::ID_NOTHING;
    }

    //
    //  If we have a target, shoot at it!  Otherwise, just stop and
    //    wait.  If we don't move while there is no target, we
    //    can't hit anything, so we won't die.
    //

    if (m_id_target != PhysicsObjectId::ID_NOTHING)
    {
        // if we have a living target
        assert(world.isAlive(m_id_target));

        steerToTargetAndShoot(world);
    }
    else
    {
        patrolMoon(world, m_moon_id);
    }
}

void UnitAiMoonGuard::shootAtShip(const WorldInterface& world, const PhysicsObjectId& target)
{
    if (!world.isAlive(target)) return;

    Vector3 target_pos = world.getPosition(target);
    Vector3 target_vel = world.getVelocity(target);

    Vector3 aim_dir = steeringBehaviour->getAimDirection(getShip().getPosition(),
                      Bullet::SPEED,
                      target_pos,
                      target_vel);
    double angle = getShip().getVelocity().getAngleSafe(aim_dir);
    if (angle <= SHOOT_ANGLE_RADIANS_MAX && getShip().isReloaded())
    {
        getShipAi().markFireBulletDesired();
        //printf("\tShooting at (%f, %f, %f)\n", target_pos.x, target_pos.y, target_pos.z);
    };
}

Vector3 SteeringBehaviour :: flee (const WorldInterface& world,
                                   const PhysicsObjectId& id_target)
{
	assert(id_target != PhysicsObjectId::ID_NOTHING);

	// no initialization needed
	m_steering_behaviour = FLEE;

	if(!world.isAlive(id_target))
	{
		assert(invariant());
		return Vector3::ZERO;
	}

	assert(world.isAlive(id_target));
	Vector3 result = flee(world, world.getPosition(id_target));
	assert(invariant());
	return result;
}

void UnitAiMoonGuard::steerToTargetAndShoot(const WorldInterface& world)
{
    assert(m_id_target != PhysicsObjectId::ID_NOTHING);
    assert(world.isAlive(m_id_target));

    Vector3 agent = getShip().getPosition();
    Vector3 agent_velocity = world.getVelocity(getShip().getId());

    Vector3 target = world.getPosition(m_id_target);
    Vector3 target_velocity = world.getVelocity(m_id_target);

    Vector3 desired_velocity;
    double agent_max_speed = world.getShipSpeedMax(getShip().getId());
    double fromAgentToTarget = agent.getAngle(target);
    double fromAgentToTargetFront = agent.getAngle(target + target_velocity);

    if (fromAgentToTargetFront < fromAgentToTarget) {
        desired_velocity = m_steering_behaviour.seek(world, m_id_target);
    }
    else {
        desired_velocity = m_steering_behaviour.getAimDirection(agent, Bullet::SPEED,
                           target, target_velocity);
        if (desired_velocity == Vector3::ZERO) {
            desired_velocity = m_steering_behaviour.pursue(world, m_id_target);
        }
        else {
            desired_velocity.setNorm(agent_max_speed);
        }
    }

    Vector3 avoid_velocity = avoidPlanets(world, desired_velocity);
    avoid_velocity = avoidParticles(world, avoid_velocity);
    avoid_velocity = avoidShips(world, avoid_velocity);
    getShipAi().setDesiredVelocity(avoid_velocity);

    double angle = agent_velocity.getAngle(desired_velocity);
    double distance = target.getDistanceSquared(agent);

    if (angle <= SHOOT_ANGLE_RADIANS_MAX && distance < FIRE_RANGE)
    {
        getShipAi().markFireBulletDesired();
    }
}

Vector3 SteeringBehaviour :: explore (const WorldInterface& world,
                                      double distance_new_position_min,
                                      double distance_new_position_max)
{
	assert(distance_new_position_min >= 0.0);
	assert(distance_new_position_min <= distance_new_position_max);
	assert(distance_new_position_min >  EXPLORE_DISTANCE_NEW_POSITION);

	double position_distance           = (distance_new_position_max + distance_new_position_min) * 0.5;
	double position_distance_tolerance = (distance_new_position_max - distance_new_position_min) * 0.5;

	bool is_new_position_needed = false;
	if(m_steering_behaviour         != EXPLORE ||
	   m_desired_distance           != position_distance ||
	   m_desired_distance_tolerance != position_distance_tolerance)
	{
		m_steering_behaviour         = EXPLORE;
		m_desired_distance           = position_distance;
		m_desired_distance_tolerance = position_distance_tolerance;
		is_new_position_needed       = true;
	}

	if(!world.isAlive(m_id_agent))
	{
		// the explore position doesn't matter at this point

		assert(invariant());
		return Vector3::ZERO;
	}

	Vector3 agent_position = world.getPosition(m_id_agent);
	double agent_speed_max = world.getShipSpeedMax(m_id_agent);
	assert(agent_speed_max >= 0.0);

	if(agent_position.isDistanceLessThan(m_explore_position, EXPLORE_DISTANCE_NEW_POSITION) ||
	   is_new_position_needed)
	{
		m_explore_position = calculateExplorePosition(agent_position);
	}

	return (m_explore_position - agent_position).getCopyWithNorm(agent_speed_max);
}

Vector3 SteeringBehaviour :: escort (const WorldInterface& world,
                                     const PhysicsObjectId& id_target,
                                     const Vector3& offset)
{
	assert(id_target != PhysicsObjectId::ID_NOTHING);

	// no initialization needed
	m_steering_behaviour = ESCORT;

	//
	//  This is a variation on the arrive steering behaviour.
	//    First we work out the desired position from the
	//    offset.  Then we arrive at that position.  Finally we
	//    add on the velocity of the target and truncate to our
	//    maximum speed.
	//
	//  A more sophisticated implementation would use a form of
	//    arrive based on pursue instead of seek.  Or maybe this
	//    already happens; I'm not sure.  In either case,
	//    slowing down for arrival messes up time predictions.
	//

	if(!world.isAlive(m_id_agent) ||
	   !world.isAlive(  id_target))
	{
		assert(invariant());
		return Vector3::ZERO;
	}

	assert(world.isAlive(m_id_agent ));
	assert(world.isAlive(  id_target));
	Vector3 agent_position  = world.getPosition(m_id_agent);
	Vector3 target_velocity = world.getVelocity(  id_target);
	Vector3 escort_position = calculateEscortPosition(world, id_target, offset);

	if(agent_position == escort_position)
	{
		// we are in the correct position, so just match speeds
		assert(invariant());
		return target_velocity;
	}

	double agent_speed_max    = world.getShipSpeedMax(m_id_agent);
	assert(agent_speed_max >= 0.0);
	double agent_acceleration = world.getShipSpeedMax(m_id_agent);
	assert(agent_acceleration >= 0.0);

	double current_distance = agent_position.getDistance(escort_position);

	double  desired_relative_speed    = calculateMaxSafeSpeed(current_distance, agent_acceleration);
	Vector3 desired_relative_velocity = (escort_position - agent_position).getCopyWithNorm(desired_relative_speed);
	Vector3 desired_agent_velocity    = desired_relative_velocity + target_velocity;

	// limit desired velocity to maximum speed
	assert(invariant());
	return desired_agent_velocity.getTruncated(agent_speed_max);
}

Vector3 SteeringBehaviour :: calculateEscortPosition (const WorldInterface& world,
                                                      const PhysicsObjectId& id_target,
                                                      const Vector3& offset) const
{
	assert(m_steering_behaviour == ESCORT);
	assert(id_target != PhysicsObjectId::ID_NOTHING);

	if(!world.isAlive(id_target))
		return Vector3::ZERO;

	Vector3 target_position = world.getPosition(id_target);
	Vector3 target_forward  = world.getForward (id_target);
	Vector3 target_up       = world.getUp      (id_target);
	Vector3 target_right    = world.getRight   (id_target);

	return target_position +
	       target_forward * offset.x +
	       target_up      * offset.y +
	       target_right   * offset.z;
}

Vector3 UnitAiMoonGuard::chargeAtTarget(const WorldInterface& world, const PhysicsObjectId& target, const Vector3& orig_velocity)
{
    if (!world.isAlive(target))
    {
        return orig_velocity;
    }

    Vector3 unit_pos = getShip().getPosition();
    Vector3 target_pos = world.getPosition(target);
    Vector3 target_forward = world.getForward(target);


    //Vector3 desired_velocity = steeringBehaviour->seek(world, target);
    Vector3 desired_velocity = steeringBehaviour->aim(world, target, Bullet::SPEED);
    //Vector3 desired_velocity = steeringBehaviour->escort(world, target, -target_forward * 50);
    Vector3 v = avoidPlanetoids(world, desired_velocity);

    //printf("\tRamming\n");
    return v;
}

void UnitAiMoonGuard::scan(const WorldInterface& world)
{
    scanCount++;
    if (scanCount == SCAN_COUNT_MAX)
    {
        //printf("Scanning...\n");
        Vector3 ship_pos = getShip().getPosition();

        nearbyShips = world.getShipIds(ship_pos, SCAN_DISTANCE_SHIP);
        getClosestShip(world);
        getClosestEnemyShip(world);

        Vector3 position = ship_pos + (getShip().getForward() * 500.f);
        nearbyRingParticles = world.getRingParticles(position, SCAN_DISTANCE_RING_PARTICLE);

        nearestPlanetoid = world.getNearestPlanetoidId(ship_pos);

        scanCount = 0;
    }
}