/*
 Name:    ArduinoProject.ino
 Created: 10-May-18 6:11:07 PM
 Author:  Kula
*/
#include <ArduinoRobot.h>
#include <Wire.h>
#include <SPI.h>

#define DESNO 0
#define LIVO 1
#define NAZAD 2
#define NAPRID 3
#define TOLERANCE 10.0 //Prag tolerancije udaljenosti prepreke
#define TURN_DEG 40
#define SPEED 100

float tkValues[4]; //Sve vrijednosti u nizu
int FindMinValue(float *);
float convertSignalToCm(float);
void GoForward(int val);
void Turn(int d);
long microsecondsToCentimeters(long);
long Read();

const int pingPin = TKD1;

void setup()
{
  Robot.begin();
}

void loop()
{
  int dir = Obstacle(); //Vraca najblizu prepreku, tj njen index
  if (!(tkValues[dir] <= TOLERANCE)) //Ako je iznad tolerancije idi naprijed
     GoForward(SPEED);
  else
      Turn(dir); //Ako ne okreni se
  delay(100);
}

float convertSignalToCm(float s)
{
  return pow(3027.4 / s, 1.2134);
}

//vraća udaljenost prednjeg senzora
long Read()
{
  long duration, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  return microsecondsToCentimeters(duration);
}

int Obstacle()
{ //Sve vrijednosti u cm; popunjava sve vrijednosti senozora i stavlja ih u niz
  tkValues[DESNO] = convertSignalToCm(Robot.analogRead(TK0));
  tkValues[LIVO] = convertSignalToCm(Robot.analogRead(TK4));
  tkValues[NAZAD] = convertSignalToCm(Robot.analogRead(TK6));
  tkValues[NAPRID] = (float)Read();
  return FindMinValue(tkValues); //Vraca najblizu prepreku u cm
}

void GoForward(int val)
{
  Robot.motorsWrite(val, val);
}

void Turn(int d)
{
  Robot.motorsStop();
  switch(d) //Okreni se u smijeru suprotno smijru najblize prepreke
  {
    case NAPRID: //Prepreka je naprijed
        if (tkValues[LIVO] > tkValues[DESNO]) //Je li lijevo dalje od desno?
          Robot.turn(-TURN_DEG); //idi lijevo
        else
          Robot.turn(TURN_DEG);// idi desno
        break;
    case LIVO:
        Robot.turn(TURN_DEG/2);
        break;
    case DESNO:
        Robot.turn(-TURN_DEG/2);
        break;
    case NAZAD: //nebitno, idi naprijed
        GoForward(SPEED);
        break;
    default:
        break;
  }
}

int FindMinValue(float* tkValues)
{
  int i;
  int ind = 0;
  float min = tkValues[0];
  for (i=1;i<4;i++)
  {
    if (min > tkValues[i])
    {
      min = tkValues[i];
      ind = i;
    }
  }
  return ind;
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}