/*
   Как работает алгоритм:
   Серво крутится от MIN_ANGLE до MAX_ANGLE с шагом 1 градус
   Каждые 2 градуса стреляем дальномером. Перед началом работы
   калибруемся: сканируем пространство и составляем "карту"
   рабочей области, получаем массив расстояний.
   В режиме seek крутимся и сравниваем значения с измеренными.
   Если находим расхождение в DEADZONE, начинаем считать, сколько
   будет таких точек. Если их больше MIN_CATCH - считаем за "цель".
   Таким образом фильтруем случайные маленькие объекты и шумы. Также
   прощаем системе несколько ошибок (MISTAKES) во время сканирования цели.
   В итоге получаем угол начала "цели" и угол конца. Ищем средний угол
   и поворачиваемся на него, то есть в центр цели, это режим hold.
   Далее в режиме удержания цели измеряем расстояние, если выпадаем из
   DEADZONE - переходим к поиску цели.
*/

#include <Servo.h>.

 #include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lox = Adafruit_VL53L0X();


// ------------- НАСТРОЙКИ --------------
#define STEP_DELAY 15   // скорость серво (меньше 5 не ставить, сонар тупит)
#define TIMEOUT 3000    // таймаут на новый поиск цели из режима удержания
#define MAX_ANGLE 135  // максимальный угол поворота
#define MIN_ANGLE 45   	// минимальный угол поворота
#define DIST_MAX 400    	// максимальное расстояние (см). Датчик бьёт до 4 метров, но будут шумы

#define DEADZONE 10     // зона нечувствительности (мин. разность с калибровкой)
#define MIN_CATCH 8    // мин. количество точек подряд, чтобы  считать цель целью
#define MISTAKES 4      // допустимое количество пропусков при сканировании цели
#define SrvAmount 7  
// ------------- НАСТРОЙКИ --------------

// Includes the Servo library


int SrvPins[7] = {6, 7, 8, 9, 10, 11, 12}; 
int DefAngle[7] = {90, 90, 27, 90, 42, 40, 90};  //Положение по умолчанию

int ModeAmount=7;  //Количество режимов
int Srv = 0; //Дефолтное значение
int value = 0; //Дефолтное значение
int ServoCur=0;           // Текущий серво 

Servo sdrive[SrvAmount]; //Приводы
float maxdist = 400;    

int16_t Xpos;
int16_t Ypos;
int16_t Zpos;
int16_t Qpos;
int16_t a0;
int16_t a1;
int16_t a2;
int16_t a3;
int16_t a4;

// ---------- ПИНЫ ----------
//#define ECHO 5
//#define TRIG 4
//#define SERVO 3
//#define MOS 2
// ---------- ПИНЫ ----------

// ---------- ДЛЯ РАЗРАБОТЧИКОВ ----------
//GTimer stepTimer(STEP_DELAY);
//GTimer sonarTimer(100);
//GTimer timeoutTimer(TIMEOUT);
//NewPing sonar(TRIG, ECHO, DIST_MAX);
boolean direct;
//boolean next;
const byte steps_num = (MAX_ANGLE - MIN_ANGLE) / 2;
int rangle = MIN_ANGLE;
int distance[steps_num + 1];
int dist;
int ddist=40;
//boolean catch_flag, catched_flag, hold_flag;
//byte catch_num;
//byte mistakes;
//byte mode = true;
//byte catch_pos;
//int hold_signal;
// ---------- ДЛЯ РАЗРАБОТЧИКОВ ----------
byte prg1[32][2]= {
{100, 10},
{100, 20},
{100, 30},
{100, 40},
{100, 50},
{100, 60},
{100, 70},
{100, 80},
{100, 90},
{100, 100},
{110, 100},
{120, 100},
{130, 100},
{120, 100},
{110, 100},
{100, 100},
{90, 100},
{80, 100},
{70, 100},
{60, 100},
{50, 100},
{40, 100},
{30, 100},
{20, 100},
{30, 100},
{40, 100},
{50, 100},
{60, 100},
{70, 100},
{80, 100},
{90, 100},
{100, 100}
};


