#include "steppers.h"
#include "leds.h"
#include "paths.h"
#include "serv.h"
#include "dc_motor.h"

Servo myservo1;
Servo myservo2;
bool servo_on = true;
bool servo_on2 = true;

bool button_on = false;

int mode = 1;

#define max_points_home_to_coffee 1
#define max_points_coffee_to_out 1

#define max_paths 2

#define button_pin 5

// TODO переместить все точки в один отдельный файл
int path_home_to_coffee[max_points_home_to_coffee][2] = { // Из home к кофемашине
  {100, -150}
};

int path_coffee_to_out[max_points_coffee_to_out][2] = { // От кофемашины на выдачу
  {111, -115}
};

int array_plan_moving[max_paths][2] = {
  {path_home_to_coffee, max_points_home_to_coffee},
  {path_coffee_to_out, max_points_coffee_to_out}
};

int now_point = 0;
int now_path = 0;

int interval_serial = 150; // ms
long previous_time_serial = 0;


void plan_moving() {
  /*
    План движения:
    - Путь 1
    - точка 1
    - точка ШД 1
    - точка ШД 2
    - ...
    - точка 2
    - ...
    - точка 5
    - Путь 2
    - точка 1
    - точка 2
    - ...
    - Путь N
  */

  int count = array_plan_moving[now_path][1];

  if (now_path < max_paths and !moving_on_path(array_plan_moving[now_path][0], count)) {
    now_point = 0;
    now_path = now_path + 1;
  }
}

void joystick() {
  int potentiometer1 = analogRead(A0);
  int potentiometer2 = analogRead(A1);

  if (millis() > previous_time_serial + interval_serial) {
    previous_time_serial = millis();
    Serial.println(get_current_position());
  }

  if (potentiometer1 < 400) {
    move_minus();
    set_speed1(map(potentiometer1, 400, 0, 0, -100));
  }
  else if (potentiometer1 > 400 and potentiometer1 < 600) {
    move_stop1();
  }
  else if (potentiometer1 > 600) {
    move_plus();
    set_speed1(map(potentiometer1, 600, 1023, 0, 100));
  }

  if (potentiometer2 < 400) {
    move_minus();
    set_speed2(map(potentiometer2, 400, 0, 0, -100));
  }
  else if (potentiometer2 > 400 and potentiometer2 < 600) {
    move_stop2();
  }
  else if (potentiometer2 > 600) {
    move_plus();
    set_speed2(map(potentiometer2, 600, 1023, 0, 100));
  }
}

void main_switch() {
  switch (mode) {
    case 1:    // Движение шаговых двигателей из дом к выдаче стаканов
      Serial.println("hd iz doma k stakanam");
      mode++;
      delay(500);
      Serial.println("vidacha stakana");
      break;
    case 2:    // ДС мотор выдает стаканчик
      if (update_and_get_state_motor() == 0){
        mode++;
      }
      break;
    case 3:    // Движение шаговых двигателей к кофемашине
      if (!moving_on_path(path_home_to_coffee, max_points_home_to_coffee)) {
        servo_on = true;
        servo_on2 = true;
        mode++;
      }
      break;
    case 4:
      // Движение сервопривода
      if (servo_on == true) {
        dvigat1();
      }
      else if (servo_on2 == true) {
        dvigat2();
      }
      else {
        Serial.println("rabota servoprivoda");
        mode++;
      }
      break;
    case 5:      // Движение шаговых двигателей от кофемашины на выдачу
      Serial.println("ot coffee navidachy");
      mode++;
      delay(200);
      break;
    case 6:      // Движение шаговых двигателей от выдачи домой
      Serial.println("hd ot vidachi domoi");
      mode++;
      delay(200);
      break;
    case 7:      // Дс мотор открывет окно выдачи
      Serial.println("otkrit okno");
      mode++;
      delay(200);
      Serial.println("ojidanie zakaza ...");
      break;
    case 8:      // Ожидание следующего заказа
      button_on = !digitalRead(button_pin);

      if (button_on) {
        mode = 1;
      }
      break;
    default:
      Serial.println("error");
      delay(10000);
  }
}

void setup() {
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(button_pin, INPUT_PULLUP);

  Serial.begin(115200);
  init_motor();
  init_led();
  init_steppers(); // Инициализация ШД
  init_endstops();
  init_servo();
  // ======= эмулируется выключение света =======
  // move(random(-100, 100), random(-100, 100));

  // while(is_finish() != true){
  //   run_all_steppers();
  // }

  // delay(1000);
  // ============================================

  // Serial.println(**array_plan_moving[0][0]); // TODO
  // Serial.println(**array_plan_moving[1][0]);
}

void loop() {
  // joystick();

  // move_to_home(); // Домашняя позиция // TODO настроить скорости и ускорения при отправке домой

  // if (not is_need_home()){
  //   plan_moving();
  // }
  run_all_steppers();

  led_flash();
  main_switch();
}

// ДЗ 
// 1. Перенести весь проект на Ардуино Мега
// 2. Перенести в отдельные библиотеки функции joystick и plan_moving (path.ino)