#pragma config(Sensor, S1,     Touch1,              sensorTouch)
#pragma config(Motor,  motorA,          MotorA,        tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int bToggle = 0;
long Counter = 0;
int timeTaken;
bool ignoreMonitor;

void valveOn(short MotorX, int Direction)
{
  if (Direction == 1) {
    motor[motorA] = -80;
  }
  else {
    motor[motorA] = 80;
  }
}

task MonitorTimeTaken() {
  // detect when the motor has been slowed
  int normalTimeTaken = 60; // increase this to decrease sensitivity
  while (true) {
    if (timeTaken > normalTimeTaken && ignoreMonitor==false) {
      nMotorEncoder[MotorA] = 0;
      Counter=0;
      timeTaken = normalTimeTaken-10;
      PlayTone(784, 10);   
      nxtDisplayString(4, "    -- BUMP --  ");
      wait1Msec(300);     
      nxtDisplayString(4, "                ");
    } 

  }
}

task TimeTaken() {
 // measure the amount of time Counter increments by 40
  int timeStart;
  long check;
  while (true) {
    timeStart = time1[T1];   
    check = Counter + 40;   
    while (Counter < check) {
        wait1Msec(1);
    }       
    timeTaken = time1[T1] - timeStart;
    nxtDisplayString(5, "CO: %d, CHK: %d", Counter, check);
    nxtDisplayString(6, "TT: %d, dir: %d", timeTaken, bToggle );
    wait1Msec(10);
  }
}

task CounterIncrement() {

  long motorPos;
  while (true) {
    motorPos = nMotorEncoder[MotorA];
    Counter = abs(motorPos);  // the counter is always a positive number

    nxtDisplayString(1, "MA: %d", motorPos);
    nxtDisplayString(2, "CC: %d", Counter);
    wait1Msec(10);
  }
}

task main() {

    // setup motor stuff
  nPowerDownDelayMinutes  = 240;  // 4 hours
  bNoPowerDownOnACAdaptor = true;
  nMotorPIDSpeedCtrl[MotorA] = mtrSpeedReg;
  nMotorEncoder[MotorA] = 0;
  ignoreMonitor = true;

  StartTask(CounterIncrement);
  StartTask(TimeTaken);
  StartTask(MonitorTimeTaken);   
  wait1Msec(300);
  ignoreMonitor = false;
 
  while (nNxtButtonPressed != 3) {

    if (SensorValue(Touch1) == 1) {
      bToggle = 1;
    }
    else {
      bToggle = 0;
    }

    valveOn(MotorA, bToggle);
    wait1Msec(200);
  }
}

task CounterIncrement() {

  long motorPos;
  long posMotor;
  while (true) {
    motorPos = nMotorEncoder[MotorA];
    if (motorPos != posMotor) {
      Counter++;
    }
    posMotor=motorPos;

    nxtDisplayString(1, "MA: %d", motorPos);
    nxtDisplayString(2, "CC: %d", Counter);
    wait1Msec(10);
  }
}

bMotorReflected[MotorA]=true;