package Grasping;

import Carmen.*;

/**
 * <p>Determine if a sensor value is within a given threshold for a given
 * amount of time.
 *
 * You may use this example or write your own if you would like to perform
 * time thresholding of a sensor<\p>
 */

class SensorTimeThreshold {
    double cycleTimeIntegral=0.0;
    double timeThresh; //Seconds
    double sampleThresh;
    double lastTime=-1;
    double cycleTime=0.0;

    public SensorTimeThreshold(double time, double thresh)
    {
        timeThresh=time;
        sampleThresh=thresh;
    }

    private void getCycleTime()
    {
        double time = Util.getTime();
        if(lastTime==-1) //Startup
            cycleTime=0;
        cycleTime = time-lastTime;
        lastTime=time;
    }

    public boolean is_greater(double sample)
    {

       getCycleTime();
       if (sample>=sampleThresh)
           cycleTimeIntegral+=cycleTime;
       else
           cycleTimeIntegral=0;

      return cycleTimeIntegral>timeThresh;
    }

    public boolean is_less(double sample)
    {
       getCycleTime();

       if (sample<=sampleThresh)
           cycleTimeIntegral+=cycleTime;
       else
           cycleTimeIntegral=0;

      return cycleTimeIntegral>timeThresh;
    }

    public boolean is_true(boolean sample)
    {
        getCycleTime();

       if (sample)
           cycleTimeIntegral+=cycleTime;
       else
           cycleTimeIntegral=0;

      return cycleTimeIntegral>timeThresh;
    }

  }