//Headerfile: Flocking.h

#ifndef _Flocking_H__
#define _Flocking_H__

#include "util/Math.h"

namespace orxonox
{
  class Element // An element that flocks
  {

  public:
    Vector3 location;                       //!< locationvector of the element
    Vector3 speed;                          //!< speedvector of the element
    Vector3 acceleration;                   //!< accelerationvector of the element
    bool movable;                                 //!< movability of the element, (false) gives the possiblity that an object can`t be moved by flocking but still gets into the calculation
    static int const SEPERATIONDISTANCE = 300;    //!< detectionradius of seperation
    static int const ALIGNMENTDISTANCE = 300;     //!< detectionradius of alignment
    static int const COHESIONDISTANCE = 5000;     //!< detectionradius of cohesion
    static int const ANZELEMENTS = 9;             //!< number of elements

    //! default constructor
    Element() {
      acceleration = Vector3(0,0,0);
      speed = Vector3(0,0,0);
      location = Vector3(0,0,0);
      movable = true;
    }

    /** constructor
    *  @param location_ sets locationvector of the element
    *  @param speed_ sets speedvector of the element
    *  @param acceleration_ sets accelerationvector of the element
    *  @param movable_ sets movability of the element
    */
    Element(Vector3 location_, Vector3 speed_, Vector3 acceleration_, bool movable_) {
      acceleration = acceleration_;
      speed = speed_;
      location = location_;
      movable = movable_;
    }

    //! function to chance values of an element
    void setValues(Vector3 location_, Vector3 speed_, Vector3 acceleration_, bool movable_) {
      acceleration = acceleration_;
      speed = speed_;
      location = location_;
      movable = movable_;
    }

    /** calculates the distance between the element and an other point given by temp
    * @param e remote object to calculate distance to
    */
    float getDistance(Element e) {
      Vector3 distance = e.location - location;
      return distance.length();
    }

    //! updates the data of an element
    void update(Element arrayOfElements[]) {
      if (this->movable == true) {calculateAcceleration(arrayOfElements);} //if element is movable, calculate acceleration
    }

    //! calculates the new acceleration of an element
    void calculateAcceleration(Element arrayOfElements[]) {
      acceleration = separation(arrayOfElements) + alignment(arrayOfElements) + cohesion(arrayOfElements);  //acceleration consisting of flocking-functions
    }

    //! separation-function (keep elements separated, avoid crashs)
    Vector3 separation(Element arrayOfElements[]) {
      using namespace Ogre;
      Vector3 steering = Vector3(0,0,0); //steeringvector
      Vector3 inverseDistance = Vector3(0,0,0);  //vector pointing away from possible collisions
      int numberOfNeighbour = 0;  //number of observed neighbours
      float distance = 0;  // distance to the actual element
      for(int i=0; i<ANZELEMENTS; i++) {  //go through all elements
        Element actual = arrayOfElements[i];  //get the actual element
        distance = getDistance(actual);  //get distance between this and actual
        if ((distance > 0) && (distance < SEPERATIONDISTANCE)) {  //do only if actual is inside detectionradius
          inverseDistance = (0,0,0);
          inverseDistance = location-actual.location;  //calculate the distancevector heading towards this
          //adaptation of the inverseDistance to the distance
          if ((distance < 200) && (distance >= 120)) {inverseDistance = 2*inverseDistance;}
          if ((distance < 120) && (distance >= 80)) {inverseDistance = 5*inverseDistance;}
          if ((distance < 80) && (distance >= 40)) {inverseDistance = 10*inverseDistance;}
          if ((distance < 40) && (distance > 0)) {inverseDistance = 10*inverseDistance;}
          steering = steering + inverseDistance;  //add up all significant steeringvectors
          numberOfNeighbour++;  //counts the elements inside the detectionradius
        }
      }
      if(numberOfNeighbour > 0) { steering = steering / (float)numberOfNeighbour; }  //devide the sum of steeringvectors by the number of elements -> separation steeringvector
      return steering;
    }

    //! alignment-function (lead elements to the same heading)
    Vector3 alignment(Element arrayOfElements[]) {
      using namespace Ogre;
      Vector3 steering = Vector3(0,0,0); //steeringvector
      int numberOfNeighbour = 0;  //number of observed neighbours
      float distance = 0;
      //go through all elements
      for(int i=0; i<ANZELEMENTS; i++) {  //just working with 3 elements at the moment
        Element actual = arrayOfElements[i];  //get the actual element
        float distance = getDistance(actual);  //get distance between this and actual
        if ((distance > 0) && (distance < ALIGNMENTDISTANCE)) {  //check if actual element is inside detectionradius
          steering = steering + actual.speed;  //add up all speedvectors inside the detectionradius
          numberOfNeighbour++;  //counts the elements inside the detectionradius
        }
      }
      if(numberOfNeighbour > 0) { steering = steering / (float)numberOfNeighbour; }  //devide the sum of steeringvectors by the number of elements -> alignment steeringvector
      return steering;
    }

    //! cohseion-function (keep elements close to each other)
    Vector3 cohesion(Element arrayOfElements[]) {
      using namespace Ogre;
      Vector3 steering = Vector3(0,0,0); //steeringvector
      int numberOfNeighbour = 0;  //number of observed neighbours
      float distance = 0;
      //go through all elements
      for(int i=0; i<ANZELEMENTS; i++) {  //just working with 3 elements at the moment
        Element actual = arrayOfElements[i];  //get the actual element
        float distance = getDistance(actual);  //get distance between this and actual
        if ((distance > 0) && (distance < COHESIONDISTANCE)) {  //check if actual element is inside detectionradius
          steering = steering + actual.location;  //add up all locations of elements inside the detectionradius
          numberOfNeighbour++;  //counts the elements inside the detectionradius
        }
      }
      if(numberOfNeighbour > 0) {
        steering = steering  / (float)numberOfNeighbour;  //devide the sum steeringvector by the number of elements -> cohesion steeringvector
        steering = steering - this->location;  //transform the vector for the ship
      }
      return steering;
    }
  };     //End of class Element
}

#endif /* _Flocking_H__*/