#ifndef _HPP_GAIA_VECTOR2
#define _HPP_GAIA_VECTOR2

#include <cmath>
#include <cassert>
#include <stdio.h>
#include <stdexcept>

#include "defines.hpp"

namespace gaia {

  class Vector2 {

    public:
      Vector2();
      Vector2(const Vector2 &src) { coords[0] = src.coords[0]; coords[1] = src.coords[1]; }//memcpy(coords, src.coords, 2 * sizeof(float)); }
      //Vector2(const Vector2 &src) { memcpy(coords, src.coords, 2 * sizeof(float)); }
      Vector2(float xy);
      Vector2(float x, float y);
      virtual ~Vector2() {};

      void Set(float xy);
      void Set(float x, float y);
      void Set(const Vector2 &vec);

      // ----- operator overloading
      bool operator == (const Vector2 &vector) const;
      bool operator != (const Vector2 &vector) const;
      void operator = (const float src);
      void operator = (const Vector2 &src);
      Vector2 operator * (const float scalar) const;
      Vector2 operator * (const Vector2 &scalar) const;
      Vector2 &operator *= (const float scalar);
      Vector2 &operator *= (const Vector2 &scalar);
      Vector2 operator / (const float scalar) const;
      Vector2 operator / (const Vector2 &scalar) const;
      Vector2 &operator /= (const Vector2 &scalar);
      Vector2 &operator += (const float scalar);
      Vector2 &operator += (const Vector2 &scalar);
      Vector2 &operator -= (const Vector2 &scalar);
      Vector2 operator + (const Vector2 &vec) const;
      const Vector2 &operator + () const;
      Vector2 operator + (const float value) const;
      Vector2 operator - (const Vector2 &vec) const;
      Vector2 operator - () const;
      Vector2 operator - (const float value) const;
      bool operator < (const Vector2 &vector) const;

      // ----- mathematics
      Vector2 GetCrossProduct(const Vector2 &fac) const;
      float GetDotProduct(const Vector2 &fac) const;
      Vector2 GetReflection(const Vector2 &normal) const;
      void Normalize(const Vector2 &ifNull);
      void Normalize();
      void NormalizeTo(float length);
      void NormalizeMax(float length);
      void NormalizeGradual(float threshold, const Vector2 &ifNull);
      Vector2 GetNormalized(const Vector2 &ifNull) const;
      Vector2 GetNormalized() const;
      Vector2 GetNormalizedTo(float length) const;
      Vector2 GetNormalizedMax(float length) const;
      Vector2 GetNormalizedGradual(float threshold, const Vector2 &ifNull) const;
      float GetDistance(const Vector2 &fac) const;
      float GetLength() const;
      float GetSquaredLength() const;
      radian GetAngle() const;
      // both need to be normalized!
      radian GetAngle(const Vector2 &test) const;
      void Rotate(const radian angle);
      Vector2 GetRotated(const radian angle) const;
      bool Compare(const Vector2 &test) const;
      Vector2 GetAbsolute() const;
      Vector2 EnforceMaximumDeviation(const Vector2 &deviant, float maxDeviation) const;

      // extrapolates a vector by a direction & a time difference in the future (positive) or in the past (negative)
      // assume direction vector to be units per sec, assume time to be in milliseconds
      void Extrapolate(const Vector2 &direction, unsigned long time_ms);

      void Print() const;

      float coords[2];

    protected:

    private:

  };


  // INLINED FUNCTIONS

  inline
  Vector2::Vector2() {
    coords[0] = 0;
    coords[1] = 0;
  }

  inline
  Vector2::Vector2(float xy) {
    coords[0] = xy;
    coords[1] = xy;
  }

  inline
  Vector2::Vector2(float x, float y) {
    coords[0] = x;
    coords[1] = y;
  }

  inline
  void Vector2::Set(float xy) {
    coords[0] = xy;
    coords[1] = xy;
  }

  inline
  void Vector2::Set(float x, float y) {
    coords[0] = x;
    coords[1] = y;
  }

  inline
  void Vector2::Set(const Vector2 &vec) {
    coords[0] = vec.coords[0];
    coords[1] = vec.coords[1];
  }


  // ----- operator overloading

  inline
  bool Vector2::operator == (const Vector2 &vector) const {
    if (coords[0] == vector.coords[0] &&
        coords[1] == vector.coords[1]) {
      return true;
    }
    return false;
  }

  inline
  bool Vector2::operator != (const Vector2 &vector) const {
    if (*this == vector) return false; else return true;
  }

  inline
  void Vector2::operator = (const float src) {
    Set(src);
  }

  inline
  void Vector2::operator = (const Vector2 &src) {
    //memcpy(coords, src.coords, 2 * sizeof(float));
    coords[0] = src.coords[0];
    coords[1] = src.coords[1];
  }

  inline
  Vector2 Vector2::operator * (const float scalar) const {
    return Vector2(coords[0] * scalar, coords[1] * scalar);
  }

  inline
  Vector2 Vector2::operator * (const Vector2 &scalar) const {
    return Vector2(coords[0] * scalar.coords[0], coords[1] * scalar.coords[1]);
  }

