///////////////////////////////////////////////////////////////////////////////
//                                                                           //
// Copyright (C) 2006 by Intel Corporation and Carnegie Mellon University    //
// Contacts: casey.j.helfrich @ intel.com                                    //
//           bdr @ cs.cmu.edu                                                //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#include "Primitives.hxx"

bool floatStrictLess(float a, float b){
  //floats represent a range of numbers.
  //a<b iff a's range is less than and disjoint
  //to b's range
  
  float d = .0001;
  float maxa = a + d;
  float minb = b - d;
  
  if(maxa<minb)
    return true;

  return false;
}

bool floatEq(float a, float b){
  return !(floatStrictLess(a,b) || floatStrictLess(b,a));
}


///////////////////////////////////////////////////////////////////////////////
// The Point3D class to describe and manipulate points in three space.
//

///////////////////////////////////////////////////////////////////////////////
// Rotation Methods
// 
// This method uses the XYZ Standard

Point3D Point3D::eulerRotate(double roll, double pitch, double yaw) const {
  
  // Total Euler rotation A = BCD
  
  // Calculate X (roll) rotation Matrix
  // 
  //     [ cos(yaw)    sin(yaw)  0 ] 
  //     [                         ]
  // D = [ -sin(yaw)   cos(yaw)  0 ]
  //     [                         ]
  //     [ 0           0         1 ]
  
  // Calculate Y (pitch) rotation Matrix
  // 
  //     [ cos(pitch)  0         -sin(pitch) ] 
  //     [                                   ]
  // C = [ 0           1         0           ]
  //     [                                   ]
  //     [ sin(pitch)  0         cos(pitch)  ]

  // Calulate Z (yaw) rotation Matrix
  // 
  //     [ 1          0          0         ] 
  //     [                                 ]
  // B = [ 0          cos(roll)  sin(roll) ]
  //     [                                 ]
  //     [ 0          -sin(roll) cos(roll) ]

  // Compose them:
  // 
  //     [ a11        a12       a13 ] 
  //     [                          ]
  // A = [ a21        a22       a23 ]
  //     [                          ]
  //     [ a31        a32       a33 ]
  //
  //
  // a = a11 = cos(pitch)*cos(yaw)
  // b = a12 = cos(pitch)*sin(yaw)
  // c = a13 = -sin(pitch)
  // d = a21 = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw)
  // e = a22 = sin(roll)*sin(pitch)*sin(yaw) - cos(roll)*cos(yaw)
  // f = a23 = cos(pitch)*sin(roll)
  // g = a31 = cos(roll)*sin(pitch)*cos(yaw) - sin(roll)*sin(yaw)
  // h = a32 = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw)
  // i = a33 = cos(pitch)*cos(roll)

  double A[3][3];
  
  A[0][0] = cos(pitch)*cos(yaw);
  A[0][1] = cos(pitch)*sin(yaw);
  A[0][2] = -sin(pitch);
  A[1][0] = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw);
  A[1][1] = sin(roll)*sin(pitch)*sin(yaw) - cos(roll)*cos(yaw);
  A[1][2] = cos(pitch)*sin(roll);
  A[2][0]= cos(roll)*sin(pitch)*cos(yaw) - sin(roll)*sin(yaw);
  A[2][1] = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw);
  A[2][2] = cos(pitch)*cos(roll);
  
  // Apply the Rotation Matrix
  double newX = x * A[0][0] + y * A[0][1] + z * A[0][2];
  double newY = x * A[1][0] + y * A[1][1] + z * A[1][2];
  double newZ = x * A[2][0] + y * A[2][1] + z * A[2][2];
  
  return Point3D(newX, newY, newZ);
}


//////////////////////////////////////////////////////////////////////////////
// The DPRLine class (for drawing connecting lines between catoms)

DPRLine::DPRLine(catomID a, catomID __b) {
  src = a;
  dest = __b;
  r = 200;
  g = 0;
  b = 0;
  type = 0;
}

DPRLine::DPRLine(catomID a, catomID __b, 
		 unsigned int _r, unsigned int _g, unsigned int _b) {
  src = a;
  dest = __b;
  r = _r;
  g = _g;
  b = _b;
  type = 0;
}


DPRLine::DPRLine(catomID a, Point3D __b)
{
  src = a;
  destPt = __b;
  r = 200;
  g = 0;
  b = 0;
  type = 2;
}
DPRLine::DPRLine(catomID a, Point3D _pb, unsigned int _r, unsigned int _g, unsigned int _b)
{
  src = a; 
  destPt = _pb;
  r = _r;
  g = _g;
  b = _b;
  type = 2;
}
  
// bool DPRLine::equals(catomID a, catomID b) {
//   return (src == a) ? (dest == b) : (src == b && dest == b);
// }

bool DPRLine::equals(catomID a, catomID _b) {
return (src == a && dest == _b);
}



bool DPRLine::equals(catomID a, Point3D _b) {
  return (src == a && (destPt-_b).norm()<0.01);
}

///////////////////////////////////////////////////////////////////////////////
// The Plane class to describe a plane in 3D space using 3 Point3Ds
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// Accessor Methods

Point3D Plane::getA() {
  return a;
}

Point3D Plane::getB() {
  return b;
}

Point3D Plane::getC() {
  return c;
}

///////////////////////////////////////////////////////////////////////////////
// Constructors

Plane::Plane(Point3D aa, Point3D bb, Point3D cc) : a(aa), b(bb), c(cc) {};


///////////////////////////////////////////////////////////////////////////////
// GUID()
// class to represent GUIDs.  Don't count on them being in sequence.  In 
// reality they will be randomly assigned 128 bit integers or the like.
//
// internally, a GUID with 0 as an ID will not be considered a valid GUID, 
// see isaGUID()
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// Accessor Methods

unsigned long GUID::getID() const {
  return id;
}

///////////////////////////////////////////////////////////////////////////////
// Data Update Methods

void  GUID::setID(unsigned long i) {
  id = i;
}

///////////////////////////////////////////////////////////////////////////////
// Comparison Methods

bool GUID::isEqual(const GUID other) const {
  return (id == other.getID());
}

bool GUID::isGreater(const GUID other) const {
  return (id < other.getID());
}

GUID GUID::getAnonGUID(void) {
  return GUID(0);
}

///////////////////////////////////////////////////////////////////////////////
// Constructors

GUID::GUID() {
  do {
    id = rand();
  } while(id == 0);
}

GUID::GUID(unsigned long i) : id(i) {}

GUID::GUID(const GUID& guid) {
  setID(guid.getID());
}

bool GUID::isaGUID() const {
  return (id != 0);
}

ostream& operator<<(ostream& os, const Point3D& p) {
  os << "(" << p.getX() << "," << p.getY() << "," << p.getZ() << ")";
  return os;
}

