/*
      Custom-built Servo Motor - Arduino Code
      by Dejan, www.HowToMechatronics.com

     Libraries:
     AS5600 encoder: https://github.com/RobTillaart/AS5600
     PID conroller: https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h

  L298N
  https://wokwi.com/projects/386822856593519617

*/

#include  <Adafruit_AS5600.h>
//#include "AS5600.h"
#include "Wire.h"
#include <PID_v1.h>

const int stepsPerRev = 800; // 800 steps for a full rotation (using 1/4 micromotor_IN1g)

Adafruit_AS5600 as5600;
//AS5600 as5600;   //  use default Wire

double Pk1 = 2;  //speed it gets there
double Ik1 = 0;
double Dk1 = 0.025;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

PID myPID(&Input, &Output, &Setpoint, Pk1, Ik1, Dk1, DIRECT);

#define motor_IN1 5
#define motor_IN2 6
#define ch1 2
#define centerSet 7
#define inputSwitch 3
#define modeSwitch 4

int ch1Value;

int encoderValue, inputValue, pwmValue;
String inString = "";  // string to hold input

int centerAngle = 2047; // 180 degrees
int angleDifference = 0;
int angleValue = 0;
int leftLimit = 30;
int rightLimit = 4067;
int rangeAdjustment = 0;
float sensitivityAdjustment = 0;
float angle = 0;

int quadrantNumber = 2;
int previousQuadrantNumber = 3;
int numberOfTurns = 0;
float totalAngle = 0;

int error = 0;

char incomingByte = 0;
int intInput = 0;

//=====================================================================
//=====================================================================
//=====================================================================

