#ifndef __VECTOR3_H__
#define __VECTOR3_H__

#include <iostream>
#include <cassert>
#include <cmath>

#include "public.h"


namespace xrt {


// The Vector3 class is for generic 3-component spatial values.
//
// We GUARANTEE that the memory layout and size of a Vector3 is
// identical to a generic float[3], and thus it's presumed relatively
// safe to cast between a (float *) and a (Vector3 *).
//
// Semantics are as expected: construction from one or three floats or
// a float pointer (presumed to point to 3 floats), [] for accessing
// individual components, the usual arithmetic (+, -, *, /).  Note
// that math ops between two vectors are always component-by-component.
// 
// There are a handful of useful friends -- routines for normalizing,
// taking distance between two points, dot and cross products, etc.
//
// It's up to the user to keep track of whether they represent
// points, vectors, or normals, and to only use the operators where
// their semantics make sense.
//
// See also the Matrix4 class for other handy friends.

class HPoint3;

class PUBLIC Vector3 {

public:

    // Constructors
    Vector3 (void) { }
    explicit Vector3 (float x) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] = x;
#else
        v[0] = x; v[1] = x; v[2] = x;
#endif
    }
    Vector3 (float x, float y, float z) {
	v[0] = x; v[1] = y; v[2] = z;
    }
    explicit Vector3 (const float *xyz) {
	assert (xyz != NULL);
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] = xyz[i];
#else
	v[0] = xyz[0]; v[1] = xyz[1]; v[2] = xyz[2];
#endif
    }
	explicit Vector3 (const HPoint3& p);

	// Direct access to the data -- use with caution
    const float *data(void) const { return v; }

    // Simple assignment
    const Vector3& operator= (float x) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] = x;
#else
	v[0] = x; v[1] = x; v[2] = x;
#endif
	return *this;
    }
    // Use the default assignment operator (bitwise copy)


    const Vector3& operator= (const HPoint3& p);

    // Component access
    float operator[] (int i) const {
	assert(i>=0 && i < 3);  // range check -- only in DEBUG mode
	return v[i];
    }
    float& operator[] (int i) {
	assert(i>=0 && i < 3);  // range check -- only in DEBUG mode
	return v[i];
    }


    // Comparisons between points, and with floats
    friend bool operator== (const Vector3 &a, const Vector3 &b) {
	return a[0] == b[0] && a[1] == b[1] && a[2] == b[2];
    }
    friend bool operator!= (const Vector3 &a, const Vector3 &b) {
	return a[0] != b[0] || a[1] != b[1] || a[2] != b[2];
    }
    friend bool operator== (const Vector3 &a, float b) {
	return a[0] == b && a[1] == b && a[2] == b;
    }
    friend bool operator!= (const Vector3 &a, float b) {
	return a[0] != b || a[1] != b || a[2] != b;
    }
    friend bool operator== (float b, const Vector3 &a) { return (a==b); }
    friend bool operator!= (float b, const Vector3 &a) { return (a!=b); }

    // less-than operator: this isn't mathematically meaningful,
    // but lets you use certain STL classes.
    friend bool operator< (const Vector3 &a, const Vector3 &b) {
	if (a[0] < b[0]) return true;
	else if (a[0] > b[0]) return false;
	if (a[1] < b[1]) return true;
	else if (a[1] > b[1]) return false;
	if (a[2] < b[2]) return true;
	else return false;
    }

    // Stream output -- very handy for debugging
    friend std::ostream& operator<< (std::ostream& out, const Vector3& a) {
	return (out << '[' << a[0] << ' ' << a[1] << ' ' << a[2] << ']');
    }


    //////////
    // The obvious vector operations:
    // 

    // Addition
    friend Vector3 operator+ (const Vector3& a, const Vector3& b) {
#ifdef VECMAT_LOOPS
        Vector3 r;
        for (int i = 0;  i < 3;  ++i)
            r[i] = a[i] + b[i];
        return r;
#else
	return Vector3(a.v[0]+b.v[0], a.v[1]+b.v[1], a.v[2]+b.v[2]);
#endif
    }
    const Vector3& operator+= (const Vector3& b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] += b.v[i];
#else
	v[0] += b.v[0];  v[1] += b.v[1];  v[2] += b.v[2];
#endif
	return *this;
    }
	friend Vector3 operator+ (const Vector3& a, float b) {
	return Vector3(a.v[0]+b, a.v[1]+b, a.v[2]+b);
    }
    friend Vector3 operator+ (float b, const Vector3& a) {
	return Vector3(a.v[0]+b, a.v[1]+b, a.v[2]+b);
    }
    const Vector3& operator+= (float b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] += b;
#else
	v[0] += b;  v[1] += b;  v[2] += b;
