#ifndef STICK_H
#define STICK_H

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

class stick
{
public:
  stick()
  {
	  
  }

  stick(pointMass& aA, pointMass& aB)
  {
	a = &aA;
	b = &aB;
	
	float length = sqrt(ofxVec2fDistance(a->currPos, b->currPos));
	lengthSquared = length * length;
  }

  stick(const stick& that)
  {
	  a = that.a;
	  b = that.b;
	  
	float length = sqrt(ofxVec2fDistance(a->currPos, b->currPos));
	lengthSquared = length * length;
  }

  void Scale( const float scaleFactor)
  {
    lengthSquared *= scaleFactor * scaleFactor;
  }

  void SetLength(const float length)
  {
    lengthSquared = length * length;
  }

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

	float dotProd, scaleFactor;

	dotProd = delta.x * delta.x + delta.y * delta.y;
	scaleFactor = lengthSquared / (dotProd + lengthSquared) - 0.5f;
	scale2dVector(delta,scaleFactor);

	/*a->currPos -= delta;
	b->currPos += delta;*/

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

#endif
