#include <ZumoMotors.h>
#include <Wire.h>
#include <LSM303.h>
#define CRB_REG_M_2_5GAUSS 0x60 // CRB_REG_M value for magnetometer +/-2.5 gauss full scale
#define CRA_REG_M_220HZ    0x1C // CRA_REG_M value for magnetometer 220 Hz update rate
float minvals[8];
float maxvals[8];
int grays[8];
int rawvals[8];
int sensors[8];
int Line;
int error = 0;
int lastError = 0;
float Kp = 1;
float Kd = 6;
int sharp;
//float kk=0.0001;
#define SPEED 150
#define GREEN_HIGH 45
#define GREEN_LO 25
bool Green_Right = false;
bool Green_Left = false;
bool Cross = false;
long greentime = 0; //начало зеленого участка
long first_greenTime = 0;
#define GreenPeriod 1500 //3 сек помним про зеленый, затем сбрасываем
ZumoMotors motors;
LSM303 compass;

bool test = false;

void setup() {
  // put your setup code here, to run once:
  
  Serial.begin(115200);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  compass.writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
  compass.writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ);    // 220 Hz compass update rate
  
  pinMode(A0, INPUT); // настраиваем выводы
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  pinMode(A6, INPUT);
  pinMode(A7, INPUT);
   pinMode(A11, INPUT);
  delay(1000);
  Calibrate();
  delay(3000);
}

void showsensors()
{
//  if(sharp>200)
// Serial.println("sharp"); 
// else
// Serial.println("");
 /*Serial.print(sensors[0]);
 Serial.print(" | ");
 Serial.print(sensors[1]);
 Serial.print(" | ");
 Serial.print(sensors[2]);
 Serial.print(" | ");
 Serial.print(sensors[3]);
 Serial.print(" | ");
 Serial.print(sensors[4]);
 Serial.print(" | ");
 Serial.print(sensors[5]);
 Serial.print(" | ");
 Serial.print(sensors[6]);
 Serial.print(" | ");
 Serial.println(sensors[7]);*/
 //Serial.print(rawvals[0]);
//Serial.print(" | ");
//Serial.print(rawvals[1]);
//Serial.print(" | ");
//Serial.print(rawvals[2]);
//Serial.print(" | ");
//Serial.print(rawvals[3]);
//Serial.print(" | ");
//Serial.print(rawvals[4]);
//Serial.print(" | ");
//Serial.print(rawvals[5]);
//Serial.print(" | ");
//Serial.print(rawvals[6]);
//Serial.print(" | ");
//Serial.println(rawvals[7]);
}

void loop() {
 sharp=analogRead(A11);
GetLine();

if (test) 
{
  showsensors();
  }
else
{
if (Cross == true)
{
  motors.setSpeeds(0, 0);
  delay(100);
  step_back();
  delay(200);
  CheckGreen2();
  delay(100);
 if (Green_Left && !Green_Right) {cross_step(150); turn_left(); }
 else if (!Green_Left && Green_Right) {cross_step(150); turn_right();}
 else if(Green_Left && Green_Right)
 {
  turn_round();
  }
 else cross_step(500);
 // else if (Green_Left && Green_Right) {Serial.println("ROUND");turn_round();}
Cross = false;  
delay(200);
}
if(sharp>280)
{
 gosharp(); 
  }
error = Line/2;
int speedDifference = Kp * error + Kd * (error - lastError);//+kk*error*error*error;
lastError = error;

int m1Speed = SPEED + speedDifference;
int m2Speed = SPEED - speedDifference;

if (m1Speed < 0)
    m1Speed = 0;
  if (m2Speed < 0)
    m2Speed = 0;
  if (m1Speed > SPEED)
    m1Speed = SPEED;
  if (m2Speed > SPEED)
    m2Speed = SPEED;
    
 motors.setSpeeds(-m2Speed, -m1Speed);

}
}

void cross_step(int t)
{
  long start_time = millis();
  while (millis() < start_time + t)
   motors.setSpeeds(-SPEED, -SPEED); 
  motors.setSpeeds(0, 0);  
}

void step_back()
{
  long start_time = millis();
  while (millis() < start_time + 70)
   motors.setSpeeds(SPEED, SPEED); 
  motors.setSpeeds(0, 0);  
}


void turn_left()
{
  long StartRotate = millis();
  while (millis() < StartRotate + 300)
   motors.setSpeeds(SPEED, -SPEED);
   
 motors.setSpeeds(0, 0);
}


void turn_right()
{
  long StartRotate = millis();
  while (millis() < StartRotate + 300)
   motors.setSpeeds(-SPEED, SPEED);
   
 motors.setSpeeds(0, 0);
}


void turn_round()
{
  long StartRotate = millis();
  while (millis() < StartRotate + 600)
   motors.setSpeeds(SPEED, -SPEED);
   
 motors.setSpeeds(0, 0);
}

void turn_left_m(int t, int v)
{
  long StartRotate = millis();
  while (millis() < StartRotate + t)
   motors.setSpeeds(v, -v);
   
 motors.setSpeeds(0, 0);
}


void turn_right_m(int t, int v)
{
  long StartRotate = millis();
  while (millis() < StartRotate + t)
   motors.setSpeeds(-v, v);
   
 motors.setSpeeds(0, 0);
}

