mL  =   brick.motor('M4').setPower;
mR  =   brick.motor('M3').setPower;
eL  =   brick.encoder('E4').read;
eR  =   brick.encoder('E3').read;
rF_1 = brick.sensor('D1').read;
rF_2 = brick.sensor('D2').read;
rF_3 = brick.sensor('A1').read;
rF_4 = brick.sensor('A2').read;
pi  =   Math.PI;
abs =   Math.abs;
wait = script.wait;
TRIK = false;

robot = {
    wheelD: 5.6,
    track: 15.4,
    cpr: 360,
    v: 60
}

function cm2cpr(cm)
{
    return (cm / (pi * robot.wheelD)) * robot.cpr;
}

function motors(vL, vR)
{
    vL = vL == undefined ? robot.v : vL;
    vR = vR == undefined ? vL : vR;
    mL(vL);
    mR(vR);
}

function sign(num) {return num > 0 ? 1 :-1}

function straight_without_pid(cm){
    path = (cm / (pi * robot.wheelD)) * robot.cpr;
    path = path + eL();
    if (cm >= 0){
        motors();
        while (eL() < path) wait(10);
    } else {
        motors(-1 * robot.v);
        while (eL() > path) wait(10);
    }
    motors(0, 0);
}

function yaw() { return brick.gyroscope().read()[6]; }

function turnGyro(angle){
    if (abs(angle) < 200) angle *= 1000;

    var sgn = sign(angle - yaw());
    if (abs(angle - yaw()) >= 180000) sgn = -1 * sgn;

    if (!TRIK) straight_without_pid(5);
    motors(40 * sgn, -40 * sgn);
    while (abs(angle - yaw()) > 3000) wait(10);
    motors(0, 0);

    if (!TRIK) straight_without_pid(-5);
}

function moveGyro(g, ang)
{
    ang *= 1000;
    goal = cm2cpr(g);
    goal += eL();
    while (eL() < goal)
    {
        u = (ang - yaw()) / 1000 * 0.7;
        motors(robot.v + u, robot.v - u);
        wait(1);
    }
    motors(0, 0);
}

function goFront(gyro0){
    gyro0 *= 1000;
    while (rF_1() > 20)
    {
        u = (gyro0 - yaw()) / 1000 * 0.7;
        motors(robot.v + u, robot.v - u);
        wait(1);
    }
    motors(0, 0);
}

function goFront_fromRight(gyro0){
    gyro0 *= 1000;
    while (rF_4() < 75)
    {
        u = (gyro0 - yaw()) / 1000 * 0.7;
        motors(robot.v + u, robot.v - u);
        wait(1);
    }
    motors(0, 0);
}

brick.gyroscope().calibrate(2000);
wait(3000);

while (1)
{
    goFront(0);
    turnGyro(-90);
    rass1 = rF_4();

    gyro0 = -90000;
    path = cm2cpr(70) + eL();

    while (eL() < path && rF_4() < 40 && rF_1() >= 20)
    {
        u = (gyro0 - yaw()) / 1000 * 0.7;
        motors(robot.v + u, robot.v - u);
        wait(1);
    }

    motors(0, 0);

    if ((rF_1() <= 20 || eL() >= path) && abs(rF_4() - rass1) <= 5) break;

    goFront_fromRight(-90);
    moveGyro(8, -90);
    turnGyro(0);
}

goFront(-90);

motors(0, 0);

minRass = 600;
flag = 0;

angles = [180, 90, 0, -90];

for (var i = 0; i < 4; i++)
{
    turnGyro(angles[i]);
    while (1)
    {
        ang = angles[i];
        ang *= 1000;
        goal = cm2cpr(70);
        goal += eL();
        while (1) {
            e = rF_4() - 18;
            p = e * 2.4;
            motors(robot.v + p, robot.v - p);
            if (eL() >= goal) break;
            if (rF_1() <= 22) break;
            if (rF_2() >= 45 && flag == 0) flag = 1;
            if (rF_1() <= 45 && flag == 1) flag = 0;
            wait(1);
        }
        motors(0, 0);

        if (flag)
        {
            if (rF_3() < 80 && rF_4() < 80)
            {
                if (rF_3() + rF_4() < minRass) { minRass = rF_3() + rF_4(); }
            } else {
                turnGyro(angles[i] - 90);
                wait(100);
                if (rF_1() + rF_2() < minRass) { minRass = rF_1() + rF_2(); }
                wait(100);
                turnGyro(angles[i]);
            }
        }
        if (rF_1() <= 22) break;
    }
}

brick.display().addLabel(Math.floor(minRass), 0, 0);
brick.display().redraw();