#include "SpringMassDamper.h"

#include "Arduino.h"

SpringMassDamper::SpringMassDamper(float m, float k, float d, float* out) : 
		_m(m), _k(k), _d(d), _out(out)  {}

float SpringMassDamper::position(int x) {
  
  float dt = (millis() - _tick) / 1000.0f;          
  
  F = _k * (x - X) - (_d * V);
  V += (F / _m) * dt;
  X += V * dt;

  _tick = millis();

  if(_out != NULL) 
  	*_out = X;       

  return X;  	

}