#endif
	return *this;
    }
    // Binary addition (a+b) that is guaranteed to have no extra copies
    friend void add (Vector3 &r, const Vector3& a, const Vector3& b) {
        r[0] = a[0] + b[0];
        r[1] = a[1] + b[1];
        r[2] = a[2] + b[2];
    }
    // Ternary addition (a+b+c) that is guaranteed to have no extra copies
    friend void add (Vector3 &r, const Vector3& a,
                     const Vector3& b, const Vector3& c) {
        r[0] = a[0] + b[0] + c[0];
        r[1] = a[1] + b[1] + c[1];
        r[2] = a[2] + b[2] + c[2];
    }

    // Subtraction
    friend Vector3 operator- (const Vector3& a, const Vector3& b) {
#ifdef VECMAT_LOOPS
        Vector3 r;
        for (int i = 0;  i < 3;  ++i)
            r[i] = a[i] - b[i];
        return r;
#else
	return Vector3(a.v[0]-b.v[0], a.v[1]-b.v[1], a.v[2]-b.v[2]);
#endif
    }
    const Vector3& operator-= (const Vector3& b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] -= b.v[i];
#else
	v[0] -= b.v[0];  v[1] -= b.v[1];  v[2] -= b.v[2];
#endif
	return *this;
    }
	friend Vector3 operator- (const Vector3& a, float b) {
	return Vector3(a.v[0]-b, a.v[1]-b, a.v[2]-b);
    }
    friend Vector3 operator- (float a, const Vector3& b) {
	return Vector3(a-b.v[0], a-b.v[1], a-b.v[2]);
    }
    const Vector3& operator-= (float b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] -= b;
#else
	v[0] -= b;  v[1] -= b;  v[2] -= b;
#endif
	return *this;
    }
    // Binary subtraction (a-b) that is guaranteed to have no extra copies
    friend void sub (Vector3 &r, const Vector3& a, const Vector3& b) {
        r[0] = a[0] - b[0];
        r[1] = a[1] - b[1];
        r[2] = a[2] - b[2];
    }
    // Ternary subtraction (a-b-c) that is guaranteed to have no extra copies
    friend void sub (Vector3 &r, const Vector3& a,
                     const Vector3& b, const Vector3& c) {
        r[0] = a[0] - b[0] - c[0];
        r[1] = a[1] - b[1] - c[1];
        r[2] = a[2] - b[2] - c[2];
    }

    // Negation
    friend Vector3 operator- (const Vector3& b) {
	return Vector3(-b.v[0], -b.v[1], -b.v[2]);
    }

    // Scalar multiplication
    friend Vector3 operator* (const Vector3& a, float b) {
	return Vector3(a.v[0]*b, a.v[1]*b, a.v[2]*b);
    }
    friend Vector3 operator* (float b, const Vector3& a) {
	return Vector3(a.v[0]*b, a.v[1]*b, a.v[2]*b);
    }
    const Vector3& operator*= (float b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] *= b;
#else
	v[0] *= b;  v[1] *= b;  v[2] *= b;
#endif
	return *this;
    }

    // Component-by-component multiplication
    friend Vector3 operator* (const Vector3& a, const Vector3& b) {
#ifdef VECMAT_LOOPS
        Vector3 r;
        for (int i = 0;  i < 3;  ++i)
            r[i] = a[i] * b[i];
        return r;
#else
	return Vector3(a.v[0]*b.v[0], a.v[1]*b.v[1], a.v[2]*b.v[2]);
#endif
    }
    const Vector3& operator*= (const Vector3& b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] *= b.v[i];
#else
	v[0] *= b.v[0];  v[1] *= b.v[1];  v[2] *= b.v[2];
