#include "PositionFormula.h"

PositionFormula::PositionFormula()
{
  clear();
}

PositionFormula::PositionFormula(Param & hash)
{
  set(hash);
}

PositionFormula::~PositionFormula()
{
}


void PositionFormula::clear()
{
  startTime=0;
  duration=0;
  offset=0;
  linear=0;
  quadratic=0;
  sineScalar=0;
  cosineScalar=0;
  timeShift=0;
}

//extracts variables from hash
void PositionFormula::set(Param & hash)
{
  int intRead;

  intRead=0;
  hash.getInt("Start Time", intRead);
  if (intRead >= 0)
    startTime=intRead;
  else
    startTime=0;

  intRead=-1;
  hash.getInt("Duration", intRead);
  duration=intRead;

  intRead=0;
  hash.getInt("Time Shift", intRead);
  timeShift=intRead;

  intRead=0;
  hash.getInt("Offset", intRead);
  offset=FixedInt(intRead);

  intRead=0;
  hash.getInt("Linear", intRead);
  linear=FixedInt(intRead);

  intRead=0;
  hash.getInt("Quadratic", intRead);
  quadratic=FixedInt(intRead);

  intRead=0;
  hash.getInt("Sine Amplitude", intRead);
  sineScalar=FixedInt(intRead);

  intRead=0;
  hash.getInt("Cosine Amplitude", intRead);
  cosineScalar=FixedInt(intRead);

  intRead=1000;
  hash.getInt("Sine Period", intRead);
  if (intRead==0)
    intRead=1000;
  sinePeriod=FixedInt(intRead);

  intRead=1000;
  hash.getInt("Cosine Period", intRead);
  if (intRead==0)
    intRead=1000;
  cosinePeriod=FixedInt(intRead);
}

//returns false if tim < startTime or tim > startTime + duration. Otherwise ans=f(tim) and returns true.
//duration == -1 means duration is inifinite.
bool PositionFormula::position (int tim, FixedInt & ans)
{
  if ((tim < startTime) ||
      (duration > -1 && tim > duration+startTime))
    return false;

  FixedInt tim2=int(tim+timeShift-startTime), tim3=tim2;
  tim2 /= 1000;

  ans=offset;
  ans+=(linear*tim2);
  ans+=(quadratic*tim2*tim2);
  if (sineScalar>0)
    ans+=sineScalar*sine(((tim3/sinePeriod)*256).toAngle());
  if (cosineScalar>0)
    ans+=cosineScalar*cosine(((tim3/cosinePeriod)*256).toAngle());
  return true;
}