void setup() {
  Serial.begin(115200);
  //Serial.print("File:\t");
  //Serial.println(__FILE__);
  //Serial.print("AS5600_LIB_VERSION: ");
  //Serial.println(AS5600_LIB_VERSION);

  Wire.begin();

  Serial.println("AS5600 Setup");

  delay(1000);

  as5600_setup();

  pinMode(motor_IN1, OUTPUT);
  pinMode(motor_IN2, OUTPUT);

  // Activate the Arduino internal pull-up resistors
  pinMode(centerSet, INPUT_PULLUP);
  pinMode(inputSwitch, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

  myPID.SetMode(AUTOMATIC);              // PID Setup
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(20);

  Serial.println("Setup Complete");
}

//=====================================================================
//=====================================================================
//=====================================================================

void loop() {
  //uint16_t rawAngle = as5600.getRawAngle();
  uint16_t angle = as5600.getAngle();

  uint16_t encoder_angle = map(angle, 0, 4095, 0, 36000);
  encoderValue = as5600.getRawAngle();

  // Read encoder value - current position
  //encoderValue = as5600.readAngle();
  Serial.print("Encoder: "); Serial.print(encoderValue); Serial.print("/"); Serial.print(encoder_angle);

  // Continuous rotation mode
  if (digitalRead(modeSwitch) == 0) {
    // Enter desired angle for the servo to go to through the serial monitor
    while (Serial.available() > 0) {
      int inChar = Serial.read();
      if (isDigit(inChar)) {
        // convert the incoming byte to a char and add it to the string:
        inString += (char)inChar;
      }
      // if you get a newline, print the string, then the string's value:
      if (inChar == '\n') {
        Setpoint = inString.toInt(); // Setpoint - desired angle
        // clear the string for new input:
        inString = "";
      }
    }
    if (digitalRead(inputSwitch) == 0) { // Potentiometer as input
      inputValue = analogRead(A0);
      if (inputValue < 400) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 300) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 200) {
        Setpoint = Setpoint - 0.3;
      }

      if (inputValue > 600) {
        Setpoint = Setpoint + 0.3;
      }
      if (inputValue > 700) {
        Setpoint = Setpoint + 0.3;
      }

      if (inputValue > 800) {
        Setpoint = Setpoint + 0.3;
      }
    }
    else if (digitalRead(inputSwitch) == 1) {
      inputValue = pulseIn(ch1, HIGH, 30000); // RC receiver as input
      if (inputValue < 1450) {
        Setpoint--;
      }
      if (inputValue < 1350) {
        Setpoint--;
      }
      if (inputValue < 1250) {
        Setpoint--;
      }
      if (inputValue < 1150) {
        Setpoint--;
      }
      if (inputValue > 1550) {
        Setpoint++;
      }
      if (inputValue > 1650) {
        Setpoint++;
      }
      if (inputValue > 1750) {
        Setpoint++;
      }
      if (inputValue > 1850) {
        Setpoint++;
      }
    }

    // Convert encoder RAW values into angle value
    angle = encoderValue * 0.087890625;
    //0.087890625 = 360/4096
    Serial.print("/"); Serial.println(angle);

    // Quadrant 1
    if (angle >= 0 && angle <= 90) {
      quadrantNumber = 1;
    }
    // Quadrant 2
    if (angle >= 90 && angle <= 180) {
      quadrantNumber = 2;
    }
    // Quadrant 3
    if (angle >= 180 && angle <= 270) {
      quadrantNumber = 3;
    }
    // Quadrant 4
    if (angle >= 270 && angle <= 360) {
      quadrantNumber = 4;
    }

    if (quadrantNumber != previousQuadrantNumber) {
      // Transition from 4th to 1st quadrant
      if (quadrantNumber == 1 && previousQuadrantNumber == 4) {
        numberOfTurns++;
      }
      // Transition from 1st to 4th quadrant
      if (quadrantNumber == 4 && previousQuadrantNumber == 1) {
        numberOfTurns--;
      }
      previousQuadrantNumber = quadrantNumber;
    }
    if (totalAngle >= 0) {
      totalAngle = (numberOfTurns * 360) + angle;
    }
    else {
      totalAngle = (numberOfTurns * 360) + angle;
    }

    // Establish Input value for PID
    Input = totalAngle;
  }

  // Limited Rotation Mode
  else if (digitalRead(modeSwitch) == 1) {

    rangeAdjustment = analogRead(A1);
    leftLimit = 0 + 30 + rangeAdjustment;
    rightLimit = 4097 - 30 - rangeAdjustment;


    if (digitalRead(inputSwitch) == 0) {  // Analog input - Potentiometer
      // Get value from potentiometer
      inputValue = analogRead(A0);
      if (inputValue < 15) {
        inputValue = 15;
      }
      if (inputValue > 1008) {
        inputValue = 1008;
      }
      Setpoint = map(inputValue, 15, 1008, -255, 255);
    }
    else if (digitalRead(inputSwitch) == 1) {  // Digital input - RC transmitter
      inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input
      Setpoint = map(inputValue, 1000, 2000, -255, 255);
    }

    // Set center angle
    if (digitalRead(centerSet) == LOW) {
      centerAngle = encoderValue;
      angleDifference = 2047 - encoderValue;
      delay(1000);
    }
    // Adjust angle value according to the center point (angleDifference)
    if (centerAngle < 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue < 4097 && encoderValue > (4096 - angleDifference)) {
        angleValue = encoderValue - 4097 + angleDifference;
      }
    }
    if (centerAngle > 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
        angleValue = encoderValue + 4097 + angleDifference;
      }
    }
    else if (centerAngle == 2047) {
      angleValue = encoderValue;
    }

    // Establish Input value for PID
    Input = map(angleValue, leftLimit, rightLimit, -255, 255);
  }
  // Adjusting sensitivity
  Pk1 = analogRead(A2) * 0.002;
  myPID.SetTunings(Pk1, Ik1, Dk1);

  // Run PID process to get Output value
  myPID.Compute();
  // Move right
  if (Output > 1 ) {
    pwmValue = Output;

    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    //digitalWrite(motor_IN1, LOW);
    //analogWrite(motor_IN2, pwmValue);

    analogWrite(motor_IN1, pwmValue);
    digitalWrite(motor_IN2, LOW);
  }
  // Move left
  else if (Output < 1 ) {
    pwmValue = abs(Output);

    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    //analogWrite(motor_IN1, pwmValue);
    //digitalWrite(motor_IN2, LOW);

    analogWrite(motor_IN1, pwmValue);
    digitalWrite(motor_IN2, HIGH);
  }
  // Do not move
  else if (Output > -1 && Output < 1) {
    pwmValue = 0;
    digitalWrite(motor_IN1, LOW);
    digitalWrite(motor_IN2, LOW);
  }

  //Serial.print(Setpoint);
  //Serial.print("\t");
  //Serial.println(totalAngle);
}