void setup() {
  Serial.begin(9600);
  while (! Serial) {
    delay(1);
  }
 /* 
  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    while(1);
  }
  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 
*/
for (int i = 0; i < 7; i++) {
    value=DefAngle[i];
    sdrive[i].attach(SrvPins[i]);
   sdrive[i].write(value);
 // Serial.println(value);
    delay(STEP_DELAY * 10);
    }  
Serial.println("-------------init drive done-----------");
calibration();
}

void loop() {
//Serial.println("-------------loop----------");
//seek();
proga();
///
 //|if (mode) seek();       // режим поиска цели
 //else hold();            // режим удержания позиции
 
}


void seek(){
  for (rangle = MIN_ANGLE; rangle <= MAX_ANGLE; rangle++) {
    sdrive[6].write(rangle);
    delay(STEP_DELAY * 1.5);

    measure();
      
    // Serial.print("Distance (mm): "); Serial.println(dist);
    search(); 
  }
 // delay(500);  
  for (rangle = MAX_ANGLE; rangle >= MIN_ANGLE; rangle--) {
    sdrive[6].write(rangle);
    delay(STEP_DELAY * 1.5);

  measure();

 //  Serial.print("Distance (mm): "); Serial.println(dist);
    search(); 
  }
  delay(500);  
}

void search(){

 //  Serial.print("angle ");  Serial.print(rangle); 
//  //     Serial.print(" dist");  Serial.println(dist);
 //      if (dist<=125) {delay(3000);}
}
void proga(){


  
 for (int prg = 0; prg < ((sizeof(prg1)/2)); prg++) {
    
  //  arm[(prg1[prg][0])].write(prg1[prg][1]);
   
    Xpos=prg1[prg][0];
    Ypos=prg1[prg][1];
    // Serial.println(prg1[prg][1]); 
    kynematic();
    //RotateServo();
    delay(500);
    }

}


/*


void hold() {
  if (!hold_flag)           // движение к середине цели
    turn_to(catch_pos);     // крутим серво
  else {                    // расчётная точка достигнута

    if (sonarTimer.isReady()) {                   // отдельный таймер сонара
      byte pos = (angle - MIN_ANGLE) / 2;         // перевод градусов в элемент массива
      int curr_dist = sonar.ping_cm();            // получаем сигнал с датчика
      if (curr_dist == 0) curr_dist = DIST_MAX;   // 0 это не только 0, но и максимум
      int diff = distance[pos] - curr_dist;       // ищем разницу
      if ((diff < DEADZONE) || (curr_dist > 1 && curr_dist < 10)) {   // если вышли из зоны ИЛИ поднесли руку на 1-10 см
        if (timeoutTimer.isReady())               // отработка таймаута
          mode = true;                            // если цель потеряна и вышло время - переходим на поиск
        hold_flag = false;
      } else {                                    // если цель всё ещё в зоне видимости
        timeoutTimer.reset();                     // сбросить таймер
      }
    }
  }
}

void search() {
  if (angle % 2 == 0 && next) {                 // каждые 2 градуса
    next = false;
    byte pos = (angle - MIN_ANGLE) / 2;         // перевод градусов в элемент массива
    int curr_dist = sonar.ping_cm();
    if (curr_dist == 0) curr_dist = DIST_MAX;
    int diff = distance[pos] - curr_dist;
    if (diff > DEADZONE) {                      // разность показаний больше мертвой зоны
      if (!catch_flag) {
        catch_flag = true;        // флаг что поймали какое то значение
        catch_pos = angle;        // запомнили угол начала предполагаемой цели
      }
      catch_num++;                // увеличили счётчик значений
      if (catch_num > MIN_CATCH)  // если поймали подряд много значений
        catched_flag = true;      // считаем, что это цель
    } else {                                    // если "пусто"
      if (catch_flag) {               // если ловим цель
        if (mistakes > MISTAKES) {    // если число ошибок превысило допустимое
          // сбросить всё
          catch_flag = false;
          catched_flag = false;
          catch_num = 0;
          mistakes = 0;
        } else {
          mistakes++;                 // увеличить число ошибок
        }
      }

      if (catched_flag) {             // поймали цель! 
        mode = false;                 // переход в режим удержание
        // catch_pos - середина цели. Считаем по разному, если двигались вперёд или назад
        if (direct) catch_pos += catch_num;
        else catch_pos -= catch_num;
        
        // сбросить всё
        hold_flag = false;
        catch_flag = false;
        catched_flag = false;
        catch_num = 0;
        mistakes = 0;        
      }
    }
  }
}
*/
void calibration() {
  Serial.println("-------------radar calibration-----------");
  // пробегаемся по рабочему диапазону
  for (rangle = MIN_ANGLE; rangle <= MAX_ANGLE; rangle++) {
    sdrive[6].write(rangle);
    delay(STEP_DELAY * 1.5);
    if (rangle % 2 == 0) {   // заполняем массив расстояний
      byte pos = (rangle - MIN_ANGLE) / 2;
      measure();
      if (dist == 0) distance[pos] = DIST_MAX;
      else distance[pos] = dist;
    }
    delay(STEP_DELAY * 1.5);
   // Serial.print("Distance (mm): "); Serial.println(dist);
   }
   Serial.println("-------------radar calibration done-----------");
  }
 







 




