#include <stdio.h>
#include <math.h>
#include "boid2.h"

static const dms::Vector3 minPosition(-10, 1, -10);
static const dms::Vector3 maxPosition(10, 5, 10);



boid::boid(boidSpecies *species)
    {
    species_ = species;
    species_->add(this);
    position_ = dms::randomPoint3(minPosition, maxPosition);
    velocity_ = dms::randomUnitVector3() *
                dms::randomFloat(species_->minSpeed, species_->maxSpeed,
                                 DMS_GAUSSIAN_DISTRIBUTION);
    setTransform(transform_);
    transform_.setTranslation(position_);
    transform_.setRotation(heading(), 0, 1, 0);
    }
 
 
void boid::update(void)
    {
    computeAcceleration();
    velocity_ += acceleration_ * dms::deltaTime();
    constrainVelocity(species_->minSpeed, species_->maxSpeed);
    position_ += velocity_ * dms::deltaTime();
    constrainPosition(minPosition, maxPosition);
    transform_.setTranslation(position_);
    transform_.setRotation(heading(), 0, 1, 0);
    }


void boid::computeAcceleration(void)
    {
    float priority = 0;
    vector<boid*> * neighbors = species_->neighbors(this);
    acceleration_.set(0,0,0);
    if (neighbors->size() > 0)
        {
        flockSeparation(neighbors, priority);
        flockAlignment(neighbors, priority);
        flockCohesion(neighbors, priority);
        }
    if (priority < 0.1)
        acceleration_ += dms::randomUnitVector3();
    delete neighbors;
    }


void boid::flockSeparation(vector<boid*> * neighbors, float& priority)
    {
    int i;
    dms::Vector3 nearest, accel;
    float nearestDistance;
    nearest = (*neighbors)[0]->position_ - position_;
    nearestDistance = nearest.length();
    for (i = 1; i < neighbors->size(); i++)
        {
        float d = (*neighbors)[i]->position_.distance(position_);
        if (d < nearestDistance)
            {
            nearest = (*neighbors)[i]->position_ - position_;
            nearestDistance = d;
            }
        }
    if (nearestDistance > species_->minSeparation)
        return;
    if (priority < 1)
        {
        acceleration_ = -nearest;
        acceleration_.normalize();
        acceleration_ *= species_->maxAcceleration;
        priority = 1;
        }
    }


void boid::flockAlignment(vector<boid*> * neighbors, float& priority)
    {
    int i;
    dms::Vector3 avgVelocity, accel;
    for (i = 0; i < neighbors->size(); i++)
        avgVelocity += (*neighbors)[i]->velocity_;
    avgVelocity /= neighbors->size();
    accel = avgVelocity - velocity_;
    float accelLen = accel.length();
    if (accelLen > species_->maxAcceleration)
        {
        accel *= species_->maxAcceleration / accelLen;
        accelLen = species_->maxAcceleration;
        }
    float alignPriority = accelLen / species_->maxAcceleration;
    if (alignPriority > priority)
        {
        acceleration_ = accel;
        priority = alignPriority;
        }
    }


void boid::flockCohesion(vector<boid*> * neighbors, float& priority)
    {
    int i;
    dms::Vector3 center, accel;
    for (i = 0; i < neighbors->size(); i++)
        center += (*neighbors)[i]->position_;
    center /= neighbors->size();
    accel = center - position_;
    float accelLen = accel.length();
    if (accelLen > species_->maxAcceleration)
        {
        accel *= species_->maxAcceleration / accelLen;
        accelLen = species_->maxAcceleration;
        }
    float cohesionPriority = 0.5 * (accelLen / species_->maxAcceleration);
    if (cohesionPriority > priority)
        {
        acceleration_ = accel;
        priority = cohesionPriority;
        }
    }


GLfloat boid::heading(void)
    {
    return dms::radiansToDegrees(atan2(-velocity_[2], velocity_[0]));
    }


void boid::constrainVelocity(float min, float max)
    {
    float speed = velocity_.length();
    if (speed == 0)
        velocity_.set(min, 0, 0);
    else if (speed < min)
        velocity_ *= min / speed;
    else if (speed > max)
        velocity_ *= max / speed;
    }


void boid::constrainPosition(const dms::Vector3& min, const dms::Vector3& max)
    {
    while (position_[0] < min[0])
        position_[0] += max[0]-min[0];
    while (position_[0] > max[0])
        position_[0] -= max[0]-min[0];

    if (position_[1] < min[1])
        position_[1] = min[1];
    else if (position_[1] > max[1])
        position_[1] = max[1];

    while (position_[2] < min[2])
        position_[2] += max[2]-min[2];
    while (position_[2] > max[2])
        position_[2] -= max[2]-min[2];
    }