#pragma config(Sensor, S1,     lightSensor1,   sensorLightActive)
#pragma config(Sensor, S2,     lightSensor2,   sensorLightActive)
#pragma config(Sensor, S3,     touchSensor,    sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
//This code uses threshold values to program the light sensors and then uses light//
//sensor 1 to follow a parrell line and light sensor 2 to stop at perpendicular lines//
//Attempts to integrate line counting with sensor 2 into movements//

int lightValue1;
int darkValue1;
int sumValue1;
int thresholdValue1;
int lightValue2;
int darkValue2;
int sumValue2;
int thresholdValue2;
int countValue = 0;
int lastSeen;       // 0 = dark and 1 = light



void findThreshold2()
{
  while(SensorValue(touchSensor) == 0)
   {
      nxtDisplayStringAt(0,31,"Read Light 2 Now");
   }

   lightValue2 = SensorValue(lightSensor2);

   wait1Msec(1000);

   while(SensorValue(touchSensor) == 0)
   {
      nxtDisplayStringAt(0,31,"Read Dark 2 Now");
   }

   darkValue2 = SensorValue(lightSensor2);

   sumValue2 = lightValue2 + darkValue2;
   thresholdValue2 = sumValue2/2;
}

void findThreshold1()
{
  while(SensorValue(touchSensor) == 0)
   {
      nxtDisplayStringAt(0,31,"Read Light 1 Now");
   }

   lightValue1 = SensorValue(lightSensor1);

   wait1Msec(1000);

   while(SensorValue(touchSensor) == 0)
   {
      nxtDisplayStringAt(0,31,"Read Dark 1 Now");
   }

   darkValue1 = SensorValue(lightSensor1);

   sumValue1 = lightValue1 + darkValue1;
   thresholdValue1 = sumValue1/2;
}

void followLine()  //line following code here

{if(SensorValue[lightSensor1] < thresholdValue1)  // If the Light Sensor reads a value less than 43:
      {
        motor[motorC] = 60;                  // Motor C is run at a 60 power level.
          motor[motorB] = 20;                  // Motor B is run at a 20 power level.
      }
      else                               // If the Light Sensor reads a value greater than or equal to 43:
      {
        motor[motorC] = 20;                  // Motor C is run at a 20 power level.
          motor[motorB] = 60;
       }

}

void stopLine()  //stop at perpendicular lines w/ lightSensor2
{
 while(true)

  if(SensorValue[lightSensor2] <thresholdValue2)
    {
    motor [motorC] = 0;
    motor [motorB] = 0;
    wait1Msec(1000);
    motor [motorC] = 20;
    motor [motorB] = 20;
    wait1Msec (500);
    }
  else
    {
    followLine();
    }
  }


void forwardLines(int numLines)
{
   lastSeen = 1;
   countValue = 0;

   while(countValue < numLines)
   {
        stopLine();

      if(SensorValue(lightSensor2) < thresholdValue2)
      {
         if(lastSeen == 1)  // 0 = dark and 1 = light
        {
           countValue = countValue + 1;
        lastSeen = 0;
      }
      }

      else
      {
       lastSeen = 1;

      }

   }
}

void turnLeft()
{
  nMotorEncoder[motorB] = 0;
 while(nMotorEncoder[motorB] > -100)
 {
   motor[motorC] = -50;
   motor[motorB] =-50;
 }
 nMotorEncoder[motorB] = 0;
 while(nMotorEncoder[motorB] < 160)
 {
   motor[motorC] = -50;
   motor[motorB] = 50;
 }

 motor[motorC] = 0;
 motor[motorB] = 0;

}

void turnRight()
{

  nMotorEncoder[motorC] = 0;
  while(nMotorEncoder[motorC] > -100)
 {
   motor[motorC] = -50;
   motor[motorB] = -50;
 }
nMotorEncoder[motorC] = 0;
 while(nMotorEncoder[motorC] < 160)
 {
   motor[motorC] = 50;
   motor[motorB] = -50;
 }

 motor[motorC] = 0;
 motor[motorB] = 0;

}
task main()
{

nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;

  findThreshold2();
  wait1Msec(1000);
  findThreshold1();
  forwardLines(2);
  turnLeft();


}