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


#define max_points_home_to_coffee 5
#define max_points_coffee_to_out 2

#define max_paths 2

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

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


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

int now_point = 0;
int now_path = 0;

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


void plan_moving() {
  int count = sizeof(array_plan_moving[now_path]) / 2;
  // int count = sizeof(array_plan_moving[now_path]) / sizeof(array_plan_moving[now_path][0]); // кол-во элементов в массиве

  if (now_path < max_paths and !moving_on_path(array_plan_moving[now_path], count)){
    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 setup() {
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);

  Serial.begin(115200);

  init_led();
  init_steppers(); // Инициализация ШД
  init_endstops();

  // ======= эмулируется выключение света =======
  // 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();
}

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