#pragma config(Sensor, S2,     colorArray,     sensorEV3_GenericI2C)
#pragma config(Sensor, S3,     pravy,          sensorEV3_Ultrasonic)
#pragma config(Sensor, S4,     lavy,           sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          lavyMotor,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          pravyMotor,    tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorD,          fotenie,       tmotorEV3_Medium, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "./include/mindsensors-lightsensorarray.h"

#define MSLS_SCALE     100
#define MSLS_KP          7  //14
#define MSLS_KD         6   //10
#define BASE_SPEED      20

#define MAX_SPEED			 100
#define MSLS_SENSORS 		 8
#define MSLS_BLACK      40
#define MSLS_RIGHT     	MSLS_SENSORS * MSLS_SCALE
#define MSLS_ON_LINE    MSLS_SENSORS / 2 * MSLS_SCALE
#define MSLS_LEFT     	0
#define MSLS_OUT        10 * MSLS_SCALE
#define MSLS_BORDER     2 * MSLS_SCALE


tByteArray signalstr;
int lastValue = 0;
int speed = 0;
int hodnotaPravy;
int hodnotaLavy;
int HodnotaVidenia = 30;
int a = 35;
int on_thing = 0;
unsigned long casVidenia = 0;
int casBlokovania = 5000;



int readLine(ubyte *values)
{
	unsigned char i, on_line = 0;
	unsigned long avg = 0;
	unsigned int sum = 0;

	if (!MSLSAreadSensors(colorArray, values))
	{
		return MSLS_ON_LINE;
	}

	for (i=0;i<MSLS_SENSORS;i++) {
		int value = (short)values[i]+1;
		if (value < MSLS_BLACK) {
			on_line = 1;
		}

		// only average in values that are above a noise threshold
		if (value < MSLS_BLACK)
		{
			avg += (long)(value) * (i * MSLS_SCALE);
			sum += value;
		}
	}

	if (!on_line)
	{
		//		if (sum == 0) return MSLS_OUT;

		// If it last read to the left of center, return 0.
		if(lastValue < (MSLS_SENSORS-1)*MSLS_SCALE/4)
		{
			setLEDColor(ledGreen);
			return MSLS_LEFT;
		}

		// If it last read to the right of center, return the max.
		else if(lastValue > 3*(MSLS_SENSORS-1)*MSLS_SCALE/4)
		{
			setLEDColor(ledRed);
			return MSLS_RIGHT ;
		}

		else
		{
			setLEDColor(ledOrange);
			return 400;
		}

	}
	setLEDColor(ledOff);

	if (sum > 0)
		lastValue = avg/sum;
	else
		lastValue = MSLS_OUT;

	return lastValue;
}

void init()
{
	MSLSAinit(colorArray);

	// Calibrate ...
	displayTextLine(1, "Biela farba ... (press key)");
	waitForButtonPress();
	flushButtonMessages();
	_MSLSAsendCommand(colorArray,MSLDA_CMD_CALIB_WHITE);
	displayTextLine(1, "Biela farba ... nakalibrovana ...");
	displayTextLine(2, "Cierna farba ... (press key)");
	waitForButtonPress();
	flushButtonMessages();
	_MSLSAsendCommand(colorArray,MSLSA_CMD_CALIB_BLACK);
	displayTextLine(2, "Cierna farba ... nakalibrovana ...");

	displayTextLine(4, "Press key to start");
	waitForButtonPress();
	flushButtonMessages();
	eraseDisplay();
}

