#ifndef DVT_ENCODERCLASS_H
#define DVT_ENCODERCLASS_H


#include "DVT_Types.h"
#include "DVT_MainClass.h"
#include "DVT_GantryClass.h"


long CurEncoderPos;
long NewEncoderPos;

void IRAM_ATTR ISREncoderSteps() {
  if (NewEncoderPos > CurEncoderPos)
  {
    if (NewEncoderPos - 5000 <= CurEncoderPos) {
      //GANTRY.SetCcSpeed(NtCcSpeedSlow);
    } else {
      GANTRY.SetCcSpeed(NtCcSpeed);
    }
    CurEncoderPos++;
  }
  else if (NewEncoderPos < CurEncoderPos)
  {
    if (NewEncoderPos + 5000 >= CurEncoderPos) {
      //GANTRY.SetCcSpeed(NtClSpeedSlow);
    } else {
      GANTRY.SetCcSpeed(NtClSpeed);
    }
    CurEncoderPos--;
  }
  else
  {
    detachInterrupt(ENCODER.GetPin());
    GANTRY.Stop();
  }
}

void IRAM_ATTR ISREncoderSwitch() {
  sysMSG("ISREncoderSwitch Trigger" + String(CurEncoderPos));
  CurEncoderPos = 0;
}

class Encoder {
  public:
    Encoder(long CurPos) {
      CurEncoderPos = CurPos;
      attachInterrupt(INDUCT_SWITCH.GetPin(), ISREncoderSwitch, RISING);
    }
    void SetPos(int Pos) {
      GANTRY.Start();
      NewEncoderPos = Pos;
      debugMSG("Pos : " + String(NewEncoderPos));
      if (CurEncoderPos > NewEncoderPos) {
        GANTRY.CcRotation();
      } else {
        GANTRY.ClRotation();
      }
      attachInterrupt(ENCODER.GetPin(), ISREncoderSteps, RISING);
    }

    int GetEncoderPos() {
      debugMSG("CurEncoderPos : " + String(CurEncoderPos));
      return CurEncoderPos;
    }
    void Reset() {
      CurEncoderPos = -1;
    }


    double puls_per_angle = 113108 / 360;
};

Encoder ENCODE = Encoder(0);

#endif