#pragma config(Sensor, S1,     HTIRL,               sensorLowSpeed)
#pragma config(Sensor, S2,     TouchSensor,         sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/HTIRL-NG-driver.h"

int motorToControl = nMotorEncoder[motorB];
int controlledMotor;

task main(){

  nxtDisplayCenteredTextLine (1, "Press");
  nxtDisplayCenteredBigTextLine (2, "Button");
  nxtDisplayCenteredTextLine (5, "at following");
  nxtDisplayCenteredTextLine (6, "motor position");
  wait10Msec(400);
  eraseDisplay();

  while (SensorValue (TouchSensor) == 0){
     nxtDisplayCenteredTextLine (1, "motor position:");
     nxtDrawCircle (29, 40, 40);
     nxtDrawLine (21, 43, 48, 19);
     nxtDrawLine (29, 0, 29, 15);
     nxtDrawLine (30, 0, 30, 12);
     nxtDrawLine (31, 0, 31, 11);
     nxtDrawLine (32, 0, 32, 9);
     nxtDrawLine (33, 0, 33, 8);
     nxtDrawLine (34, 0, 34, 7);
     nxtDrawLine (35, 0, 35, 5);
     nxtDrawLine (36, 0, 36, 4);
     nxtDrawLine (37, 0, 37, 3);
     nxtDrawLine (38, 0, 38, 2);
     nxtDrawLine (39, 0, 39, 1);
     nxtDrawLine (40, 0, 40, 1);
     nxtDrawLine (41, 0, 41, 1);
     nxtDrawLine (42, 0, 42, 1);
     nxtDrawLine (43, 0, 43, 1);

     nxtDrawLine (67, 0, 67, 15);
     nxtDrawLine (66, 0, 66, 12);
     nxtDrawLine (65, 0, 65, 11);
     nxtDrawLine (64, 0, 64, 9);
     nxtDrawLine (63, 0, 63, 8);
     nxtDrawLine (62, 0, 62, 7);
     nxtDrawLine (61, 0, 61, 5);
     nxtDrawLine (60, 0, 60, 4);
     nxtDrawLine (59, 0, 59, 3);
     nxtDrawLine (58, 0, 58, 2);
     nxtDrawLine (57, 0, 57, 1);
     nxtDrawLine (56, 0, 56, 1);
     nxtDrawLine (55, 0, 55, 1);
     nxtDrawLine (54, 0, 54, 1);
     nxtDrawLine (53, 0, 53, 1);
  }

  eraseDisplay();
  nMotorEncoder[motorB] = 0;
  while (SensorValue (TouchSensor) == 1) {}
  PlaySound(soundBeepBeep);

  while (true){

    motorToControl = nMotorEncoder[motorB];
    controlledMotor = 1+motorToControl/90;

    nxtDisplayCenteredTextLine (1, "You are");
    nxtDisplayCenteredTextLine (2, "controlling");
    nxtDisplayCenteredBigTextLine (4, "motor");
    nxtDisplayCenteredBigTextLine (6, "%d", controlledMotor);

    if (motorToControl >= 360){
      nMotorEncoder[motorB] = 0;
    }

    if (motorToControl >=0 && motorToControl < 90) {
      if (SensorValue (TouchSensor) == 1){
        PFMotor(pfmotor_S1_C4_A, (ePWMMotorCommand)7);
      }
      else{
        PFMotor(pfmotor_S1_C4_A, (ePWMMotorCommand)0);
      }
    }
    else {}

    if (motorToControl >90 && motorToControl < 180) {
      if (SensorValue (TouchSensor) == 1){
        PFMotor(pfmotor_S1_C4_B, (ePWMMotorCommand)7);
      }
      else{
        PFMotor(pfmotor_S1_C4_B, (ePWMMotorCommand)0);
      }
    }
    else {}

    if (motorToControl >180 && motorToControl < 270) {
      if (SensorValue (TouchSensor) == 1){
        PFMotor(pfmotor_S1_C1_A, (ePWMMotorCommand)7);
      }
      else{
        PFMotor(pfmotor_S1_C1_A, (ePWMMotorCommand)0);
      }
    }
    else {}

    if (motorToControl >270 && motorToControl <360) {
      if (SensorValue (TouchSensor) == 1){
        PFMotor(pfmotor_S1_C1_B, (ePWMMotorCommand)7);
      }
      else{
        PFMotor(pfmotor_S1_C1_B, (ePWMMotorCommand)0);
      }
    }
  }
}