#ifndef DVT_GANTRYCLASS_H
#define DVT_GANTRYCLASS_H

#include "DVT_MainClass.h"

class Gantry {
  private:
    int nt_cl_speed;
    int nt_cc_speed;
    long CurEncoderPos;
    long NewEncoderPos;
  public:
    Gantry(int nt_cl_speed, int nt_cc_speed) {
      this->nt_cl_speed = nt_cl_speed;
      this->nt_cc_speed = nt_cc_speed;
    }
    void Start() {
      ARM_STOP.SetState(HIGH);
      sysMSG("SYSTEM: Gantry Started");
    }
    void Stop() {
      ARM_STOP.SetState(LOW);
      sysMSG("SYSTEM: Gantry Stopped");
      ARM_SPEED.SetAnalogState(0);
    }
    void ClRotation() {
      ARM_DIRECT.SetState(LOW);
      ARM_SPEED.SetAnalogState(this->nt_cl_speed);
      debugMSG("Clockwise Rotation");
    }
    void CcRotation() {
      ARM_DIRECT.SetState(HIGH);
      ARM_SPEED.SetAnalogState(this->nt_cc_speed);
      debugMSG("Counter Clockwise Rotation");
    }
    void SetClSpeed(int Speed) {
      if (Speed >= 0 && Speed <= 255) {
        this->nt_cl_speed = Speed;
        debugMSG("nt_cl_speed : " + String(Speed));
      } else {
        debugMSG("Invalid value : " + String(Speed) + " is not in range (0-255)");
      }
    }
    void SetCcSpeed(int Speed) {
      if (Speed >= 0 && Speed <= 255) {
        this->nt_cc_speed = Speed;
        debugMSG("nt_cc_speed : " + String(Speed));
      } else {
        debugMSG("Invalid value : " + String(Speed) + " is not in range (0-255)");
      }
    }
    int GetClSpeed() {
      debugMSG("nt_cl_speed : " + String(this->nt_cl_speed));
      return this->nt_cl_speed;
    }
    int GetCcSpeed() {
      debugMSG("nt_cc_speed : " + String(this->nt_cc_speed));
      return this->nt_cc_speed;
    }
};


Gantry GANTRY = Gantry(NtClSpeed, NtCcSpeed);

#endif