#ifndef LP_POINTMASS2_H
#define LP_POINTMASS2_H

class LP_Pointmass2
{
public:
  LP_Pointmass2()
  : currPos(0.0f, 0.0f)
  , prevPos(0.0f, 0.0f)
  , force(0.0f, 0.0f)
  , mass(0.0f)
  , friction(0.0f)
  {
  }

  LP_Pointmass2(float xPos, float yPos, float aMass)
  : currPos(xPos, yPos)
  , prevPos(xPos, yPos)
  , force(0.0f, 0.0f)
  , mass(aMass)
  , friction(0.99f)
  {
  }

  LP_Pointmass2(LP_Pointmass2& that)
  : currPos(that.currPos)
  , prevPos(that.prevPos)
  , force(that.force)
  , mass(that.mass)
  , friction(that.friction)
  {
  }

  void Move(const float dt)
  {
      float t, a, c, dtdt, f1, f2;

      dtdt = dt * dt;
      f1 = friction;
      f2 = 1.0f + f1;

      a = force.x / mass;
      c = currPos.x;
      t = f2 * c - f1 * prevPos.x + a * dtdt;
      prevPos.x = c;
      currPos.x = t;

      a = force.y / mass;
      c = currPos.y;
      t = f2 * c - f1 * prevPos.y + a * dtdt;
      prevPos.y = c;
      currPos.y = t;
  }

  float SquareVelocity() const
  {
    float t1 = currPos.x - prevPos.x;
    float t2 = currPos.y - prevPos.y;
    return t1 * t1 + t2 * t2;
  }

  static float Dist(LP_Pointmass2& a, LP_Pointmass2& b)
  {
    return LP_Vector2::Dist(a.currPos, b.currPos);
  }

  void SetFriction(const float aFriction)
  {
    friction = 1.0f - aFriction;
  }

  void SetForce(const LP_Vector2& aForce)
  {
    force = aForce;
  }

  void AddForce(const LP_Vector2& aForce)
  {
    force += aForce;
  }

  LP_Vector2 currPos;
  LP_Vector2 prevPos;
  LP_Vector2 force;
  float mass;
  float friction;
};

#endif
