#include <Servo.h>
#include "RobotControl.h"
bool mis = false;
//*************** zmiany nazw pinów *********************
#define trig 27
#define echo 26
//*****************zmienne globalne**********************
int dystans;
//detektor***
const int s0 = 53;  
const int s1 = 52;  
const int s2 = 51;  
const int s3 = 50;  
const int out = 49;   
int red = 0;  
int green = 0;  
int blue = 0;
//***
//Manipulator
Servo lokiec;
Servo ramie;
Servo dlon;
Servo nadgarstek;
Servo palce;
Servo bark;
int bark_home, ramie_home, dlon_home, nadgarstek_home, palce_home, lokiec_home;

void color()  {    
  digitalWrite(s2, LOW);  
  digitalWrite(s3, LOW);  
  red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);  
  digitalWrite(s3, HIGH);  
  blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);  
  digitalWrite(s2, HIGH);  
  green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);  
}

int zmierzOdleglosc() {
  long czas;
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
 
  czas = pulseIn(echo, HIGH);
  dystans = czas / 58;
 
  return dystans;
}

void Manipulator(){
  for (int angle = ramie_home; angle <= 120; angle++){
      ramie.write(angle);
      delay(50); 
  }
  
  for (int angle = dlon_home; angle >= 90; angle--){
      dlon.write(angle);   
      delay(50); 
  }

  for (int angle = 120; angle >= 100; angle--) {
      ramie.write(angle);
      delay(50); 
  }
 
  for (int angle = palce_home; angle >= 0; angle--) {
      palce.write(angle);
      delay(10); 
  }
}

void setup() { 
  randomSeed(analogRead(0)); //generator liczb losowych
  //****************************silniki DC****************************************
  Serial.begin (9600);
  pinMode(2, OUTPUT); //Sygnał PWM silnika nr 1
  pinMode(3, OUTPUT); //Sygnał PWM silnika nr 2
  pinMode(22, OUTPUT); //***Piny    //
  pinMode(23, OUTPUT); //Silnik 1*** //
  pinMode(24, OUTPUT); //***Piny    //
  pinMode(25, OUTPUT); //Silnik 2*** //
  pinMode(echo, INPUT); //***czujnik odległości    //
  pinMode(trig, OUTPUT); // HC-SR04*** //


  //**Ustawienienie sygnału PWM sterujący prędkością silników DC**\\ 
  digitalWrite(12, HIGH); //Ustawiam  prędkość na pinie 2
  digitalWrite(13, HIGH); //Ustawiam prędkość na pinie 3

  //********************************************************************************
  //                         detektor kolorów 
  pinMode(s0, OUTPUT);  
  pinMode(s1, OUTPUT);  
  pinMode(s2, OUTPUT);  
  pinMode(s3, OUTPUT);  
  pinMode(out, INPUT);   

  digitalWrite(s0, HIGH);  
  digitalWrite(s1, HIGH);  
  //**********************************************************************************
  //*******************************Manipulator****************************************
  bark.attach(2);
  ramie.attach(3);
  lokiec.attach(4);
  dlon.attach(5);
  palce.attach(6);
  
  bark_home = 0;
  bark.write(bark_home);
  ramie_home = 80;
  ramie.write(ramie_home);
  lokiec_home = 135;
  lokiec.write(lokiec_home);
  dlon_home = 180;
  dlon.write(dlon_home);
  palce_home = 90;
  palce.write(palce_home);
}

void rozpoznajKolor(){
  color();
  if (red < blue && red < green && red < 20)
  {
    if (red <= 10 && green <= 10 && blue <= 10)
    {
      Serial.println("WHILE");
    }
    else
    {
      Serial.println(" - (CZERWONY)");
      
    }
  }
  else if (blue < red && blue < green)
  {
    if (red <= 10 && green <= 10 && blue <= 10)
    {
      Serial.println("WHILE");
    }
    else
    {
      Serial.println(" - (NIEBIESKI)");
    }
  }
  else if (green < red && green < blue)
  {
    if (red <= 10 && green <= 10 && blue <= 10)
    {
      Serial.println("WHILE");
    }
    else
    {
      Serial.println(" - (ZIELONY)");
      
    }
  }
  else if (red < blue && green < blue && green < 10 && blue >= 9)
  {
    
    Serial.println(" - (ZOLTY)");
  }
  else if (red >= 20 && green >= 20 && blue >= 20) {
    
    Serial.println(" - (CZARNY)");
  }
     else if (red <= 7 && green <= 7 && blue <= 7)
  {
    
    Serial.println(" - (BIALY)");
  }
  else
  {
    Serial.println();
  }
}

void wykrywanieObiektow(){
  zmierzOdleglosc();
  jedzProsto();

  Serial.print(dystans);
  Serial.print(" G: " + Zielony());
  Serial.print(" C: " + Czerwony())

  if(dystans<16){   // parametr 16 wyznaczono doświadczalnie 
    czekaj();
    color();
    delay(1500);
    if(Zielony()) Zawroc();
    else if(Czerwony()) {
      czekaj();
      delay(1000);
      Manipulator();
      mis=true;
      Zawroc();
    }
    else czekaj();
  }
}


//*********************warunki kolorów***********************
bool Czerwony()
{
 color();
if (red < blue && red < green && red < 20)
  {
    if (red <= 10 && green <= 10 && blue <= 10)
    {
      Serial.println("WHILE");
      return false;
    }
    else
    {
     return true;
      
    }
  }

}

bool Bialy()
{
 color();
 if (red <= 10 && green <= 10 && blue <= 10)
    return true;
else
  return false;
}

bool Zielony()
{
   color();
  if (green < red && green < blue)
  {
    if (red <= 10 && green <= 10 && blue <= 10)
      return false;
    else
      return true;
  }
}
bool Zolty()
{
   color();
if (red < blue && green < blue && green < 10 && blue >= 9)
    return true;
else
    return false;
}


void loop() {
  if(mis == false){
    wykrywanieObiektow();
  }
  else {
    czekaj();
  }
}