#include <math.h>
#include <a8/a8.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/vec2.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <a8/vec2.h>

namespace a8
{
    const Vec2 Vec2::UP = Vec2(0.0f, 1.0f);
    const Vec2 Vec2::RIGHT = Vec2(1.0f, 0.0f);
    const Vec2 Vec2::DOWN = Vec2(0.0f, -1.0f);
    const Vec2 Vec2::LEFT = Vec2(-1.0f, 0.0f);

    void Vec2::Normalize()
    {
        glm::vec2 v = glm::normalize(glm::vec2(x, y));
        if (isnan(v[0])) {
            abort();
        }
        if (isnan(v[1])) {
            abort();
        }
        x = v[0];
        y = v[1];
    }

    bool Vec2::operator == (const Vec2& b) const
    {
        return std::abs(x - b.x) < 0.01f && std::abs(y - b.y) < 0.01f;
    }

    Vec2 Vec2::operator + (const Vec2& b) const
    {
        glm::vec2 v = glm::vec2(x, y) + glm::vec2(b.x, b.y);
        return Vec2(v[0], v[1]);
    }

    Vec2 Vec2::operator - (const Vec2& b) const
    {
        glm::vec2 v = glm::vec2(x, y) - glm::vec2(b.x, b.y);
        return Vec2(v[0], v[1]);
    }

    Vec2 Vec2::operator * (float scale) const
    {
        glm::vec2 v = glm::vec2(x, y) * scale;
        return Vec2(v[0], v[1]);
    }

    Vec2 Vec2::operator / (float scale) const
    {
        glm::vec2 v = glm::vec2(x, y) / scale;
        return Vec2(v[0], v[1]);
    }

    void Vec2::Rotate(float angle)
    {
        Eigen::Vector3f v(x, y, 0);
        v = Eigen::AngleAxisf(angle * 3.1415926f, Eigen::Vector3f::UnitZ()) * v;
        x = v[0];
        y = v[1];
    }

    float Vec2::CalcAngle(const Vec2& b) const
    {
        float a1 = acos(Dot(b) / Norm() / b.Norm());
        bool at_right_side = Vec2::RIGHT.Dot(*this) > 0.0001f;
        if (at_right_side) {
            a1 = -a1;
        }
        return a1 / 3.1415926f;
    }

    float Vec2::CalcAngleEx(const Vec2& b) const
    {
        float a1 = acos(Dot(b) / Norm() / b.Norm());
        return a1 / 3.1415926f;
    }

    Vec2 Vec2::FromAngle(float angle)
    {
        Vec2 vec2;
        float hu = angle * 3.1415926f / 180.0f;
        vec2.x = cos(hu);
        vec2.y = sin(hu);
        vec2.Normalize();
        return vec2;
    }

    float Vec2::Distance(const Vec2& b) const
    {
        Vec2 v = b - *this;
        return v.Norm();
    }

    float Vec2::ManhattanDistance(const Vec2& b)
    {
        float distance = std::fabs(x - b.x) + std::fabs(y - b.y);
        return distance;
    }

    Vec2 Vec2::Perp()
    {
        return Vec2(y, -x);
    }

    float Vec2::Dot(const Vec2& v) const
    {
        return x*v.x + y*v.y;
    }

    float Vec2::Norm() const
    {
        return fabs(sqrt(x*x + y*y));
    }

    bool Vec2::IsZero() const
    {
        return fabs(x) < 0.00001f && fabs(y) < 0.00001f;
    }

}