#ifndef LP_JOINT2_H
#define LP_JOINT2_H

#include "lp_pointmass2.h"
#include "lp_vector2.h"

class LP_Joint2
{
public:
  LP_Joint2()
  : a(NULL)
  , b(NULL)
  , shortConstSquared(0.0f)
  , longConstSquared(0.0f)
  {
  }

  LP_Joint2(LP_Pointmass2& aA, LP_Pointmass2& aB, float aShortConst, float aLongConst)
  : a(&aA)
  , b(&aB)
  {
    LP_Vector2 delta(b->currPos);
    delta -= a->currPos;

    float dotProd = LP_Vector2::DotProd(delta, delta);

    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()
  {
    LP_Vector2 delta(b->currPos);
    delta -= a->currPos;

    float scaleFactor;
    float dotProd = LP_Vector2::DotProd(delta, delta);

    if(dotProd < shortConstSquared)
    {
      scaleFactor = shortConstSquared / (dotProd + shortConstSquared) - 0.5f;
      delta.Scale(scaleFactor);
      a->currPos -= delta;
      b->currPos += delta;
    }
    else if(dotProd > longConstSquared)
    {
      scaleFactor = longConstSquared / (dotProd + longConstSquared) - 0.5f;
      delta.Scale(scaleFactor);
      a->currPos -= delta;
      b->currPos += delta;
    }
  }

  LP_Pointmass2* a;
  LP_Pointmass2* b;

  float shortConstSquared;
  float longConstSquared;
};

#endif
