#include <motor.h>
#include <Wire.h>

#define MAX 9999999

Motor motor;

byte ir_sensors;
byte address[4];
int value[4];
float average = 0;
int idz;
int averages[4] = {0,0,0,0};
int index = 0, num = 0;
int deg[] = {720,30,0,30,60,90,60,30,0,30,60,90,60};
float max_speed = 120;
int kom;

void setup() {
    Serial.begin(9600);
    pinMode(20, INPUT);
    pinMode(21, INPUT);
    digitalWrite(20, HIGH);
    digitalWrite(21, HIGH);
    Wire.begin();

    Serial3.begin(38400);
    kom = compass();
}

void configure_compass() {
    Wire.beginTransmission(0x21);
    Wire.write(0x77);
    Wire.write(0x08);
    Wire.write(64 + 32 + 2);
    Wire.endTransmission();
}

int compass() {
    byte lsb = 0, msb = 0;
    int value;
    Wire.beginTransmission(0x21);
    Wire.requestFrom(0x21, 2);
    int wait = 100;
    while ((!Wire.available()) && (wait--)) delay(1);

    if (Wire.available())
    {
      msb = Wire.read();
      if (Wire.available())
          lsb = Wire.read();
    }
    else msb = lsb = 0;
    Wire.endTransmission();
    value = (msb << 8) | lsb;
    return value;
}

int counter = 0;

int yo_mama() {
    int ret;
    while (true) {
      for (int i = 1; i < 5; i++) {
          Serial3.write(i);
          delay(1);
          if (Serial3.available()) {
              ir_sensors = Serial3.read();
              value[i - 1] = ir_sensors & 0b00111111;
              address[i - 1] = ir_sensors >> 6;
              address[i - 1] &= 0b00000011;
          }
          Serial3.write(i);
          delay(1);
          if (Serial3.available()) {
              ir_sensors = Serial3.read();
              value[i - 1] += ir_sensors << 6;
          }
      }

      int mix = 999999;
      index = 0;
      for (int j = 0; j < 4; j++) {
          if (value[j] < mix) {
               mix = value[j];
               index = j;
          }
      }
      average += mix;
      if((index + 1) == 1) averages[0]++;
      else if((index + 1) == 2) averages[1]++;
      else if((index + 1) == 3) averages[2]++;
      else if((index + 1) == 4) averages[3]++;
      counter ++;
      if (counter == 10) {
          num = average / 10;
          average = 0;
          counter = 0;
          mix = 0;
          int result;
          for(int j = 0; j < 4;j++) {
              if(averages[j] > mix) {
                  mix = averages[j];
                  result = j + 1;
              }
          }
          mix = 9999999;
          if(value[result - 1] > 800) {
              for(int j = 0; j < 4;j++) {
                if(averages[j] < mix) {
                    mix = averages[j];
                    result = j + 1;
                }
              }
          }

          if (result == 1) ret = address[0] + 1;
          else if (result == 2) ret = address[1] + 4;
          else if (result == 3) ret = address[2] + 7;
          else if (result == 4) ret = address[3] + 10;
          for(int j = 0; j < 4;j++) {
              averages[j] = 0;
          }
          break;
      }
   }
   return ret;
}

inline void movement(int direction_nw, int speed_nw,int direction_ne,int speed_ne) {

  motor.go_pair(1,4,direction_nw,speed_nw);

  motor.go_pair(2,3,direction_ne,speed_ne);
}


void loop() {
    Serial.println(compass());
    while (!(compass() - 200 < kom && compass() + 200 > kom)) {
        if (compass() > kom)
            motor.rotate(1,50);
        else
            motor.rotate(0,50);
    }

    int sensor_number = yo_mama();
    int distance = num;
    /*if (distance > 2500) {
        if (sensor_number == 2) movement(1, max_speed, 0, max_speed);
        else if (sensor_number == 5)movement(0, max_speed, 0, max_speed);
        else if (sensor_number == 8)movement(0, max_speed, 1, max_speed);
        else if (sensor_number == 11)movement(1, max_speed, 1, max_speed);
    } else {
        switch(sensor_number)
        {
            case 1:
                    if (distance <= 700) {
                      movement(1, max_speed, 1, max_speed * 2 / 3);
                    } else {
                      movement(1, max_speed * 2 / 3, 1, max_speed);
                    }
                    break;
            case 2:
                    if (distance <= 700) {
                      movement(1, max_speed, 1, max_speed);
                    } else {
                      movement(1, max_speed, 0, max_speed/3);
                    }
                    break;
            case 3:
                    if (distance <= 700) {
                      movement(1, 0, 1, max_speed);
                    } else {
                      movement(1, max_speed * 2 / 3, 0, max_speed);
                    }
                    break;
            case 4:
                    if (distance <= 700) {
                      movement(0, max_speed, 1, max_speed);
                    } else {
                      movement(0, max_speed, 0, max_speed / 3);
                    }
                    break;
            case 5:
                    movement(0, max_speed, 0, max_speed);
                    break;
            case 6:
                    if (distance <= 700) {
                      movement(1, max_speed, 0, max_speed);
                    } else {
                      movement(0, max_speed / 3, 0, max_speed);
                    }
                    break;
            case 7:
                    if (distance <= 700) {
                      movement(1, max_speed, 1, 0);
                    } else {
                      movement(0, max_speed, 1 , max_speed * 2 / 3);
                    }
                    break;
            case 8:
                    if (distance <= 700) {
                      movement(1, max_speed, 1, max_speed);
                    } else {
                      movement(0, max_speed / 3, 1, max_speed);
                    }
                    break;
            case 9:
                    if (distance <= 700) {
                      movement(1, max_speed * 2 / 3, 1, max_speed);
                    } else {
                      movement(1, max_speed, 1, max_speed * 2 / 3);
                    }
                    break;
            case 10:
                    if (distance <= 700) {
                      movement(0, max_speed / 3, 1, max_speed);
                    } else {
                      movement(1, max_speed * 2/3, 1, max_speed);
                    }
                    break;
            case 11:
                   if(distance <= 700){
                     movement(1, max_speed, 0, max_speed);
                     }
                   else{
                     movement(1, max_speed, 0, 0);
                     }
                    break;
            case 12:
                    if (distance <= 700) {
                      movement(1, max_speed, 0, max_speed / 3);
                    } else {
                      movement(1, max_speed, 1, max_speed * 2/3);
                    }
                    break;
          }
      }*/
}