var __interpretation_started_timestamp__;
var pi = 3.141592653589793;
var d = 5.6
var l = 17.5
var x = 0 // Начальные координаты робота
var y = 0
var kp = 3

// Моторы
var mLeft = brick.motor(M4).setPower;
var mRight = brick.motor(M3).setPower;
var cpr = 360 // Показания энкодера за оборот
var v = 90

// Энкодеры
var eLeft = brick.encoder(E4);
var eRight = brick.encoder(E3);
eLeft.reset();
eRight.reset();

var cellLength = 69;

// Датчики расстояния
var svFront = brick.sensor(A1);
var svLeft = brick.sensor(A2);
var svLight = brick.sensor(A5);
var dist_threshold = 30;


function forward_enc(length)
{
	eLeft.reset();
	eRight.reset();
	left = eLeft.readRawData();
	right = eRight.readRawData();

	while ((left+right)/2 < (length * cpr) / (pi * d))
	{
		mLeft(100);
		mRight(100);
		left = eLeft.readRawData();
		right = eRight.readRawData();
		script.wait(1);
	}

	mLeft(0);
	mRight(0);
}

function forward_kp()
{
	sFront = svFront.read();

	while (sFront > dist_threshold)
	{
		sLeft = svLeft.read();
		err = sLeft - dist_threshold;
		u = err * kp;

		mLeft(v - u);
		mRight(v + u);

		sFront = svFront.read();
		script.wait(10);

	}

	mLeft(0);
	mRight(0);

}


function rotate(angle)
{
	eLeft.reset();
	eRight.reset();
	left = eLeft.readRawData();
	right = eRight.readRawData();

	if (angle == 90) // turn right
	{
		while ((left + Math.abs(right))/2 < (pi * l / 4) / (pi * d) * cpr)
		{
			mLeft(100);
			mRight(-100);
			left = eLeft.readRawData();
			right = eRight.readRawData();
			script.wait(10);
		}
	}
	else
	{
		while ((Math.abs(left) + right)/2 < (pi * l / 4) / (pi * d) * cpr)
		{
			mLeft(-100);
			mRight(100);
			left = eLeft.readRawData();
			right = eRight.readRawData();
			script.wait(10);
		}
	}

	mLeft(0);
	mRight(0);

}

var main = function()
{
	__interpretation_started_timestamp__ = Date.now();

	rotate(-90);
	forward_kp();
	rotate(90);
	forward_kp();

	return;
}