[#pragma config(Sensor, S1,     lightright,          sensorLightActive)
#pragma config(Sensor, S3,     lightleft,           sensorLightActive)
#pragma config(Sensor, S4,     lightmid,            sensorLightActive)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define STRAIGHT 1;

void forwardHard ();
void forwardLight();
void turnLeft ();
void turnRight ();
void turnHardLeft ();
void turnHardRight ();
void stopp ();
const int v = 20;

task main(){
  int thresholdlr = 50;
  int thresholdm = 34;
  int lichtwertleft, lichtwertright, lichtwertmid;
  int statecode1 = 0;
  int statecode2 = 0;

  wait1Msec(1000);
  while(1==1){
      lichtwertleft = (SensorValue[lightleft]);
        lichtwertmid = SensorValue[lightmid];
        lichtwertright = SensorValue[lightright];
        nxtDisplayCenteredTextLine(1, "%i links:",lichtwertleft );
        nxtDisplayCenteredTextLine(2, "%i mitte:",lichtwertmid );
        nxtDisplayCenteredTextLine(3, "%i rechts:",lichtwertright);
    if(nNxtButtonPressed == 3){
      while(1==1){
        lichtwertleft = (SensorValue[lightleft]);
        lichtwertmid = SensorValue[lightmid];
        lichtwertright = SensorValue[lightright];
        nxtDisplayCenteredTextLine(1, "%i links:",lichtwertleft );
        nxtDisplayCenteredTextLine(2, "%i mitte:",lichtwertmid );
        nxtDisplayCenteredTextLine(3, "%i rechts:",lichtwertright);


        if (SensorValue[lightmid]<=thresholdm) statecode1 = 1;
        else if (SensorValue[lightmid]>thresholdm) statecode1 = 2;


        switch (statecode1){
        case 1:
          if (SensorValue[lightright]>thresholdlr && SensorValue[lightleft]>thresholdlr) statecode2 = 1;
          else if (SensorValue[lightright]<=thresholdlr && SensorValue[lightleft]>thresholdlr) statecode2 = 2;
          else if (SensorValue[lightright]>thresholdlr && SensorValue[lightleft]<=thresholdlr) statecode2 = 3;

          switch (statecode2){
          case 1:
            forwardHard ();
            break;
          case 2:
            turnRight ();
            break;
          case 3:
            turnLeft ();
            break;
          }
          break;
        case 2:
          forwardLight();
          if (SensorValue[lightright]<=thresholdlr && SensorValue[lightleft]>thresholdlr) statecode2 = 1;
          else if(SensorValue[lightright]>thresholdlr && SensorValue[lightleft]<=thresholdlr) statecode2 = 2;

          switch (statecode2){
          case 1:
            turnHardRight ();
            break;
          case 2:
            turnHardLeft ();
            break;
          }
          break;
        }

      }
    }
  }

}

void forwardHard (){
  motor[motorB]=v;
  motor[motorC]=v;
}
void forwardLight (){
  motor[motorB]=v*0.75;
  motor[motorC]=v*0.75;
}
void turnLeft (){
  motor[motorB]=v;
  motor[motorC]=0;
}
void turnRight(){
  motor[motorB]=0;
  motor[motorC]=v;
}
void stopp (){
  motor[motorB]=0;
  motor[motorC]=0;
}
void turnHardRight (){
  motor[motorB]=-v;
  motor[motorC]=v;
}
void turnHardLeft (){
  motor[motorB]=v;
  motor[motorC]=-v;
}