#pragma once
#include <iostream>
#include <sstream>
#include <cmath>

namespace geom {

struct Vector3f {
    float x, y, z;

    Vector3f(float x = 0.0f, float y = 0.0f, float z = 0.0f)
    : x(x), y(y), z(z)
    { }

    Vector3f(Vector3f const &other)
    : x(other.x), y(other.y), z(other.z)
    { }

    void set(Vector3f const &other) {
        this->x = other.x;
        this->y = other.y;
        this->z = other.z;
    }

    void set(const float x, const float y, const float z)
    {
        this->x = x;
        this->y = y;
        this->z = z;
    }

    void set(const float a)
    {
        this->x = a;
        this->y = a;
        this->z = a;
    }

    void setToZero() {
        x = y = z = 0.0f;
    }

    bool isZero()
    {
        return x == 0.0f && y == 0.0f && z == 0.0f;
    }
    bool isZero(float epsilon) {
        float epsSquare = epsilon*epsilon;
        return x * x <= epsSquare && y * y <= epsSquare && z * z <= epsSquare;
    }

    void setToAbsoluteValues() {
        if (x < 0.0f) x = -x;
        if (y < 0.0f) y = -y;
        if (z < 0.0f) z = -z;
    }
    void add(Vector3f const &other) {
        x += other.x;
        y += other.y;
        z += other.z;
    }

    void add(const float ax, const float ay, const float az) {
        x += ax;
        y += ay;
        z += az;
    }

    void add(float a) {
        x += a;
        y += a;
        z += a;
    }

    void subtract(Vector3f const &other) {
        x -= other.x;
        y -= other.y;
        z -= other.z;
    }

    void subtract(const float ax, const float ay, const float az) {
        x -= ax;
        y -= ay;
        z -= az;
    }

    void subtract(float a) {
        x -= a;
        y -= a;
        z -= a;
    }

    void scale(const float scale) {
        this->x *= scale;
        this->y *= scale;
        this->z *= scale;
    }

    void scale(const float sx, const float sy, const float sz) {
        this->x *= sx;
        this->y *= sy;
        this->z *= sz;
    }

    void scaleAdd(const float scale, Vector3f const &other) {
        x += scale*other.x;
        y += scale*other.y;
        z += scale*other.z;
    }

    void scaleAdd(const double scale, Vector3f const &other) {
        x = (float)(x + scale*other.x);
        y = (float)(y + scale*other.y);
        z = (float)(z + scale*other.z);
    }

    void divide(const float divisor) {
        this->x /= divisor;
        this->y /= divisor;
        this->z /= divisor;
    }

    /**
     * scalar product
     */
    float dot(Vector3f const &other) const
    {
        return x * other.x + y * other.y + z * other.z;
    }

    /**
     * cross product stored in this vector
     */
    void crossWith(Vector3f const &other)
    {
        float nx = y * other.z - z * other.y;
        float ny = z * other.x - x * other.z;
        float nz = x * other.y - y * other.x;
        x = nx;
        y = ny;
        z = nz;
    }

    bool normalize() {
        float l = length();
        if (l == 0.0f) return false;
        x /= l; y /= l; z /= l;
        return true;
    }
    bool normalizeDoublePrececision() {
        double l = std::sqrt(x * (double)x  +  y * (double)y  +  z * (double)z);
        if (l == 0.0) return false;
        x = (float)(x/l); y = (float)(y/l); z = (float)(z/l);
        return true;
    }

    bool normalizeFastInaccurateIEEE() {
        float x = lengthSquared();
        float lhalf = 0.5f*x;
        union
        {
             float l;
             int i;
        } u;
        u.l = x;
        u.i = 0x5f3759df - (u.i >> 1);
        u.l = u.l * (1.5f - lhalf * u.l * u.l); /* This line can be repeated arbitrarily many times to increase accuracy */
        u.l = u.l * (1.5f - lhalf * u.l * u.l);

        if (u.l==0.0f) return false;

        x /= u.l;   y /= u.l; z /= u.l;

        return true;
    }

