// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
// библиотека для работы с сервоприводами
#include <Servo.h> 

// создаём объект для работы с сервоприводом 
Servo myServo;

// даём разумное имя для пинов, к которым подключены светодиоды
#define INDICATOR_PIN_1  5
#define INDICATOR_PIN_2  6
// даём разумное имя для пина, к которому подключена кнопка
#define SERVO_PIN        10
// даём разумное имя для пина, к которому подключена кнопка
#define BUTTOM_PIN       11
// даём разумное имя для пина, к которому подключена пищалка
#define BUZZER_PIN       12

#define N  3

// множитель фильтра
#define BETA 0.22
// дрифт углов Эйлера
#define DRIFT_LIMIT  0.3
// компенсация точного местоположения объекта в пространстве
#define DRIFT_COMPRASION  10

// создаём объект для фильтра Madgwick
Madgwick filter;
 
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;
// создаём объект для работы с компасом
Compass compass;
 
// переменные для данных с гироскопа, акселерометра и компаса
float gx, gy, gz, ax, ay, az, mx, my, mz;
 
// получаемые углы ориентации (Эйлера)
float yaw, pitch, roll;
float prevYaw, prevPitch, prevRoll;

// переменная для хранения частоты выборок фильтра
float fps = 100;
 
unsigned long prevMillis;
unsigned long currMillis;

// создаём структуру
struct vector
{
  float x;
  float y;
  float z;
};

// переменная состояния
int flag = 0;

// калибровочные значения компаса
// полученные в калибровочной матрице из примера «compassCalibrateMatrixx»
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
 
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  {-0.018, 0.077, 1.782}
};


// состояния шкатулки:
// открыта, закрыта, ввод нового пароля, подтверждение нового пароля
enum State
{
    OPENED,
    CLOSED,
    SETPWD,
    CNFPWD,
};

// массив для хранения кода шкатулки
vector codeBox[3];
// массив для хранения кода подтверждения
vector confirmCodeBox[3];
// массив для хранения кода, при попытке открыть шкатулку
vector codeOpenBox[3];

// состояние шкатулки
State state;

void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  Serial.println("Begin init...");
  
  pinMode(INDICATOR_PIN_1, OUTPUT);
  pinMode(INDICATOR_PIN_2, OUTPUT);
  // подключаем сервопривод
  myServo.attach(SERVO_PIN);
  // инициализация акселерометра
  accel.begin();
  // инициализация гироскопа
  gyro.begin();
  // инициализация компаса
  compass.begin();
  // задаем начальное ненулевое значение в переменную
  prevMillis = millis();
  // калибровка компаса
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
  // даём время стабилизироваться объекту в пространстве
  while(!stayIMU(12000)) {
  }
  // выводим сообщение об удачной инициализации
  Serial.println("Initialization completed");
  // выбираем текущее состояние шкатулки
  state = OPENED;
}
 
void loop()
{
  // проверяем состояние шкатулки
  switch (state) {
    // шкатулка открыта
    case OPENED: {
      // открываем шкатулку
      open();
      // пока кнопка не нажата
      while (digitalRead(BUTTOM_PIN) == HIGH) {
        // ничего не делаем
      }
      // переходим в состоянии установки нового пароля
      state = SETPWD;
      break;
    }
    
    // шкатулка закрыта
    case CLOSED: {
      // закрываем шкаиулку
      close();
      // пока кнопка не нажата
      while (digitalRead(BUTTOM_PIN ) == HIGH){
        // следим за ориентацией объекта в пространстве
        readYawPitchRoll();
      }
      // считываем код, для открытия шкатулки
      readOpenCode();
      // проверяем правильный ли код
      if(checkOpenCode() == true) {
        // если код правильный
        Serial.println("Password accepted");
        for(int i = 1; i < 15; i++) {
          tone(BUZZER_PIN, 2000, 1000/i);
          if(readAzimut() == true) {
            Serial.println("Box is opened");
            open();
            return;
          }
          delay(2000/i);
        }
        Serial.println("Box is not opened");
      } else {
        Serial.println("Password not accepted");
        tone(BUZZER_PIN, 500, 2000);
      }
      break;
    }
    
    // установка нового кода
    case SETPWD: {
      // устанавливаем новый код
      setCode();
      // уходим на подтверждение
      state = CNFPWD;
      break;
    }
    
    // подтверждаем кодовую последовательность
    case CNFPWD: {
      // повторяем ввод нового кода
      setConfirmCode();
      if(checkConfirmCode() == true) {
        Serial.println("Code confirm accepted");
        tone(BUZZER_PIN, 2000, 300);
        close();
      }else {
        Serial.println("Code confirm not accepted");
        tone(BUZZER_PIN, 500, 2000);
        // повторяем ввод кода
        state = SETPWD;
      }
      break;
    }
    
  }
}

// считывание углов Эйлера
void readYawPitchRoll() {
 
  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с гироскопа в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // считываем данные с компаса в Гауссах
  compass.readCalibrateGaussXYZ(&mx, &my, &mz);
 
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
   
  // получение углов yaw, pitch и roll из фильтра
  yaw =  filter.getYawDeg();
  pitch = filter.getPitchDeg();
  roll = filter.getRollDeg();
}

