#ifndef __MATHLIB_H
#define __MATHLIB_H

#include <iostream>
#include <cmath>
#include <assert.h>

#ifdef WIN32

#define M_PI 3.14159265
void sincos (float rad, double *s, double *c);

#endif

#define MATHLIB_INLINE

struct vector3d
{
	float values[3];

	vector3d ();
	vector3d (float u, float v, float w);
	vector3d (const vector3d &vec);
	vector3d operator= (const vector3d &vec);
	~vector3d ();

	// Setter and getter
	inline void setValues (float u, float v, float w) {
		values[0] = u;
		values[1] = v;
		values[2] = w;
	}

	inline float& operator[] (const int index) {
		assert (index >= 0);
		assert (index <= 3);

		return values[index];
	}

	vector3d operator* (const float &f);
	vector3d operator/ (const float &f);
	void operator*= (const float &f);
	void operator/= (const float &f);

	float operator* (const vector3d &vec);
	vector3d operator+ (const vector3d &vec);
	vector3d operator- (const vector3d &vec);
	void operator+= (const vector3d &vec);
	void operator-= (const vector3d &vec);
	bool operator== (const vector3d &vec);
	bool operator!= (const vector3d &vec);
	vector3d cross (const vector3d &vec);

	float length2 ();
	float length ();
	float get_angle () {
		return atan2 (values[1], values[0]);
	}

	vector3d &rotate_x (float angle);
	vector3d &rotate_y (float angle);
	vector3d &rotate_z (float angle);
	vector3d &normalize ();

	void print ();
	void print (const char* str);
};

#ifdef MATHLIB_INLINE

inline
vector3d vector3d::operator* (const float &f)
{
  return vector3d (values[0] * f, values[1] * f, values[2] * f);
}

inline
vector3d vector3d::operator/ (const float &f)
{
  return vector3d (values[0] / f, values[1] / f, values[2] / f);
}

inline
void vector3d::operator*= (const float &f)
{
	values[0] *= f;
	values[1] *= f;
	values[2] *= f;
}

inline
void vector3d::operator/= (const float &f)
{
	values[0] /= f;
	values[1] /= f;
	values[2] /= f;
}

inline
float vector3d::operator* (const vector3d &vec)
{
  return values[0] * vec.values[0] + values[1] * vec.values[1] + values[2] * vec.values[2];
}

inline
vector3d vector3d::operator+ (const vector3d &vec)
{
  return vector3d (values[0] + vec.values[0], values[1] + vec.values[1], values[2] + vec.values[2]);
}

inline
vector3d vector3d::operator- (const vector3d &vec)
{
  return vector3d (values[0] - vec.values[0], values[1] - vec.values[1], values[2] - vec.values[2]);
}

inline
void vector3d::operator+= (const vector3d &vec)
{
  values[0] += vec.values[0];
  values[1] += vec.values[1];
  values[2] += vec.values[2];
}

inline
void vector3d::operator-= (const vector3d &vec)
{
  values[0] -= vec.values[0];
  values[1] -= vec.values[1];
  values[2] -= vec.values[2];
}

inline
bool vector3d::operator== (const vector3d &vec)
{
  if ( (values[0]==vec.values[0]) && (values[1]==vec.values[1]) && (values[2]==vec.values[2]) )
    return true;
  return false;
}

inline
bool vector3d::operator!= (const vector3d &vec)
{
	return ! (operator==(vec));
}

inline
vector3d vector3d::cross (const vector3d &vec)
{
  return vector3d (values[1] * vec.values[2] - values[2] * vec.values[1],
      values[2] * vec.values[0] - values[0] * vec.values[2],
      values[0] * vec.values[1] - values[1] * vec.values[0]);
}

inline
float vector3d::length2 ()
{
  return values[0]*values[0] + values[1]*values[1] + values[2]*values[2];
}

inline
float vector3d::length ()
{
  return sqrt (values[0]*values[0] + values[1]*values[1] + values[2]*values[2]);
}
#endif

vector3d cross_product (vector3d &a, vector3d &b);

inline bool point_within (vector3d *a, vector3d *b, vector3d *point);

vector3d integrate_rk45 (float t,vector3d& ydot, vector3d& y);

vector3d integrate (float t, vector3d& ydot, vector3d& y);

int solve_quadratic (float a, float b, float c, float *x1, float *x2);

#endif /* __MATHLIB_H */