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

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

#define MIN_SPEED 1
#define MAX_SPEED 2


boid::boid(void)
    {
    position_ = dms::randomPoint3(minPosition, maxPosition);
    velocity_ = dms::randomUnitVector3() *
                dms::randomFloat(MIN_SPEED, MAX_SPEED, DMS_GAUSSIAN_DISTRIBUTION);
    setTransform(transform_);
    transform_.setTranslation(position_);
    transform_.setRotation(heading(), 0, 1, 0);
    }
 
 
void boid::update(void)
    {
    computeAcceleration();
    velocity_ += acceleration_ * dms::deltaTime();
    constrainVelocity(MIN_SPEED, MAX_SPEED);
    position_ += velocity_ * dms::deltaTime();
    constrainPosition(minPosition, maxPosition);
    transform_.setTranslation(position_);
    transform_.setRotation(heading(), 0, 1, 0);
    }


void boid::computeAcceleration(void)
    {
    acceleration_ += dms::randomUnitVector3();
    }


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];
        velocity_[1] = 0;
        acceleration_[1] = 0;
        }
    else if (position_[1] > max[1])
        {
        position_[1] = max[1];
        velocity_[1] = 0;
        acceleration_[1] = 0;
        }

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