#ifndef JOINT_H
#define JOINT_H

#include "additionalFunctions.h"
#include "pointmass.h"

class joint
{
public:
  joint()
  {
	  a = NULL;
	  b = NULL;
	  shortConstSquared = 0.0f;
	  longConstSquared = 0.0f;
  }

  joint(pointMass& aA, pointMass& aB, float aShortConst, float aLongConst/*, int _id*/)
  {
	  ofxVec2f delta;
	a = &aA;
	b = &aB;
	
    delta = b->currPos;
    delta -= a->currPos;

    float dotProd = delta.x * delta.x + delta.y * delta.y;

    shortConstSquared = dotProd * aShortConst * aShortConst;
    longConstSquared = dotProd * aLongConst * aLongConst;
  }

  void Scale(const float scaleFactor)
  {
    float sf2 = scaleFactor * scaleFactor;
    shortConstSquared *= sf2;
    longConstSquared *= sf2;
  }

  void SetConstraints(float aShortConst, float aLongConst)
  {
    shortConstSquared = aShortConst * aShortConst;
    longConstSquared = aLongConst * aLongConst;
  }

  void Satisfy()
  {
	ofxVec2f delta;
    delta = b->currPos;
    delta -= a->currPos;

    float scaleFactor;
    float dotProd =  delta.x * delta.x + delta.y * delta.y;

    if(dotProd < shortConstSquared)
    {
      scaleFactor = shortConstSquared / (dotProd + shortConstSquared) - 0.5f;
      scale2dVector(delta,scaleFactor);
		if(a->fixed) b->currPos += delta;
		if(b->fixed) a->currPos -= delta;
		
		if(!b->fixed && !a->fixed) {
			a->currPos -= delta;
			b->currPos += delta;
		}
    }
    else if(dotProd > longConstSquared)
    {
      scaleFactor = longConstSquared / (dotProd + longConstSquared) - 0.5f;
      scale2dVector(delta,scaleFactor);
		if(a->fixed) b->currPos += delta;
		if(b->fixed) a->currPos -= delta;
		
		if(!b->fixed && !a->fixed) {
			a->currPos -= delta;
			b->currPos += delta;
		}

    }
	
	a->bind();
	b->bind();
  }

  pointMass* a;
  pointMass* b;

  float shortConstSquared;
  float longConstSquared;
};

#endif