#endif
	return *this;
    }

    // Scalar division -- watch out for divide-by-zero
    friend Vector3 operator/ (const Vector3& a, float b) {
	if (b == 0.0f)
	    return Vector3(0.0f);
	else {
            float binv = 1.0f/b;
	    return Vector3(a.v[0]*binv, a.v[1]*binv, a.v[2]*binv);
        }
    }
    friend Vector3 operator/ (float a, const Vector3& b) {
        Vector3 dst;
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            if (b[i] == 0.0f) dst[i] = 0.0f;
            else dst[i] = a / b[i];
#else
        if (b[0] == 0.0f) dst[0] = 0;
        else dst[0] = a / b[0];
        if (b[1] == 0.0f) dst[1] = 0;
        else dst[1] = a / b[1];
        if (b[2] == 0.0f) dst[2] = 0;
        else dst[2] = a / b[2];
#endif
        return dst;
    }
    const Vector3& operator/= (float b) {
	if (b == 0.0f)
	    *this = 0.0f;
	else {
            float binv = 1.0f/b;
#ifdef VECMAT_LOOPS
            for (int i = 0;  i < 3;  ++i)
                v[i] *= binv;
#else
	    v[0] *= binv;  v[1] *= binv;  v[2] *= binv;
#endif
	}
	return *this;
    }

    // Component-by-component division -- watch out for divide-by-zero
    friend Vector3 operator/ (const Vector3& a, const Vector3& b) {
	return Vector3((b.v[0] == 0.0f) ? 0.0f : a.v[0]/b.v[0],
		      (b.v[1] == 0.0f) ? 0.0f : a.v[1]/b.v[1],
		      (b.v[2] == 0.0f) ? 0.0f : a.v[2]/b.v[2]);
    }
    const Vector3& operator/= (const Vector3& b) {
#ifdef VECMAT_LOOPS
	for (int i = 0;  i < 3;  ++i)
            v[i] = (b.v[i] == 0.0f) ? 0.0f : v[i]/b.v[i];
#else
	v[0] = (b.v[0] == 0.0f) ? 0.0f : v[0]/b.v[0];
	v[1] = (b.v[1] == 0.0f) ? 0.0f : v[1]/b.v[1];
	v[2] = (b.v[2] == 0.0f) ? 0.0f : v[2]/b.v[2];
#endif
	return *this;
    }

    // a += b*c, but faster because it avoids any extra temporary copies
    friend void mad (Vector3 &a, float b, const Vector3& c) {
#ifdef VECMAT_LOOPS
        for (int i = 0;  i < 3;  ++i)
            a[i] += b * c[i];
#else
        a[0] += b * c[0];
        a[1] += b * c[1];
        a[2] += b * c[2];
#endif
    }
    // Cross product
    friend Vector3 cross (const Vector3& a, const Vector3& b) {
	return Vector3 (a.v[1]*b.v[2] - b.v[1]*a.v[2],
		       a.v[2]*b.v[0] - b.v[2]*a.v[0],
		       a.v[0]*b.v[1] - b.v[0]*a.v[1]);
    }

 	friend Vector3 operator^ (const Vector3& a, const Vector3& b) {
		return cross (a, b);
	}

    friend Vector3 bilerp (const Vector3& a, const Vector3& b, 
                           const Vector3& c, const Vector3& d,
                           float s, float t)
    {
	float s1 = 1.0f - s;
	float t1 = 1.0f - t;
        Vector3 r;
        for (int i = 0;  i < 3;  ++i)
            r[i] = t1*(s1*a[i] + s*b[i]) + t*(s1*c[i] + s*d[i]);
        return r;
    }

    // Return the Euclidean length
    float squaredLength () const {
    return v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
	}
    float length (void) const {
    return sqrtf (squaredLength());
	}
    friend float length (const Vector3& a) {
	return sqrtf (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
    }
    friend float squaredLength (const Vector3& a) {
	return (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
    }

    // Member normalize alters the Vector3, returns original length
    float normalize (void) {
	float len = this->length();
	if (len != 0.0f) {
#ifdef VECMAT_LOOPS
            for (int i = 0;  i < 3;  ++i)
                v[i] /= len;
#else
	    v[0] /= len; v[1] /= len; v[2] /= len;
#endif
	}
	return len;
    }
    // Functional normalize returns a normalized Vector3, leaves alone original
    //friend Vector3 normalize (const Vector3& P);

    // Return the Euclidean distance between two points
    // NOTE: must be 'dist' since 'distance' is something else in STL
    friend float distSquared (const Vector3 &a, const Vector3 &b) {
#ifdef VECMAT_LOOPS
        float x[3];
        for (int i = 0;  i < 3;  ++i) {
            x[i] = a[i] - b[i];
            x[i] *= x[i];
        }
        return x[0] + x[1] + x[2];
#else
	float x = a[0] - b[0];
	float y = a[1] - b[1];
	float z = a[2] - b[2];
	return (x*x + y*y + z*z);
#endif
    }
    friend float dist (const Vector3& a, const Vector3 &b) {
	return sqrtf (distSquared (a, b));
    }

    // Make two unit vectors that are orthogonal to yourself and each
    // other.  This assumes that *this is already normalized.  We get
    // the first orthonormal by taking the cross product of *this and
    // (1,1,1), unless *this is 1,1,1, in which case we cross with
    // (-1,1,1).  Either way, we get something orthogonal.  Then
    // cross(this,a) is mutually orthogonal to the other two.
    void makeOrthonormals (Vector3 &a, Vector3 &b) const {
        if (v[0] != v[1] || v[0] != v[2])
            a = Vector3 (v[2]-v[1], v[0]-v[2], v[1]-v[0]);  // (1,1,1)X(this)
        else
            a = Vector3 (v[2]-v[1], v[0]+v[2], -v[1]-v[0]);  // (-1,1,1)X(this)
        a.normalize ();
        b = cross (*this, a);
    }

    int longestAxis (void) const {
        return v[0] > v[1] ? (v[0] > v[2] ? 0 : 2) : (v[1] > v[2] ? 1 : 2);
    }

    bool iszero (float eps = 1.0e-6) const {
        return fabsf(v[0]) <= eps && fabsf(v[1]) <= eps && fabsf(v[2]) <= eps;
    }

public:
    union {
		float v[3];
		struct { float x, y, z; };
	};

};


// Dot product
__inline float dot (const Vector3& a, const Vector3& b) {
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

// Functional normalize returns a normalized Vector4, leaves alone original
__inline Vector3 normalize (const Vector3& P) {
float len = P.length();
return (len == 0.0f) ? Vector3(0.0f)
	                    : Vector3(P[0]/len, P[1]/len, P[2]/len);
}


};  /* end namespace xrt */


#endif  /* !defined(__VECTOR3_H__) */