#include "RSnsr.h"

RSnsr::RSnsr(int handle, int addr, char rt) : Srf10(handle, addr, rt)
{
  dist1 = 0;
  dist2 = 0;
  oldFlag = false;
} // RSnsr

int RSnsr::mReadS()
{
  int dist;

  dist = readS();
  mid = dist1;
  if(dist2 >= mid)
  {
    high = dist2;
    low = 0;
  }
  else
  {
    low = dist2;
    high = 0;
  }

  curDist = (dist >= mid)?
            ((high)?((dist >= high)?high:dist):mid):
            ((low)?((dist >= low)?dist:low):mid);

/*
        if(dist >= mid)
          if(high)
            if(dist >= high)
              curDist = high;
            else
              curDist = dist;
          else
            curDist = mid;
        else
          if(low)
            if(dist >= low)
              curDist = dist;
            else
              curDist = low;
          else
            curDist = mid;
*/

  if(oldFlag)
    dist2 = dist;
  else
    dist1 = dist;

  oldFlag = !oldFlag;

  if(curDist)
    return curDist;
  else return dist;
} //mReadS