void measure() {
/*
VL53L0X_RangingMeasurementData_t measure;
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    dist=measure.RangeMilliMeter;
    } else {
    Serial.println(" out of range ");
  }
  */
}
 
 
void kynematic(){
 //координаты из массива
 // Xpos = _gtv6;
  //Ypos = _gtv7;
  //Zpos = _gtv8;
  //Qpos = _gtv9;
  //--------------eof
  
  int Xp = Xpos;
  int Yp = Ypos;
  int Zp = Zpos - 50; // скорректированная координата Z  mm
  int Qp = Qpos;
  a0 = atan2(Yp, Xp)*57.3; // Угол поворота 0-й оси относительно выбранного вами 0 ° начала координат
        int Xt = sqrt(pow(Xp, 2) + pow(Yp, 2)); // Координата X целевой  точки в основной плоскости движения звеньев 60, 90 и 10
        int dX = 10*sin(Qp/57.3); // Корректировка X на ориентацию 3-го звена (10)
        int dY = 10*cos(Qp/57.3); // Корректировка Y на ориентацию 3-го звена (10)
        int Xn = Xt - dX; // Позиция X в основной плоскости, куда должен попасть шарнир 3-го звена
        int Yn = Zp + dY; // Позиция Y в основной плоскости, куда должен попасть шарнир 3-го звена
        int d = sqrt(pow(Xn, 2) + pow(Yn, 2)); // Длина отрезка между 1-м и 3-м  шарнирами
        int q1 = atan2(Yn, Xn)*57.3; // Вспомогательный угол 1
        float sq2 = (pow(d, 2) + pow(60, 2) - pow(90, 2)) / (2 * 60 * d);
        int q2 = acos(sq2)*57.3;
        float sq3 = (pow(60, 2) + pow(90, 2) - pow(d, 2)) / (2 * 60 * 90);
        int q3 = acos(sq3)*57.3;
        a1 = q1 + q2; // Угол поворота сервомотора 1-й оси относительно горизонтали
        a2 = 180 - q3; // Угол поворота сервомотора 2-й оси относительно 1-й
        a3 = 180 - q2 - q3 - q1 + Qp; // Угол поворота 3-й оси относительно перпендикуляра к 2-й оси 
        a4 = 0; // Угол поворота захвата 
       


        sdrive[1].write(a1);sdrive[2].write(a2);sdrive[3].write(a3);
        
        



       Serial.print("a1-");Serial.print(a1);
       Serial.print("  a2-");Serial.print(a2);
 Serial.print("  a3-");Serial.print(a3);
  Serial.print("  a4-");Serial.println(a4);

       // координаты для серв
       // _gtv11 = a4;
       // _gtv10 = a3;
       // _gtv3 = a2;
       // _gtv2 = a1;
       // _gtv1 = a0;

}