#ifndef LP_STICK2_H
#define LP_STICK2_H

class LP_Stick2
{
public:
  LP_Stick2()
  : a(NULL)
  , b(NULL)
  , lengthSquared(0.0f)
  {
  }

  LP_Stick2(LP_Pointmass2& aA, LP_Pointmass2& aB)
  : a(&aA)
  , b(&aB)
  {
    float length = LP_Vector2::Dist(a->currPos, b->currPos);
    lengthSquared = length * length;
  }

  LP_Stick2(const LP_Stick2& that)
  : a(that.a)
  , b(that.b)
  {
    float length = LP_Vector2::Dist(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()
  {
    LP_Vector2 delta(b->currPos);
    delta -= a->currPos;

    float dotProd, scaleFactor;

    dotProd = LP_Vector2::DotProd(delta, delta);
    scaleFactor = lengthSquared / (dotProd + lengthSquared) - 0.5f;
    delta.Scale(scaleFactor);

    a->currPos -= delta;
    b->currPos += delta;
  }

  LP_Pointmass2* a;
  LP_Pointmass2* b;

  float lengthSquared;

};

#endif