  inline
  Vector2 &Vector2::operator *= (const float scalar) {
    coords[0] *= scalar;
    coords[1] *= scalar;
    return *this;
  }

  inline
  Vector2 &Vector2::operator *= (const Vector2 &scalar) {
    coords[0] *= scalar.coords[0];
    coords[1] *= scalar.coords[1];
    return *this;
  }

  inline
  Vector2 Vector2::operator / (const float scalar) const {
    return Vector2(coords[0] / scalar, coords[1] / scalar);
  }

  inline
  Vector2 Vector2::operator / (const Vector2 &scalar) const {
    return Vector2(coords[0] / scalar.coords[0], coords[1] / scalar.coords[1]);
  }

  inline
  Vector2 &Vector2::operator /= (const Vector2 &scalar) {
    coords[0] /= scalar.coords[0];
    coords[1] /= scalar.coords[1];
    return *this;
  }

  inline
  Vector2 &Vector2::operator += (const float scalar) {
    coords[0] += scalar;
    coords[1] += scalar;
    return *this;
  }

  inline
  Vector2 &Vector2::operator += (const Vector2 &scalar) {
    coords[0] += scalar.coords[0];
    coords[1] += scalar.coords[1];
    return *this;
  }

  inline
  Vector2 &Vector2::operator -= (const Vector2 &scalar) {
    coords[0] -= scalar.coords[0];
    coords[1] -= scalar.coords[1];
    return *this;
  }

  inline
  Vector2 Vector2::operator + (const Vector2 &vec) const {
    return Vector2(coords[0] + vec.coords[0], coords[1] + vec.coords[1]);
  }

  inline
  const Vector2 &Vector2::operator + () const {
    return *this;
  }

  inline
  Vector2 Vector2::operator + (const float value) const {
    return Vector2(coords[0] + value, coords[1] + value);
  }

  inline
  Vector2 Vector2::operator - (const Vector2 &vec) const {
    return Vector2(coords[0] - vec.coords[0], coords[1] - vec.coords[1]);
  }

  inline
  Vector2 Vector2::operator - () const {
    return Vector2(-coords[0], -coords[1]);
  }

  inline
  Vector2 Vector2::operator - (const float value) const {
    return Vector2(coords[0] - value, coords[1] - value);
  }

  inline
  bool Vector2::operator < (const Vector2 &vector) const {
    if (coords[0] == vector.coords[0]) {
      return coords[1] < vector.coords[1];
    } else return coords[0] < vector.coords[0];
  }


  // ----- mathematics!!! don't we just love it

  inline
  float Vector2::GetDotProduct(const Vector2 &fac) const {
    return (coords[0] * fac.coords[0] + coords[1] * fac.coords[1]);
  }

  inline
  Vector2 Vector2::GetReflection(const Vector2 &normal) const {
    // I - 2.0 * dot(N, I) * N
    return (*this) - (normal * GetDotProduct(normal) * 2.0f);
  }

  inline
  void Vector2::Normalize(const Vector2 &ifNull) {
    if (fabs(this->coords[0]) < 0.000001f && fabs(this->coords[1]) < 0.000001f) {
      this->coords[0] = ifNull.coords[0];
      this->coords[1] = ifNull.coords[1];
    } else {
      float f = 1.0f / sqrt(GetDotProduct(*this));

      coords[0] *= f;
      coords[1] *= f;
    }
  }

  inline
  void Vector2::Normalize() {
    if (fabs(this->coords[0]) < 0.000001f && fabs(this->coords[1]) < 0.000001f) {
      //printf("[Vector2::Normalize] Trying to normalize 0-vector");
      int *foo = (int*)-1; // make a bad pointer
      printf("%d\n", *foo); // causes segfault
      throw std::range_error("[Vector2::Normalize] Trying to normalize 0-vector");
    }
    float f = 1.0f / sqrt(GetDotProduct(*this));

    coords[0] *= f;
    coords[1] *= f;
  }

  inline
  void Vector2::NormalizeTo(float length) {
    if (fabs(this->coords[0]) < 0.000001f && fabs(this->coords[1]) < 0.000001f) {
      //printf("[Vector2::NormalizeTo] Trying to normalize 0-vector");
      int *foo = (int*)-1; // make a bad pointer
      printf("%d\n", *foo); // causes segfault
      throw std::range_error("[Vector2::Normalize] Trying to normalize 0-vector");
    }
    float f = length / sqrt(GetDotProduct(*this));

    coords[0] *= f;
    coords[1] *= f;
  }

  inline
  void Vector2::NormalizeMax(float length) {
    float thisLength = GetLength();
    if (thisLength > length) {// && thisLength > 0.000001f) {
      Normalize();
      *this *= length;
    }
  }

  inline
  void Vector2::NormalizeGradual(float threshold, const Vector2 &ifNull) {
    if (GetLength() < threshold) {
      float bias = GetLength() / threshold;
      *this = ifNull * (1.0f - bias) + GetNormalized(0) * bias;
      Normalize(ifNull);
    } else Normalize();
  }