    float lengthSquared() const {
        return x * x  +  y * y  +  z * z;
    }
    float length() const {
        return std::sqrt(lengthSquared());
    }

    bool setLength(float newLength) {
        float l = length();
        if (l == 0.0f) return false;
        x = (x/l)*newLength; y = (y/l)*newLength; z = (z/l)*newLength;
        return true;
    }

    /**
     * inverts the components so that invert[v(x,y,z)] = v(1/x,1/y,1/z)
     */
    void invert()
    {
        x = 1.0f/x;
        y = 1.0f/y;
        z = 1.0f/z;
    }

    float distanceSquared(Vector3f const &other) const {
        float dx = other.x - x;
        float dy = other.y - y;
        float dz = other.z - z;
        return dx * dx + dy * dy + dz * dz;
    }
    float distance(Vector3f const &other) const {
        return std::sqrt(distanceSquared(other));
    }

    float getMaxComponent() const {
        return x > y ? (x > z ? x : z) : (y > z ? y : z);
    }

    float getMinComponent() const {
        return x < y ? (x < z ? x : z) : (y < z ? y : z);
    }
    void rotateAroundZ(float angle) {
        float nx;
        nx = x*cosf(angle) - y*sinf(angle);
        y  = y*sinf(angle) + y*cosf(angle);
        x = nx;
    }

    void rotateAroundY(float angle) {
        float nx;
        nx = x*cosf(angle) + z*sinf(angle);
        z  =-x*sinf(angle) + z*cosf(angle);
        x = nx;
    }

    void rotateAroundX(float angle) {
        float ny;
        ny = y*cosf(angle) - z*sinf(angle);
        z  = y*sinf(angle) + z*cosf(angle);
        y = ny;
    }

    /*
    Vector3f const& operator = (Vector3f const & other) const
    {
          set(other);
          return *this;
    }
    */
    Vector3f operator + (Vector3f const & other) const
    {
          return Vector3f(x + other.x, y + other.y, z + other.z);
    }

    Vector3f operator + (float offset) const
    {
          return Vector3f(x + offset, y + offset, z + offset);
    }

    Vector3f operator - (Vector3f const & other) const
    {
          return Vector3f(x - other.x, y - other.y, z - other.z);
    }

    Vector3f operator - (float offset) const
    {
          return Vector3f(x - offset, y - offset, z - offset);
    }

    Vector3f operator * (float scale) const
    {
          return Vector3f(x * scale, y * scale, z * scale);
    }

    Vector3f operator / (float scale) const
    {
        return Vector3f(x / scale, y / scale, z / scale);
    }

    /**
     * cross product
     */
    Vector3f operator *(Vector3f const &other) const
    {
        return Vector3f(
            y * other.z - z * other.y,
            z * other.x - x * other.z,
            x * other.y - y * other.x
        );
    }

    bool operator == (Vector3f const &b) const {
        return x == b.x && y == b.y && z == b.z;
    }

    bool operator != (Vector3f const &b) const
    {
        return x != b.x || y != b.y || z != b.z;
    }

    bool operator < (Vector3f const &b) const
    {
        return lengthSquared() < b.lengthSquared();
    }
    bool operator <= (Vector3f const &b) const
    {
        return lengthSquared() <= b.lengthSquared();
    }
    bool operator > (Vector3f const &b) const
    {
        return lengthSquared() > b.lengthSquared();
    }
    bool operator >= (Vector3f const &b) const
    {
        return lengthSquared() >= b.lengthSquared();
    }

    float& operator[] (const unsigned int i) {
        switch (i) {
            case 0: return x;
            case 1: return y;
            default: return z;
        }
    }

    std::string toString() const {
        std::stringstream str;
        str << "("<<x<<"; "<<y<<"; "<<z<<")";
        return str.str();
    }

    friend std::ostream& operator<<(std::ostream& out, const Vector3f& vec) // output
    {
        out << "("<<vec.x<<"; "<<vec.y<<"; "<<vec.z<<")";
        return out;
    }
};

}