#include <iostream>
#include <sstream>
#include <math.h>
#include <assert.h>

#include "mathlib.h"

using namespace std;

#ifdef WIN32

void sincos (float rad, double *s, double *c){
	*s = sin (rad);
	*c = cos (rad);
}

#endif

vector3d::vector3d ()
{
	values[0] = 0.;
	values[1] = 0.;
	values[2] = 0.;
}

vector3d::vector3d (float u, float v, float w)
{
	values[0] = u;
	values[1] = v;
	values[2] = w;
}

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

vector3d vector3d::operator=(const vector3d &vec) {
	if (this != &vec) {
		values[0] = vec.values[0];
		values[1] = vec.values[1];
		values[2] = vec.values[2];
	}
	return *this;
}

vector3d::~vector3d ()
{
}

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

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

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

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

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

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

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

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

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

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;
}

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

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]);
}

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

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

vector3d& vector3d::rotate_x (float angle)
{
 	vector3d old (*this);
	double s,c;
	sincos (angle, &s, &c);

	values[0] = old[0];
	values[1] = c * old[1] - s * old[2];
	values[2] = s * old[1] + c * old[2];

 	return *this;
}

vector3d& vector3d::rotate_y (float angle)
{
	vector3d old (*this);
	double s,c;
	sincos (angle, &s, &c);

	values[0] = c * old[0] + s * old[2];
	values[1] = old[1];
	values[2] =	- s * old[0] + c * old[2];

	return *this;
}

vector3d& vector3d::rotate_z (float angle)
{
 	vector3d old (*this);
	double s,c;
	sincos (angle, &s, &c);

	values[0] = c * old[0] + -s * old[1];
	values[1] = s * old[0] + c * old[1];
	values[2] = old[2];

	return *this;
}

vector3d& vector3d::normalize () {
	float norm = length ();
	values [0] /= norm;
	values [1] /= norm;
	values [2] /= norm;

	return *this;
}

void vector3d::print ()
{
  std::cout << values[0] << " " << values[1] << " " << values[2] << std::endl;
}

void vector3d::print (const char *str)
{
  std::cout << str << values[0] << " " << values[1] << " " << values[2] << std::endl;
}

vector3d cross_product (vector3d &a, vector3d &b) {
	return a.cross (b);
}

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

vector3d integrate_rk45 (float t, vector3d& ydot, vector3d& y)
{
  vector3d k1 = ydot * t;
  vector3d k2 = (ydot + k1 / 2) * t;
  vector3d k3 = (ydot + k2 / 2 ) * t;
  vector3d k4 = (ydot + k3) * t;

  return y + (k1 + k2 * 2 + k3 * 2 + k4) / 6;
}

vector3d integrate (float t, vector3d& ydot, vector3d& y)
{
  return integrate_rk45 (t, ydot, y);
}

int solve_quadratic (float a, float b, float c, float *x1, float *x2) {
	if (b*b - 4 * a * c < 0)
		return 0;
	*x1 = static_cast<float> ((-b + sqrt (b*b - 4 * a * c)) / (2 * a));
	*x2 = static_cast<float> ((-b - sqrt (b*b - 4 * a * c)) / (2 * a));

	return 1;
}