/////   ultim vers yo 14/05/23//////



#include <Arduino.h> /* Just in case of using the vscode ide for arduino programming, not needed if using the commom arduino ide */
#include <RotaryEncoder.h>
#define PPR 29
#define RPWM 5
#define LPWM 6

/*
  Pins connected to aduino( in english )

  IBT-2 pin 1 (RPWM) to Arduino pin 5(PWM)
  IBT-2 pin 2 (LPWM) to Arduino pin 6(PWM)
  IBT-2 pins 3 (R_EN), 4 (L_EN), 7 (VCC) to Arduino 5V
  IBT-2 pin 8 (GND) to Arduino's GND
  IBT-2 pins 5 (R_IS) and 6 (L_IS) not connected
*/

// rotary encoder pins
#define PIN_IN1 2
#define PIN_IN2 3

// bts7960 pins
const int L_EN = 7;
const int R_EN = 8;
const int L_PWM = 6;
const int R_PWM = 5;

// potentiometer pin
const int pot = A0;
float vel = 150;

// declaration of encoder object
//RotaryEncoder encoder(DT, CLK, RotaryEncoder::LatchMode::TWO03);
RotaryEncoder *encoder = nullptr;

float tgtangle = 800;
float angle = 0;

void moveTo(float now, float target)
{
  if (target > 0)
  {
    if (now <= target)
    {
      analogWrite(LPWM, 0);
      analogWrite(RPWM, vel);
    }
    else
    {
      analogWrite(RPWM, 0);
      analogWrite(LPWM, 0);
      delay(200);
      tgtangle = -1600;
    }
  }
  else
  {
    if (now >= target)
    {
      analogWrite(RPWM, 0);
      analogWrite(LPWM, vel);
    }
    else
    {
      analogWrite(RPWM, 0);
      analogWrite(LPWM, 0);
      delay(200);
      tgtangle = 800;
    }
  }
}

void getAngle()
{
  static int pos = 0;
  encoder->tick();
  int newPos = encoder->getPosition();
  if (pos != newPos)
  {
    angle = (float)newPos / PPR * 360;
    Serial.print("Desired angle: ");
    Serial.print(tgtangle);
    Serial.print(" || Actual angle: ");
    Serial.print(angle);
    Serial.print(" || Direction: ");
    Serial.print((int)(encoder->getDirection()));
    Serial.print(" || RPM: ");
    Serial.println((int)(encoder->getRPM()));
    pos = newPos;
  }
}

void checkPosition()
{
  encoder->tick(); // just call tick() to check the state.
}

void setup()
{
  Serial.begin(9600);
  encoder = new RotaryEncoder(PIN_IN1, PIN_IN2, RotaryEncoder::LatchMode::TWO03);
  attachInterrupt(digitalPinToInterrupt(PIN_IN1), checkPosition, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_IN2), checkPosition, CHANGE);
}

void loop()
{
  getAngle();
  moveTo(angle, tgtangle);
}