optimization working

#define led 13
#define dryrunswitch 12
#define actualrunswitch 11
#define AIN1 6
#define BIN1 9
#define AIN2 5
#define BIN2 8
#define PWM1 3
#define PWM2 10
#define stby 7
#define basespeed 90
#define Kp 30// to be determined
#define Kd 40
#define l 1
#define s 2
#define r 3
#define b 4
#define finish 5
int y =0;

void motor1(bool dir1, bool dir2, uint8_t pwm);
void motor2(bool dir1, bool dir2, uint8_t pwm);

void motor1(bool dir1, bool dir2, uint8_t pwm)
{
    digitalWrite(AIN1, dir1);
    digitalWrite(AIN2, dir2);
    analogWrite(PWM1, pwm);
}

void motor2(bool dir1, bool dir2, uint8_t pwm)
{
    digitalWrite(BIN1, dir1);
    digitalWrite(BIN2, dir2);
    analogWrite(PWM2, pwm);
}

int index = 0;
int path[100];
bool linestate[5];
float error, error_prev;
int steeringval;
int leftmotspeed;
int rightmotspeed;
float numerator;
float denominator;

void turnleft()
{
    motor1(HIGH, LOW, basespeed);
    motor2(HIGH, LOW, basespeed);
    while ((linestate[4] == 1) || (linestate[0] == 1)) 
    {
        Serial.print("Moving forward");
        readir();
    }
    int start_time = millis();
    while (millis() - start_time < 10){}
    motor1(HIGH, HIGH,basespeed);
    motor2(HIGH, LOW, basespeed);
    Serial.print("stopped moving forward and turning left");
    Serial.print("");
    while (1)
    {
        readir();
        Serial.print("trying to come out of the line");
        if (linestate[2] == 0)
        {
            break;
        }
    }
    while (1)
    {
        readir();
        Serial.print("turning left into the line");
        if ( linestate[2] == 1)
        {
            break;
        }
    }
    Serial.print("reached line");
    motor2(HIGH, HIGH, 0);
}

void turnright()
{
    motor1(HIGH, LOW, basespeed);
    motor2(HIGH, HIGH, 0);
    Serial.println("turning right");
    while (1)
    {

        readir();
        Serial.println("trying to go out of the right line");
        if (linestate[2] == 0) 
        {
            break;
        }
    }
    while (1) //
    {
        readir();
        Serial.println("finding the right line");
        if (linestate[1] && linestate[2])
        {
            break;
        }
    }
    motor1(HIGH, HIGH, 0);
}

void turnback()
{
    int start_time1 = millis();
    while(millis() - start_time1 < 400){}
    Serial.println("turning back");
    motor1(HIGH, LOW, 120);
    motor2(LOW, HIGH, 120);
    while (1)
    {
        readir();
        if (linestate[2] == 0)
        {
            break;
        }
    }
    while (1)
    {
        readir();
        if (linestate[2] == 1)
        {
            break;
        }
    }
    motor1(HIGH, HIGH, 0);
    motor2(HIGH, HIGH, 0);
}

void readir()
{
    for (int i = 0; i < 5; i++)
    {
        linestate[i] = digitalRead(18 - i);
        // Serial.print(linestate[i]);
        // Serial.print("\t");
    }
    Serial.println("");
}

void linefollow()
{
    Serial.print("Following line");
    error_prev = error;
    numerator = ((linestate[0] * (-2)) + (linestate[1] * (-1)) + (linestate[3]) + (linestate[4] * 2));
    denominator = linestate[0] + linestate[1] + linestate[2] + linestate[3] + linestate[4];
    error = (numerator) / (denominator);
    // Serial.print("Numerator: ");
    // Serial.print(numerator);
    // Serial.print("\t");
    // Serial.print("Dinominator: ");
    // Serial.print(denominator);
    // Serial.print("\n");

    // Serial.print("Error: ");
    // Serial.print(error);
    // Serial.print("\n");
    steeringval = Kp * error + Kd * (error - error_prev);
    leftmotspeed = (basespeed) + steeringval;
    rightmotspeed = (basespeed)-steeringval;
    motor1(1, 0, (leftmotspeed < 0 ? 0 : leftmotspeed));
    // Serial.print(leftmotspeed);
    // Serial.print("\t");
    motor2(1, 0, (rightmotspeed < 0 ? 0 : rightmotspeed));
    // Serial.println(rightmotspeed);
}


void setup()
{
    Serial.begin(9600);
    pinMode(14, INPUT);
    pinMode(15, INPUT);
    pinMode(16, INPUT);
    pinMode(17, INPUT);
    pinMode(18, INPUT);
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(PWM1, OUTPUT);
    pinMode(BIN1, OUTPUT);
    pinMode(BIN2, OUTPUT);
    pinMode(PWM2, OUTPUT);
    pinMode(actualrunswitch, INPUT);
    pinMode(dryrunswitch, INPUT);
    pinMode(led, OUTPUT);
    digitalWrite(stby, HIGH);
}

void loop()
{   while(dryrunswitch==LOW)
    {
        ;
    }
    while (1)
    {
        
    
    
    readir();
    linefollow();
    if ((linestate[0] == 1) && (linestate[1] == 1) && (linestate[2] == 1) && (linestate[3] == 0) && (linestate[4] == 0))
    {
        if ((linestate[0] == 1) && (linestate[1] == 1) && ((linestate[2] == 1) || (linestate[3] == 0)) && (linestate[4] == 0))
        {
            turnleft();
        }
    }
    
    else if ((linestate[0] == 1) && (linestate[1] == 1) && (linestate[2] == 1) && (linestate[3] == 1) && (linestate[4] == 1))
        {
            Serial.println("either finish or left");
            int start_time2 = millis();
            while(millis() - start_time2 < 200){
                Serial.println("moving forward !!!");
            }
            readir();
            if((linestate[0] == 1) && (linestate[1] == 1) && (linestate[2] == 1) && (linestate[3] == 1) && (linestate[4] == 1)){
                Serial.println("end detected");
                motor1(HIGH, HIGH, 0);
                motor2(HIGH, HIGH, 0);
                digitalWrite(led, HIGH);
                y = 1;
                while(1){
                    delay(1);
                }
            }
            else{
                Serial.println("left activated through here");
                turnleft(); // not added finish
            }
            
        }

    else if (linestate[0] && linestate[2]){
        turnleft();
    }

    else if ((linestate[0] == 0) && (linestate[1] == 0) && (linestate[2] == 1) && (linestate[3] == 1) && (linestate[4] == 1))
    {
        motor1(HIGH, LOW, basespeed);
        motor2(HIGH, LOW, basespeed);
        while ((linestate[4] == 1) && (linestate[0] == 0))
        {
            Serial.println("moving forward in a r or s junction");
            readir();
        }

        motor1(HIGH, HIGH, 0);
        motor2(HIGH, HIGH, 0);

        if (linestate[0]+linestate[1]+linestate[2]+linestate[3]+linestate[4])
        {
            Serial.println("Straight line detected");
            linefollow();
        }
        else
        {
            turnright();
        }
    }
    else if (!(linestate[0] || linestate[1] || linestate[2] || linestate[3] || linestate[4])){
        turnback();
    }
    }
}