//=====================================================================
//=====================================================================
//=====================================================================
void as5600_setup() {


  if (!as5600.begin()) {
    Serial.println("Could not find AS5600 sensor, check wiring!");
    while (1)
      delay(10);
  }

  Serial.println("AS5600 found!");

  // Test getZMCount function
  uint8_t zmCount = as5600.getZMCount();
  Serial.print("ZM Count (burn count): ");
  Serial.println(zmCount);

  // Test getZPosition function
  uint16_t zPos = as5600.getZPosition();
  Serial.print("Z Position: ");
  Serial.println(zPos);

  // Test setZPosition function (XOR current value with 0xADA to change it)
  uint16_t testPos = zPos ^ 0xADA; // XOR with 0xADA to get different value
  testPos &= 0x0FFF;               // Keep within 12-bit range
  Serial.print("Setting Z Position to ");
  Serial.print(testPos);
  Serial.print(" (0x");
  Serial.print(testPos, HEX);
  Serial.print(")... ");
  if (as5600.setZPosition(testPos)) {
    Serial.println("Success");
    uint16_t newZPos = as5600.getZPosition();
    Serial.print("New Z Position: ");
    Serial.print(newZPos);
    Serial.print(" (0x");
    Serial.print(newZPos, HEX);
    Serial.println(")");
  } else {
    Serial.println("Failed");
  }

  // Test getMPosition function
  uint16_t mPos = as5600.getMPosition();
  Serial.print("M Position: ");
  Serial.println(mPos);

  // Test setMPosition function (XOR current value with 0xBEE)
  uint16_t testMPos = mPos ^ 0xBEE;
  testMPos &= 0x0FFF;
  Serial.print("Setting M Position to ");
  Serial.print(testMPos);
  Serial.print(" (0x");
  Serial.print(testMPos, HEX);
  Serial.print(")... ");
  if (as5600.setMPosition(testMPos)) {
    Serial.println("Success");
    uint16_t newMPos = as5600.getMPosition();
    Serial.print("New M Position: ");
    Serial.print(newMPos);
    Serial.print(" (0x");
    Serial.print(newMPos, HEX);
    Serial.println(")");
  } else {
    Serial.println("Failed");
  }

  // Test getMaxAngle function
  uint16_t maxAngle = as5600.getMaxAngle();
  Serial.print("Max Angle: ");
  Serial.println(maxAngle);

  // Test setMaxAngle function (XOR current value with 0xCAB)
  uint16_t testMaxAngle = maxAngle ^ 0xCAB;
  testMaxAngle &= 0x0FFF;
  Serial.print("Setting Max Angle to ");
  Serial.print(testMaxAngle);
  Serial.print(" (0x");
  Serial.print(testMaxAngle, HEX);
  Serial.print(")... ");
  if (as5600.setMaxAngle(testMaxAngle)) {
    Serial.println("Success");
    uint16_t newMaxAngle = as5600.getMaxAngle();
    Serial.print("New Max Angle: ");
    Serial.print(newMaxAngle);
    Serial.print(" (0x");
    Serial.print(newMaxAngle, HEX);
    Serial.println(")");
  } else {
    Serial.println("Failed");
  }

  // Test watchdog functions
  Serial.print("Enabling watchdog... ");
  if (as5600.enableWatchdog(true)) {
    Serial.println("Success");
    Serial.print("Watchdog status: ");
    Serial.println(as5600.getWatchdog() ? "ENABLED" : "DISABLED");
  } else {
    Serial.println("Failed");
  }

  Serial.print("Disabling watchdog... ");
  if (as5600.enableWatchdog(false)) {
    Serial.println("Success");
    Serial.print("Watchdog status: ");
    Serial.println(as5600.getWatchdog() ? "ENABLED" : "DISABLED");
  } else {
    Serial.println("Failed");
  }

  // Test power mode functions
  Serial.print(
    "Setting power mode to Normal (options: NOM=0, LPM1=1, LPM2=2, "
    "LPM3=3)... ");
  if (as5600.setPowerMode(AS5600_POWER_MODE_NOM)) {
    Serial.println("Success");
    as5600_power_mode_t mode = as5600.getPowerMode();
    Serial.print("Power mode: ");
    switch (mode) {
      case AS5600_POWER_MODE_NOM:
        Serial.println("Normal");
        break;
      case AS5600_POWER_MODE_LPM1:
        Serial.println("Low Power Mode 1");
        break;
      case AS5600_POWER_MODE_LPM2:
        Serial.println("Low Power Mode 2");
        break;
      case AS5600_POWER_MODE_LPM3:
        Serial.println("Low Power Mode 3");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Test hysteresis functions
  Serial.print(
    "Setting hysteresis to OFF (options: OFF=0, 1LSB=1, 2LSB=2, 3LSB=3)... ");
  if (as5600.setHysteresis(AS5600_HYSTERESIS_OFF)) {
    Serial.println("Success");
    as5600_hysteresis_t hysteresis = as5600.getHysteresis();
    Serial.print("Hysteresis: ");
    switch (hysteresis) {
      case AS5600_HYSTERESIS_OFF:
        Serial.println("OFF");
        break;
      case AS5600_HYSTERESIS_1LSB:
        Serial.println("1 LSB");
        break;
      case AS5600_HYSTERESIS_2LSB:
        Serial.println("2 LSB");
        break;
      case AS5600_HYSTERESIS_3LSB:
        Serial.println("3 LSB");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Test output stage functions
  Serial.print(
    "Setting output stage to Analog Full (options: ANALOG_FULL=0, "
    "ANALOG_REDUCED=1, DIGITAL_PWM=2, RESERVED=3)... ");
  if (as5600.setOutputStage(AS5600_OUTPUT_STAGE_ANALOG_FULL)) {
    Serial.println("Success");
    as5600_output_stage_t outputStage = as5600.getOutputStage();
    Serial.print("Output stage: ");
    switch (outputStage) {
      case AS5600_OUTPUT_STAGE_ANALOG_FULL:
        Serial.println("Analog Full (0% to 100%)");
        break;
      case AS5600_OUTPUT_STAGE_ANALOG_REDUCED:
        Serial.println("Analog Reduced (10% to 90%)");
        break;
      case AS5600_OUTPUT_STAGE_DIGITAL_PWM:
        Serial.println("Digital PWM");
        break;
      case AS5600_OUTPUT_STAGE_RESERVED:
        Serial.println("Reserved");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Test PWM frequency functions
  Serial.print(
    "Setting PWM frequency to 115Hz (options: 115HZ=0, 230HZ=1, 460HZ=2, "
    "920HZ=3)... ");
  if (as5600.setPWMFreq(AS5600_PWM_FREQ_115HZ)) {
    Serial.println("Success");
    as5600_pwm_freq_t pwmFreq = as5600.getPWMFreq();
    Serial.print("PWM frequency: ");
    switch (pwmFreq) {
      case AS5600_PWM_FREQ_115HZ:
        Serial.println("115 Hz");
        break;
      case AS5600_PWM_FREQ_230HZ:
        Serial.println("230 Hz");
        break;
      case AS5600_PWM_FREQ_460HZ:
        Serial.println("460 Hz");
        break;
      case AS5600_PWM_FREQ_920HZ:
        Serial.println("920 Hz");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Test slow filter functions
  Serial.print(
    "Setting slow filter to 16x (options: 16X=0, 8X=1, 4X=2, 2X=3)... ");
  if (as5600.setSlowFilter(AS5600_SLOW_FILTER_16X)) {
    Serial.println("Success");
    as5600_slow_filter_t slowFilter = as5600.getSlowFilter();
    Serial.print("Slow filter: ");
    switch (slowFilter) {
      case AS5600_SLOW_FILTER_16X:
        Serial.println("16x");
        break;
      case AS5600_SLOW_FILTER_8X:
        Serial.println("8x");
        break;
      case AS5600_SLOW_FILTER_4X:
        Serial.println("4x");
        break;
      case AS5600_SLOW_FILTER_2X:
        Serial.println("2x");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Test fast filter threshold functions
  Serial.print(
    "Setting fast filter threshold to Slow Only (options: SLOW_ONLY=0, "
    "6LSB=1, 7LSB=2, 9LSB=3, 18LSB=4, 21LSB=5, 24LSB=6, 10LSB=7)... ");
  if (as5600.setFastFilterThresh(AS5600_FAST_FILTER_THRESH_SLOW_ONLY)) {
    Serial.println("Success");
    as5600_fast_filter_thresh_t fastThresh = as5600.getFastFilterThresh();
    Serial.print("Fast filter threshold: ");
    switch (fastThresh) {
      case AS5600_FAST_FILTER_THRESH_SLOW_ONLY:
        Serial.println("Slow filter only");
        break;
      case AS5600_FAST_FILTER_THRESH_6LSB:
        Serial.println("6 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_7LSB:
        Serial.println("7 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_9LSB:
        Serial.println("9 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_18LSB:
        Serial.println("18 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_21LSB:
        Serial.println("21 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_24LSB:
        Serial.println("24 LSB");
        break;
      case AS5600_FAST_FILTER_THRESH_10LSB:
        Serial.println("10 LSB");
        break;
    }
  } else {
    Serial.println("Failed");
  }

  // Reset position settings to defaults
  as5600.setZPosition(0);
  as5600.setMPosition(4095);
  as5600.setMaxAngle(4095);

  delay(2000);
}