  inline
  Vector2 Vector2::GetNormalized(const Vector2 &ifNull) const {
    Vector2 tmp(*this);
    //printf("huuuu: "); Print(); tmp.Print();
    //Vector2 tmp(coords[0], coords[1]);
    tmp.Normalize(ifNull);
    return tmp;
  }

  inline
  Vector2 Vector2::GetNormalized() const {
    if (fabs(this->coords[0]) < 0.000001f && fabs(this->coords[1]) < 0.000001f) {
      //printf("[Vector2::GetNormalized] Trying to normalize 0-vector");
      int *foo = (int*)-1; // make a bad pointer
      printf("%d\n", *foo); // causes segfault
      throw std::range_error("[Vector2::Normalize] Trying to normalize 0-vector");
    }
    Vector2 tmp(*this);
    //Vector2 tmp(coords[0], coords[1]);
    tmp.Normalize();
    return tmp;
  }

  inline
  Vector2 Vector2::GetNormalizedTo(float length) const {
    if (fabs(this->coords[0]) < 0.000001f && fabs(this->coords[1]) < 0.000001f) {
      //printf("[Vector2::GetNormalizedTo] Trying to normalize 0-vector");
      int *foo = (int*)-1; // make a bad pointer
      printf("%d\n", *foo); // causes segfault
      throw std::range_error("[Vector2::Normalize] Trying to normalize 0-vector");
    }
    Vector2 tmp(*this);
    //Vector2 tmp(coords[0], coords[1]);
    tmp.NormalizeTo(length);
    return tmp;
  }

  inline
  Vector2 Vector2::GetNormalizedMax(float length) const {
    if (GetLength() > length) return GetNormalized() * length; else return Vector2(*this);
  }

  inline
  Vector2 Vector2::GetNormalizedGradual(float threshold, const Vector2 &ifNull) const {
    Vector2 tmp(*this);
    tmp.NormalizeGradual(threshold, ifNull);
    return tmp;
  }

  inline
  float Vector2::GetDistance(const Vector2 &fac) const {
    float v3[2];
    v3[0] = coords[0] - fac.coords[0];
    v3[1] = coords[1] - fac.coords[1];

    // premature optimization ;)
    if (v3[0] == 0.0f && v3[1] == 0.0f) return 0.0f;

    float length = sqrt(pow(v3[0], 2) + pow(v3[1], 2));

    if (length < 0.000001) length = 0;
    return length;
  }

  inline
  float Vector2::GetLength() const {
    // premature optimization ;)
    if (this->coords[0] == 0.0f && this->coords[1] == 0.0f) return 0.0f;

    float length = sqrt(pow(coords[0], 2) + pow(coords[1], 2));

    if (length < 0.000001f) length = 0;
    return length;
  }

  inline
  float Vector2::GetSquaredLength() const {
    return pow(coords[0], 2) + pow(coords[1], 2);
  }

  inline
  radian Vector2::GetAngle() const {
    float angle = atan2(coords[1], coords[0]);
    if (angle < 0) angle += 2 * pi;
    return angle;
  }

  inline
  radian Vector2::GetAngle(const Vector2 &test) const {
    radian angle = -atan2(coords[0] * test.coords[1] - coords[1] * test.coords[0], coords[0] * test.coords[0] + coords[1] * test.coords[1]);
    //angle = ModulateIntoRange(-pi, pi, angle);
    return angle;
  }

  inline
  void Vector2::Rotate(const radian angle) {
    float x = (coords[0] * cos(angle)) - (coords[1] * sin(angle));
    float y = (coords[1] * cos(angle)) + (coords[0] * sin(angle));
    coords[0] = x;
    coords[1] = y;
  }

  inline
  Vector2 Vector2::GetRotated(const radian angle) const {
    Vector2 result;
    float x = (coords[0] * cos(angle)) - (coords[1] * sin(angle));
    float y = (coords[1] * cos(angle)) + (coords[0] * sin(angle));
    result.coords[0] = x;
    result.coords[1] = y;
    return result;
  }

  inline
  bool Vector2::Compare(const Vector2 &test) const {
    if (test.coords[0] == coords[0] &&
        test.coords[1] == coords[1]) {
      return true;
    } else {
      return false;
    }
  }

  inline
  Vector2 Vector2::GetAbsolute() const {
    return Vector2(fabsf(coords[0]), fabsf(coords[1]));
  }

  inline
  Vector2 Vector2::EnforceMaximumDeviation(const Vector2 &deviant, float maxDeviation) const {
    Vector2 result = *this;
    Vector2 difference = deviant - *this;
    float differenceDistance = difference.GetLength();
    if (differenceDistance > maxDeviation) {
      result += difference.GetNormalized() * (differenceDistance - maxDeviation);
    }
    if (maxDeviation == 0.0f) assert(deviant.GetDistance(result) < 0.001f);
    return result;
  }

  inline
  void Vector2::Extrapolate(const Vector2 &direction, unsigned long time_ms) {
    *this += direction * (time_ms / 1000.0f);
  }

  inline
  void Vector2::Print() const {
    printf("%f, %f\n", coords[0], coords[1]);
  }



}

#endif
