#include <AccelStepper.h>

#include <WiFi.h>

const char * ssid = "MTS_GPON_272f00";
const char * password = "A7fa6j9e";

#include <cmath>

typedef struct {
  float x;
  float y;
  float z;
}
XYZ;

#define MotorInterfaceType 8

#define pinX 4
#define pinY 0
#define buttonPin  2
#define pinZ 15

const int radius = 10;
const int stepsPerRevolution = 4096;
const double revolutionLength = radius * 2 * PI;
const double mmPerStep = revolutionLength / stepsPerRevolution;
const double stepsPerMm = stepsPerRevolution / revolutionLength;

int getAxisX(){
  return std::round(map(analogRead(pinY), 0, 4095, -100, 100));
}

int getAxisY(){
  return std::round(map(analogRead(pinX), 0, 4095, -100, 100));
}

int getAxisZ(){
  return std::round(map(analogRead(pinZ), 0, 4095, 100, -100));
}

int tick = 0;

class Lift {
  private: XYZ target = {0, 0, 0};
  private: AccelStepper stepper;
  private: int mode = 1;
  private: int pinStop;
  private: const char* name;
  private: int zero = 0;
  private: XYZ position;

  public: Lift(const char* _name, XYZ _position, int pin1, int pin2, int pin3, int pin4, int _pinStop): 
    name (_name), 
    position(_position),
    pinStop(_pinStop) 
  {
    stepper = AccelStepper(MotorInterfaceType, pin1, pin2, pin3, pin4);
    stepper.setMaxSpeed(2000);
    stepper.setAcceleration(500);
    pinMode(pinStop, INPUT_PULLUP);
  }

  private: int getLengthToPoint(const XYZ point) {
    XYZ delta = {
      point.x - position.x,
      point.y - position.y,
      point.z - position.z
    };

    return std::round(std::sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z));
  };

  private: int getLengthToTarget() {
    return getLengthToPoint(target);
  }

  public: void setTarget(const XYZ _target) {
    target.x = _target.x;
    target.y = _target.y;
    target.z = _target.z;
  };

  public: void setMode(const int value) {
    mode = value;
    printf("%s Mode %d", name, mode);
  }

  public: void update() {

    printf(" [%s]", name);
  

    // if (tick % 100 == 0) {
      // printf("%d %f", getAxisX(), getAxisX());
      // printf(" Target x %.2f y %.2f z %.2f %d", target.x, target.y, target.z);
    // }

    switch (mode) {
      case 0: {
        /* Пауза */
        delay(1000);
        setMode(2);
        break;
      }
      case 1: {
        double tension = digitalRead(pinStop);
        /* Калибровка */
        if (tension == LOW) {
          stepper.setSpeed(0);
          stepper.setCurrentPosition(0);
          stepper.disableOutputs();
          setMode(0);
          printf(" Callibration complete %d", stepper.currentPosition());
        } else {
          stepper.setSpeed(-1000);
          stepper.run();
          printf(" Callibration %d", stepper.currentPosition());
        }
        break;
      }
      case 2: {        
        stepper.moveTo(getLengthToTarget());
        if (stepper.distanceToGo() == 0){
          stepper.disableOutputs();
          printf(" Done %d", stepper.currentPosition());
        } else {
          stepper.enableOutputs();
          stepper.run();
          printf(" Run  %d/%d", stepper.currentPosition(), getLengthToTarget());
        }
        break;
      }
      case 3: {

        /* Ручное управление */
        break;
      }
      default: {
        break;
      }
    }
  }
};

XYZ target = {
  2500,
  0,
  1500
};

Lift lifts[2] = {
  Lift("A", {
    0,
    0,
    3000
  }, 19, 5, 18, 17, 16),
  Lift("B", {
    5000,
    0,
    3000
  }, 25, 27, 26, 14, 12)
};

void setup() {
  Serial.begin(115200);

    pinMode(pinY, INPUT);
    pinMode(pinX, INPUT);
    pinMode(pinZ, INPUT);

  /*
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi..");
    Serial.println("Connected to the WiFi network");
    Serial.println(WiFi.localIP());
  }
  */

  for(Lift &lift : lifts){
    lift.setTarget(target);
  }
}



void loop() {
  tick++;
  // delay(1);


   if (tick % 10 == 0) {
      target.x = target.x + getAxisX() / 1;
      target.y = target.y + getAxisY() / 1;
      target.z = target.z + getAxisZ() / 1;
      for(Lift &lift : lifts){
        lift.setTarget(target);
      }
  }

  printf("\n%d [T] %.0f %.0f %.0f", tick, target.x, target.y, target.z);
  for(Lift &lift : lifts){
    lift.update();
  }
}