Robotc Line Follow and Obstacle Avoidence

#pragma config(Sensor, S1,     sonar,          sensorSONAR)
#pragma config(Sensor, S2,     rlight,         sensorLightActive)
#pragma config(Sensor, S3,     llight,         sensorLightActive)
#pragma config(Sensor, S4,     touch,          sensorTouch)
#pragma config(Motor,  motorB,          lmotor,        tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          rmotor,        tmotorNXT, PIDControl, encoder)

void findLine(int threshold) {
    bool wave = false;
    int motorHigh = 40;
    int motorLow = 10;
    int bypass = 0;

    while(SensorValue(llight) > threshold && SensorValue(rlight) > threshold) {
        if(wave) {
            motor[lmotor] = motorHigh;
            motor[rmotor] = motorLow;
        } else {
            motor[rmotor] = motorLow;
            motor[lmotor] = motorHigh;
        }
        bypass++;

        if(bypass >= 4000) {
            wave = !wave;
            bypass = 0;
        }
    }
}

void navigateBottle() {//TODO: Fix this up
    while(SensorValue(llight) = 0) {
        if(SensorValue(sonar) < 5) {
            motor[lmotor] = 0;
            motor[rmotor] = 50;
        }else{
            motor[lmotor] = 50;
            motor[rmotor] = 50;
            wait1Msec(1000);
            motor[lmotor] = 50;
            motor[rmotor] = 0;
        }
    }
}

void followLine(int threshold) {
    int motorHigh = 30;
    int motorLow = 0;
    int counter = 0;

    while(SensorValue(touch) == 0) {
        if(SensorValue(llight) > threshold && SensorValue(rlight) > threshold) {
            counter++;
            if(counter >= 8000) {
                motor[lmotor] = 0;
                motor[rmotor] = 0;
                writeDebugStreamLine("Temporary crazy moment");
                wait1Msec(1000);
                return;
            }
        }

        if(SensorValue(llight) < threshold && SensorValue(rlight) < threshold) {
            writeDebugStreamLine("Turning");

            if(SensorValue(llight) < SensorValue(rlight)) {
                motor[lmotor] = motorHigh;
                motor[rmotor] = motorLow;
            }else{
                motor[lmotor] = motorLow;
                motor[rmotor] = motorHigh;
            }
            counter = 0;

            wait1Msec(50);
        } else {
            if(SensorValue(llight) < threshold) {
                motor[lmotor] = motorHigh;
                motor[rmotor] = motorLow;
                counter = 0;
            }

            if(SensorValue(rlight) < threshold) {
                motor[lmotor] = motorLow;
                motor[rmotor] = motorHigh;
                counter = 0;
            }
        }
    }
}

task main() {
    while(SensorValue(touch) == 0) { }
    int thresholdValue = (SensorValue(llight) + SensorValue(rlight)) / 2;

    writeDebugStreamLine("Threshold %d", thresholdValue);
    wait1Msec(1000);

    while(SensorValue(touch) == 0) { }
    wait1Msec(1000);

    followLine(thresholdValue);
}