#pragma config(Sensor, S1,     Touch1,              sensorTouch)
#pragma config(Sensor, S2,     Touch2,              sensorTouch)
#pragma config(Sensor, S3,     Touch3,              sensorTouch)
#pragma config(Motor,  motorA,          Motor1,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          Motor2,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          Motor3,        tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(NXT)

int valvePos[4];     // array to store the current valve positions
int leverLFT = 50;   // motor left power
int leverRGT = -50;  // motor right power

// move the motor until it is stopped.
void valveOn(short MotorX, int Direction)
{
  int motorBack = 40;   // motor relief
  int mpos;
  int menc;
  int mpow;

  // rotate the motor until it has stopped (stays at the same value for more than 25msec)
  while(mpos != menc) {
    mpos = nMotorEncoder[MotorX];
    motor[MotorX] = Direction;
    wait1Msec(25);  // increase this delay slightly if the valve doesn't move
    menc = nMotorEncoder[MotorX];
  }
  motor[MotorX] = 0; //wait1Msec(10);
  // back the lever of a little to stop the motor from squealing.
  if (Direction == leverRGT) {  // right
    mpos = nMotorEncoder[MotorX] + motorBack;
    mpow = leverLFT;
    }
  else {                        //left
    mpos = nMotorEncoder[MotorX] - motorBack;
    mpow = leverRGT;
  }
  nMotorEncoderTarget[MotorX] = mpos;
  while (nMotorEncoder[MotorX] != mpos) {
    motor[MotorX] = mpow;
    }
  motor[MotorX] = 0;
  return;
}

void valveMove(short MotorX, int Valve, int Position) {
  //  valve positions:
  //    4 unkown (on startup)
  //    1 lever right   motor: -50
  //    2 lever left    motor: +50

  // if the valve state is unknown (4), set it up
  if (valvePos[Valve] == 4) {
    valveOn(MotorX,leverRGT);
    valvePos[Valve] = 1;
    return;
  }

  // if current position is the same as the new position, don't bother
  if (valvePos[Valve] == Position) {
    return;
  }

  switch (Position) {
    case 1:                                 //    1 lever right
      valveOn(MotorX,leverRGT);
      break;
    case 2:                                 //    2 lever left
      valveOn(MotorX,leverLFT);
      break;
    }
  valvePos[Valve] = Position;
  return;
}


task main() {

  // setup motor stuff
  nPowerDownDelayMinutes  = 240;  // 4 hours
  bNoPowerDownOnACAdaptor = true;
  nMotorPIDSpeedCtrl[Motor1] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[Motor2] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[Motor3] = mtrSpeedReg;
  nMaxRegulatedSpeedNxt = 750;

  nMotorEncoder[Motor1] = 0;
  nMotorEncoder[Motor2] = 0;
  nMotorEncoder[Motor3] = 0;

  // valve positions are unkown at startup
  valvePos[1] = 4;
  valvePos[2] = 4;
  valvePos[3] = 4;
 
  //set valves to start positions
  valveMove(Motor1,1,2);       // valveMove(motor, valvePos[no], position)
  valveMove(Motor2,2,1);
  valveMove(Motor3,3,1);

  wait1Msec(1000);
  PlaySound(soundBeepBeep);

  // main loop, move the relivant valve when the touch sensor is pressed.
  while (nNxtButtonPressed != 3) {
    nxtDisplayBigStringAt (0,60,"M1: %d  ", valvePos[1]);
    nxtDisplayBigStringAt (0,40,"M2: %d  ", valvePos[2]);
    nxtDisplayBigStringAt (0,20,"M3: %d  ", valvePos[3]);

    if (SensorValue(Touch1) == 1) {
      if (valvePos[1] == 1) {
        valveMove(Motor1,1,2);
        wait1Msec(100);
      }
      else {
        valveMove(Motor1,1,1);
        wait1Msec(100);
      }
    }
    if (SensorValue(Touch2) == 1) {
      if (valvePos[2] == 1) {
        valveMove(Motor2,2,2);
        wait1Msec(100);
      }
      else {
        valveMove(Motor2,2,1);
        wait1Msec(100);
      }
    }
    if (SensorValue(Touch3) == 1) {
      if (valvePos[3] == 1) {
        valveMove(Motor3,3,2);
        wait1Msec(100);
      }
      else {
        valveMove(Motor3,3,1);
        wait1Msec(100);
      }
    }
    wait1Msec(200);
  }
}

void valveMove(short MotorX, int Valve, int Position)

void valveMove(tMotors MotorX, int Valve, int Position)