void gosharp()
{
   motors.setSpeeds(0,0);
   delay(100);
   turn_left_m(350, 150);
   delay(100);
  cross_step(300);
  delay(100);
   turn_right_m(350, 150);
   delay(100);
   cross_step(650);
   delay(100);
    turn_right_m(350, 150);
    delay(100);
    GetLine();
    while (sensors[0] < 60 && sensors[1] < 60 && sensors[2] < 60 && sensors[3] < 60 && sensors[4] < 60 && sensors[5] < 60 && sensors[6] < 60 && sensors[7] < 60)
    {
       GetLine();
        motors.setSpeeds(-100,-100);
      }
      motors.setSpeeds(0,0);
      delay(150);
    cross_step(200);  
    delay(150);
    turn_left();  
  }
  
void Calibrate()
{
 long _start = millis();
 motors.setSpeeds(100, -100);
 LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767};
  unsigned char index;
//обнуляем значения
for (int i = 0; i < 8; i++)
   {
    minvals[i] = 1000;
    maxvals[i] = 0;
   }
 while (millis() - _start < 5000)
 {
    compass.read();

    running_min.x = min(running_min.x, compass.m.x);
    running_min.y = min(running_min.y, compass.m.y);

    running_max.x = max(running_max.x, compass.m.x);
    running_max.y = max(running_max.y, compass.m.y);

   GetRawSensors();
   for (int i = 0; i < 8; i++)
   {
    minvals[i] = min(minvals[i], rawvals[i]);
    maxvals[i] = max(maxvals[i], rawvals[i]);
   }
 }  
 motors.setSpeeds(0, 0);
   for (int i = 0; i < 8; i++)
   {
    grays[i] = (minvals[i] + maxvals[i])/2;
   } 
   
  // Set calibrated values to compass.m_max and compass.m_min
  compass.m_max.x = running_max.x;
  compass.m_max.y = running_max.y;
  compass.m_min.x = running_min.x;
  compass.m_min.y = running_min.y;  
}

void GetRawSensors()
{
 rawvals[0] = analogRead(A7);
 rawvals[1] = analogRead(A3);
 rawvals[2] = analogRead(A4);
 rawvals[3] = analogRead(A5);
 rawvals[4] = analogRead(A1);
 rawvals[5] = analogRead(A6);
 rawvals[6] = analogRead(A2);
 rawvals[7] = analogRead(A0);
}

void CheckGreen2()
{
  GetLine();
  Green_Left = false;
  Green_Right = false;
  int left = sensors[0] + sensors[1] + sensors[2];
  int right = sensors[7] + sensors[6] + sensors[5];
  if (left - (right) > 40) Green_Left = true;
  if ((right) - left > 40) Green_Right = true;
  if(Green_Left == false && Green_Right == false)
  {
    if(left+right>=170)
    {
      Green_Right = true;
      Green_Left = true;
    }
    }
  Serial.print("Left: ");
  Serial.println(left);  
  Serial.print("Right: ");
  Serial.println(right);  

}

void CheckGreen()
{
if (!Cross)
 { 
  if (sensors[0] > GREEN_LO && sensors[0] < GREEN_HIGH && sensors[1] > GREEN_LO && sensors[1] < GREEN_HIGH && sensors[2] > GREEN_LO && sensors[2] < GREEN_HIGH)
   {
   // if (first_greenTime ==0) first_greenTime = millis();
   // if  (millis() < first_greenTime + 100) 
     if (!Green_Left && !Green_Right)
     {Green_Left = true;
      greentime = millis();
      }
   } 
   else
   if (millis() > greentime + GreenPeriod)
    {
      Green_Left = false;
      first_greenTime = 0;
    }

  if (sensors[7] > GREEN_LO && sensors[7] < GREEN_HIGH && sensors[6] > GREEN_LO && sensors[6] < GREEN_HIGH && sensors[5] > GREEN_LO && sensors[5] < GREEN_HIGH)
   {
  //  if (first_greenTime ==0) first_greenTime = millis();
   //if  (millis() < first_greenTime + 100) 
    if (!Green_Left && !Green_Right)
    {Green_Right = true;
    greentime = millis();
    }
   }
   else
   if (millis() > greentime + GreenPeriod)
    { 
      Green_Right = false;
      first_greenTime = 0;
    }  
}
}
 
void GetSensors()
{
  GetRawSensors();
  for (int i = 0; i < 8; i++)
   {
    //float s = 100 * (max((rawvals[i]-minvals[i]),0)/(maxvals[i]-minvals[i]));
    float s = map(rawvals[i], minvals[i], maxvals[i], 0, 100);
    sensors[i] = s;
   }
}

void GetLine()

{
   GetSensors();
   int S3 = sensors[3];
   int S4 = sensors[4];
   int S2 = sensors[2];
   if (S2 < 50) S2 = 0;
   int S5 = sensors[5];
   if (S5 < 50) S5 = 0;
   int S1 = sensors[1];
   if (S1 < 50) S1 = 0;
   int S6 = sensors[6];
   if (S6 < 50) S6 = 0;
   int S0 = sensors[0];
   if (S0 < 50) S0 = 0;
   int S7 = sensors[7];
   if (S7 < 50) S7 = 0;   
  //для первого датчика коэф равен 1, второго 2, третьего 3, четвертого 4
  Line = (S3 + 2* S2 + 3*S1+4*S0 - S4- 2* S5 - 3*S6 - 4*S7);
  
   if (S0 > 60 && S1 > 60 && S2 > 60 && S3 > 60 && S4 > 60 && S5 > 60 && S6 > 60 && S7 > 60)
    Cross = true;

}