task main()
{
	int position, error, lastError = readLine(&signalstr[0]);
	int baseSpeed = BASE_SPEED;
	int leftSpeed = BASE_SPEED;
	int rightSpeed = BASE_SPEED;

	init();
	setMotorSpeed(lavyMotor, 0);
	setMotorSpeed(pravyMotor, 0);
				setMotorSpeed(motorD, -a);
			wait1Msec(100);
			setMotorSpeed(motorD, 0);

	while (true)
	{
		hodnotaPravy =getUSDistance(pravy);
		hodnotaLavy =getUSDistance(lavy);
		if (hodnotaPravy > HodnotaVidenia && hodnotaLavy > HodnotaVidenia && on_thing == 0)
		{
			on_thing = 0;
		}
		else if((nSysTime - casVidenia) > casBlokovania && hodnotaPravy < HodnotaVidenia && on_thing == 0)
		{
			setMotorSpeed(lavyMotor, 0);
			setMotorSpeed(pravyMotor, 0);
			setMotorSpeed(motorD, a);
			wait1Msec(200);
			setMotorSpeed(motorD, 0);
			setMotorSpeed(motorD, -a);
			wait1Msec(100);
			setMotorSpeed(motorD, 0);
			wait1Msec(1000);
			casVidenia = nSysTime;
			//setMotorSpeed(lavyMotor, baseSpeed);
			//setMotorSpeed(pravyMotor, baseSpeed);
		}
		else if((nSysTime - casVidenia) > casBlokovania && hodnotaLavy < HodnotaVidenia && on_thing == 0)
		{
			setMotorSpeed(lavyMotor, 0);
			setMotorSpeed(pravyMotor, 0);
			setMotorSpeed(motorD, a);
			wait1Msec(200);
			setMotorSpeed(motorD, 0);
			setMotorSpeed(motorD, -a);
			wait1Msec(100);
			setMotorSpeed(motorD, 0);
			wait1Msec(1000);
			casVidenia = nSysTime;
		}
		else if(on_thing == 1)
		{
		if(hodnotaPravy < HodnotaVidenia || hodnotaLavy < HodnotaVidenia)
		{
		position = readLine(&signalstr[0]);
		displayTextLine(6, "Pozicia: %d", position);
		displayTextLine(4, "A: %d", a);
		if (position < MSLS_OUT)										// folow line
		{

			// 0 ... on line, -X ... on left side, +X ... on right side
			error = position - MSLS_ON_LINE;
			speed = MSLS_KP /(float)MSLS_SCALE * error + MSLS_KD /(float)MSLS_SCALE * (error - lastError);
			displayTextLine(2, "Err: %d, LastErr: %d", error, lastError);
			lastError = error;
			rightSpeed = baseSpeed + speed;
			leftSpeed = baseSpeed - speed;
			//		}
		}
		else																						// no line, continue straight
		{
			rightSpeed = baseSpeed;
			leftSpeed = baseSpeed;
		}
		if (rightSpeed > MAX_SPEED) rightSpeed = MAX_SPEED;
		if (leftSpeed > MAX_SPEED) leftSpeed = MAX_SPEED;
		if (rightSpeed < -1*MAX_SPEED) rightSpeed = -1*MAX_SPEED;
		if (leftSpeed < -1*MAX_SPEED) leftSpeed = -1*MAX_SPEED;
		setMotorSpeed(lavyMotor, leftSpeed);
		setMotorSpeed(pravyMotor, rightSpeed);
		}
		else
		{
		on_thing = 0;
		}
		}


		//////////////////////////////////////////////////////////////////////////////////////////////////////////////
		// MSLS_ON_LINE ... on line, 0 ... on left side, 800 ... on right side,
		position = readLine(&signalstr[0]);
		displayTextLine(6, "Pozicia: %d", position);
		displayTextLine(4, "A: %d", a);
		if (position < MSLS_OUT)										// folow line
		{

			// 0 ... on line, -X ... on left side, +X ... on right side
			error = position - MSLS_ON_LINE;
			speed = MSLS_KP /(float)MSLS_SCALE * error + MSLS_KD /(float)MSLS_SCALE * (error - lastError);
			displayTextLine(2, "Err: %d, LastErr: %d", error, lastError);
			lastError = error;
			rightSpeed = baseSpeed + speed;
			leftSpeed = baseSpeed - speed;
			//		}
		}
		else																						// no line, continue straight
		{
			rightSpeed = baseSpeed;
			leftSpeed = baseSpeed;
		}
		if (rightSpeed > MAX_SPEED) rightSpeed = MAX_SPEED;
		if (leftSpeed > MAX_SPEED) leftSpeed = MAX_SPEED;
		if (rightSpeed < -1*MAX_SPEED) rightSpeed = -1*MAX_SPEED;
		if (leftSpeed < -1*MAX_SPEED) leftSpeed = -1*MAX_SPEED;
		setMotorSpeed(lavyMotor, leftSpeed);
		setMotorSpeed(pravyMotor, rightSpeed);
	}
}