// двигается ли шкатулка
bool stayIMU() {
  // запоминаем предыдущие значения углов для сглаживания мелкого дрифта
  float prevYaw = yaw;
  float prevPitch = pitch;
  float prevRoll = roll;
  
  // считываем углы Эйлера
  readYawPitchRoll();
  // проверяем дрифт углов
  if ((prevYaw -   yaw   < DRIFT_LIMIT && prevYaw - yaw     > -DRIFT_LIMIT) &&
      (prevPitch - pitch < DRIFT_LIMIT && prevPitch - pitch > -DRIFT_LIMIT) &&
      (prevRoll -  roll  < DRIFT_LIMIT && prevRoll - roll   > -DRIFT_LIMIT)) {
    // шкатулка остановилась
    return true;
  }
  else
    // катулка движется в пространстве 
    return false;
}

// двигается ли шкатулка с проверкой на время
bool stayIMU(int interval) {
  // текущее состояние шкатулки
  if (stayIMU() == false) {
    flag = 0;
    return false;
  }
  
  if (flag == 0) {
    // запоминаем текущее время
    prevMillis = millis();
    flag = 1;
    return false;
  }
  // ожидаем выдержку по времени
  if(flag == 1 && millis() - prevMillis > interval) {
    flag = 0;
    prevMillis = millis();
    return true;
  } else {
    return false;
  }
  prevMillis = millis();
  return false;
}

// установка кода
void setCode() {
  Serial.println("Set code:");
  for(int i = 0; i < N; i++) {
    while(!stayIMU(3000)) {
    }
    codeBox[i].x = yaw;
    codeBox[i].y = pitch;
    codeBox[i].z = roll;
    tone(BUZZER_PIN, 1000, 100);
    Serial.print("Saving set ");
    Serial.println(i+1);
  }
  delay(200);
  tone(BUZZER_PIN, 1000, 100);
  delay(200);
  tone(BUZZER_PIN, 1000, 100);
}

// подтверждение кода
void setConfirmCode() {
  Serial.println("Confirm code:");
  for(int i = 0; i < N; i++) {
    while(!stayIMU(3000)) {
    }
    confirmCodeBox[i].x = yaw;
    confirmCodeBox[i].y = pitch;
    confirmCodeBox[i].z = roll;
    tone(BUZZER_PIN, 1000, 100);
    Serial.print("Saving confirm ");
    Serial.println(i+1);
  }
  delay(200);
  tone(BUZZER_PIN, 1000, 100);
  delay(200);
  tone(BUZZER_PIN, 1000, 100);
}

// проверка кода
bool checkConfirmCode() {
  bool checkCode = true;
  for(int i = 0; i < N && checkCode == true; i++) {
    if ((abs(codeBox[i].x) - abs(confirmCodeBox[i].x) < DRIFT_COMPRASION && abs(codeBox[i].x) - abs(confirmCodeBox[i].x) > -DRIFT_COMPRASION) &&
        (abs(codeBox[i].y) - abs(confirmCodeBox[i].y) < DRIFT_COMPRASION && abs(codeBox[i].y) - abs(confirmCodeBox[i].y) > -DRIFT_COMPRASION) &&
        (abs(codeBox[i].z) - abs(confirmCodeBox[i].z) < DRIFT_COMPRASION && abs(codeBox[i].z) - abs(confirmCodeBox[i].z) > -DRIFT_COMPRASION)) {
      checkCode = true;
    } else {
      checkCode = false;
    }
  }
  return checkCode;
}

// считывание пароля для открытия шкатулки
void readOpenCode() {
  for(int i = 0; i < N; i++) {
    while(!stayIMU(3000)) {
    }
    codeOpenBox[i].x = yaw;
    codeOpenBox[i].y = pitch;
    codeOpenBox[i].z = roll;
    tone(BUZZER_PIN, 1000, 100);
    delay(200);
    Serial.print("Save ");
    Serial.println(i+1);
  }
}

// проверка введённого пароля с паролем шкатулки
bool checkOpenCode() {
  bool checkPass = true;
  for(int i = 0; i < N && checkPass == true; i++) {
    if ((abs(codeOpenBox[i].x) - abs(codeBox[i].x) < DRIFT_COMPRASION && abs(codeOpenBox[i].x) - abs(codeBox[i].x) > -DRIFT_COMPRASION) &&
        (abs(codeOpenBox[i].y) - abs(codeBox[i].y) < DRIFT_COMPRASION && abs(codeOpenBox[i].y) - abs(codeBox[i].y) > -DRIFT_COMPRASION) &&
        (abs(codeOpenBox[i].z) - abs(codeBox[i].z) < DRIFT_COMPRASION && abs(codeOpenBox[i].z) - abs(codeBox[i].z) > -DRIFT_COMPRASION)) {
      checkPass = true;
    } else {
      checkPass = false;
    }
  }
  return checkPass;
}

// проверка напраление шкатулки относительно севера
bool readAzimut() {
  bool checkAzimut = true;
  float x = compass.readAzimut();
  Serial.println(x);
  if (x < DRIFT_COMPRASION && x > -DRIFT_COMPRASION){
    checkAzimut = true;
  } else {
    checkAzimut = false;
  }
  return checkAzimut;
}

// вывод углов Эйлера в serial-порт
void printYawPitchRoll() {
  Serial.print("yaw: ");
  Serial.print(yaw);
  Serial.print("\t\t");
  Serial.print("pitch: ");
  Serial.print(pitch);
  Serial.print("\t\t");
  Serial.print("roll: ");
  Serial.println(roll);
}

// Функция отпирает замок
void open(void) {
  digitalWrite(INDICATOR_PIN_1, HIGH);
  digitalWrite(INDICATOR_PIN_2, HIGH);
  myServo.write(0);
  state = OPENED;
}

// функция запирает замок
void close(void) {
  digitalWrite(INDICATOR_PIN_1, LOW);
  digitalWrite(INDICATOR_PIN_2, LOW);
  myServo.write(70);
  state = CLOSED;
}