#include <AS5600.h>
#include "AX12A.h"

#define DirectionPin   (14u)
#define BaudRate      (1000000ul)
#define ID1       (4u)
#define ID2       (5u)
#include <SoftwareSerial.h>
#include <XYZrobotServo.h>


//AS5600 Sensor
#include <Wire.h>

SoftwareSerial servoSerial(2, 4); //(18,19)


int input = 0;
XYZrobotServo servo1(servoSerial, 2);

const uint8_t playtime = 100;
const int max_angle = 712,min_angle = 312;

const byte numChars = 32;
int dir = 25; //16
//int brake = 5;
int pwma = 27;
int speed = 0;
int i;
//int Turns = 7;
//bool State = 0;
volatile bool Startup = 0;
volatile bool richtung;
const byte Test_button = 14;
const byte button_oben = 13; //25
const byte button_unten = 26;
int magnetStatus = 0; //value of the status register (MD, ML, MH)

int lowbyte; //raw angle 7:0
word highbyte; //raw angle 7:0 and 11:8
int rawAngle; //final raw angle 
float degAngle; //raw angle in degrees (360/4096 * [value between 0-4095])

int quadrantNumber, previousquadrantNumber; //quadrant IDs
int numberofTurns = 0; //number of turns
int lastnumberofTurns = 0;
float correctedAngle = 0; //tared angle - based on the startup value
float startAngle = 0; //starting angle
float totalAngle = 0; //total absolute angular displacement
float previoustotalAngle = 0; //for the display printing

int sollposition; //von 0 bis 1 * totalTurns

 // initialize prozent to an invalid value
int previousProzent = -1; // variable to store the previous valid value
int prozent;
int totalTurns; // number of Turns after Kalibration sollte ungefähr 33 entsprechen (23/0.7)

int x ,position;
int val[10] = {0,0,0,0,0};
int choice;


void IRAM_ATTR nachUnten(){
  richtung = HIGH;
  speed = 255;
  //sollposition = sollposition - 3;
}
void IRAM_ATTR nachOben(){
  richtung = LOW;
  speed = 255;
  //sollposition = sollposition + 3;
}
void IRAM_ATTR Start(){
  Startup = 1;

}


void setup() {
  Serial.begin(115200); //start serial - tip: don't use serial if you don't need it (speed considerations)
  Wire.begin();

   ax12a.begin(BaudRate, DirectionPin, &Serial2);
  ax12a.setEndless(ID1, OFF);
  ax12a.setEndless(ID2, OFF);
  ax12a.setMaxTorque(ID1, 1000);
  ax12a.setMaxTorque(ID2, 1000);

  servoSerial.begin(115200);

  ax12a.move(ID1,512);
  ax12a.move(ID2,512);
  servo1.setPosition(512, playtime);
  
//  pinMode(Turns, OUTPUT);
  pinMode(dir, OUTPUT);
//  pinMode(brake, OUTPUT);
  pinMode(pwma, OUTPUT);
  pinMode(button_oben, INPUT_PULLUP);
  pinMode(button_unten, INPUT_PULLUP);
  pinMode(Test_button, INPUT_PULLUP);

  //Kalibrieren();

  attachInterrupt(digitalPinToInterrupt(button_oben), nachUnten, FALLING);
  attachInterrupt(digitalPinToInterrupt(button_unten), nachOben, FALLING);
  attachInterrupt(digitalPinToInterrupt(Test_button), Start, FALLING);
  
    //start i2C  
	//Wire.setClock(800000L); //fast clock

checkMagnetPresence(); //check the magnet (blocks until magnet is found)

  ReadRawAngle(); //make a reading so the degAngle gets updated
  startAngle = degAngle; //update startAngle with degAngle - for taring

}

/*void motor(int prozent) {

  sollposition = (totalTurns * prozent * 0.01);
  Serial.print("sollposition: ");
  Serial.println(sollposition);
  if (sollposition > numberofTurns) {
    richtung = LOW;
    speed = 255;
    //Serial.println("Bewegung nach Oben");
  } else if (sollposition < numberofTurns) {
    richtung = HIGH;
    speed = 255;
    //Serial.println("Bewegung nach Unten");
  } else {
    speed = 0;
    Serial.println("In Sollposition");
  }
  }*/
  /*void Positionsabfrage() {
  if (prozent == -1 && Serial.available() > 0) { // check if prozent is not set and data is available to read
    int newProzent = Serial.parseInt(); // read the integer value from serial buffer

    if (newProzent >= 1 && newProzent <= 100) { // check if the value is within the valid range
      prozent = newProzent; // assign the new valid value to prozent
      previousProzent = prozent; // update previousProzent with the new valid value
      Serial.print("Received percentage value: ");
      Serial.println(prozent);
    } else {
      Serial.println("Invalid percentage value! Please enter a number between 0 and 100.");
      if (previousProzent != -1) { // check if there's a previous valid value
        prozent = previousProzent; // restore prozent to its previous valid value
      }
    }
  sollposition = (totalTurns * prozent * 0.01);
  Serial.print("sollposition: ");
  Serial.println(sollposition);
  prozent = -1;
  }


}*/



void loop() {

  /*if(Startup){
   
    
    Kalibrieren();

    Startup = 0;
    }
    //Positionsabfrage();
    */
    /*  i++;
    if (i == 50) {
    Serial.print("NumberofTurns:");
    Serial.println(numberofTurns);
    //Serial.print("correctedAngle: ");
    //Serial.println(correctedAngle, 2);
    if (speed == 0) {
    Serial.println("In Sollposition");
    } else if (richtung == HIGH && speed == 255) {
    Serial.println("Bewegung nach Unten");
    } else if (richtung == LOW && speed == 255) {
    Serial.println("Bewegung nach Oben");
    }
    i = 0;*/
  
 if (Serial.available()){
    for(int n = 0; n < 10 ; n++){
    val[n] = Serial.read() - 48; //convert ASCII to int and assigns into array
   }
    position = 0;
    for (int i = 2; i < 5; i++) {
      position = position * 10 + val[i];
    }
    choice = val[0];
    Serial.print("Motor: ");
    Serial.print(choice);
    Serial.print(" position: ");
    Serial.print(position);
    switch (val[0]) {
      case 1:
        ax12_1(position);
        Serial.print("Ready");
        break;
      case 2:
        a116(position);
        Serial.print("Ready");
        break;
      case 3:
        ax12_2(position);
        Serial.print("Ready");
        break;
      case 4:
        //motor(position); 
        prozent = position;
        sollposition = (totalTurns * prozent * 0.01);
        Serial.print("sollposition: ");
        Serial.println(sollposition);
        Serial.print("Ready");
        break;
      case 5:
        //auto_close();
        Serial.print("Ready");
        break;
      case 6:
        Kalibrieren();
        Serial.print("Ready");
        break;
    }
  }



 if (sollposition > numberofTurns) {
    richtung = LOW;
    speed = 255;
    //Serial.println("Bewegung nach Oben");
  } else if (sollposition < numberofTurns) {
    richtung = HIGH;
    speed = 255;
    //Serial.println("Bewegung nach Unten");
  } 
  else {
    speed = 0;
    //Serial.println("In Sollposition");
   
  }
    digitalWrite(dir,richtung);
    analogWrite(pwma,speed);
    delay(50);
    ReadRawAngle(); //ask the value from the sensor
    correctAngle(); //tare the value
    checkQuadrant(); //check quadrant, check rotations, check absolute angular position
  i++;
  if (i == 50) {
  Serial.print("NumberofTurns:");
  Serial.println(numberofTurns);
  //Serial.print("correctedAngle: ");
  //Serial.println(correctedAngle, 2);
  if (speed == 0) {
    Serial.println("In Sollposition");
  } else if (richtung == HIGH && speed == 255) {
    Serial.println("Bewegung nach Unten");
  } else if (richtung == LOW && speed == 255) {
    Serial.println("Bewegung nach Oben");
  }
  i = 0;
  }



}

void ax12_1(int position){
     if((position<min_angle) || (position>max_angle)){
    Serial.print("invalide position. Try again");
    return;
    }
  ax12a.moveSpeed(ID1,position, playtime);
}

void ax12_2(int position){
     if((position<min_angle) || (position>max_angle)){
    Serial.print("invalide position. Try again");
    return;
    }
  ax12a.moveSpeed(ID2,position, playtime);
}

void a116(int position){
    if((position<min_angle) || (position>max_angle)){
    Serial.print("invalide position. Try again");
    return;
  }
  servo1.setPosition(position, playtime);
}

void Kalibrieren(){
  Serial.print("Start Kalibrieren");
  digitalWrite(dir,HIGH);
  analogWrite(pwma,255); 
  Serial.println(digitalRead(button_unten));
  while(digitalRead(button_unten)){
  ReadRawAngle(); //ask the value from the sensor
  correctAngle(); //tare the value
  checkQuadrant();
  digitalWrite(dir,HIGH);
  analogWrite(pwma,255); 
  }

  Serial.println("Archieved 0");
  numberofTurns = 0;

  while(digitalRead(button_oben)){
  ReadRawAngle(); //ask the value from the sensor
  correctAngle(); //tare the value
  checkQuadrant();
  digitalWrite(dir,LOW);
  analogWrite(pwma,255);
  }

  totalTurns = numberofTurns;
  Serial.print("TotalTurns:");
  Serial.println(totalTurns);

  sollposition = totalTurns * 0.5;
  while(numberofTurns != sollposition) {
  digitalWrite(dir,HIGH);
  analogWrite(pwma,255);
    ReadRawAngle(); //ask the value from the sensor
    correctAngle(); //tare the value
    checkQuadrant();
  }

  Serial.println("Kalib end");
}




void ReadRawAngle()
{ 
  //7:0 - bits
  Wire.beginTransmission(0x36); //connect to the sensor
  Wire.write(0x0D); //figure 21 - register map: Raw angle (7:0)
  Wire.endTransmission(); //end transmission
  Wire.requestFrom(0x36, 1); //request from the sensor
  
  while(Wire.available() == 0); //wait until it becomes available 
  lowbyte = Wire.read(); //Reading the data after the request
 
  //11:8 - 4 bits
  Wire.beginTransmission(0x36);
  Wire.write(0x0C); //figure 21 - register map: Raw angle (11:8)
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);
  
  while(Wire.available() == 0);  
  highbyte = Wire.read();
  
  highbyte = highbyte << 8; //shifting to left

  rawAngle = highbyte | lowbyte; //int is 16 bits (as well as the word)
  //360/4096 = 0.087890625
  degAngle = rawAngle * 0.087890625; 
  
  //Serial.print("Deg angle: ");
  //Serial.println(degAngle, 2); //absolute position of the encoder within the 0-360 circle
  
}

void correctAngle()
{
  //recalculate angle
  correctedAngle = degAngle - startAngle; //this tares the position

  if(correctedAngle < 0) //if the calculated angle is negative, we need to "normalize" it
  {
  correctedAngle = correctedAngle + 360; //correction for negative numbers (i.e. -15 becomes +345)
  }
  else
  {
    //do nothing
  }
  //Serial.print("Corrected angle: ");
  //Serial.println(correctedAngle, 2); //print the corrected/tared angle  
}

void checkQuadrant()
{
  /*
  //Quadrants:
  4  |  1
  ---|---
  3  |  2
  */

  //Quadrant 1
  if(correctedAngle >= 0 && correctedAngle <=90)
  {
    quadrantNumber = 1;
  }

  //Quadrant 2
  if(correctedAngle > 90 && correctedAngle <=180)
  {
    quadrantNumber = 2;
  }

  //Quadrant 3
  if(correctedAngle > 180 && correctedAngle <=270)
  {
    quadrantNumber = 3;
  }

  //Quadrant 4
  if(correctedAngle > 270 && correctedAngle <360)
  {
    quadrantNumber = 4;
  }
  //Serial.print("Quadrant: ");
  //Serial.println(quadrantNumber); //print our position "quadrant-wise"

  if(quadrantNumber != previousquadrantNumber) //if we changed quadrant
  {
    if(quadrantNumber == 1 && previousquadrantNumber == 4)
    {
      numberofTurns--; // 4 --> 1 transition: CW rotation
    }

    if(quadrantNumber == 4 && previousquadrantNumber == 1)
    {
      numberofTurns++; // 1 --> 4 transition: CCW rotation
    }
    //this could be done between every quadrants so one can count every 1/4th of transition

    previousquadrantNumber = quadrantNumber;  //update to the current quadrant
  
  }  
  //Serial.print("Turns: ");
  //Serial.println(numberofTurns,0); //number of turns in absolute terms (can be negative which indicates CCW turns)  

  //after we have the corrected angle and the turns, we can calculate the total absolute position
  totalAngle = (numberofTurns*360) + correctedAngle; //number of turns (+/-) plus the actual angle within the 0-360 range
  //Serial.print("Total angle: ");
  //Serial.println(totalAngle, 2); //absolute position of the motor expressed in degree angles, 2 digits
  
}

void checkMagnetPresence()
{  
  //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly

  while((magnetStatus & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1
  {
    magnetStatus = 0; //reset reading

    Wire.beginTransmission(0x36); //connect to the sensor
    Wire.write(0x0B); //figure 21 - register map: Status: MD ML MH
    Wire.endTransmission(); //end transmission
    Wire.requestFrom(0x36, 1); //request from the sensor

    while(Wire.available() == 0); //wait until it becomes available 
    magnetStatus = Wire.read(); //Reading the data after the request

    Serial.print("Magnet status: ");
    Serial.println(magnetStatus, BIN); //print it in binary so you can compare it to the table (fig 21)      
  }      
  
  //Status register output: 0 0 MD ML MH 0 0 0  
  //MH: Too strong magnet - 100111 - DEC: 39 
  //ML: Too weak magnet - 10111 - DEC: 23     
  //MD: OK magnet - 110111 - DEC: 55

  //Serial.println("Magnet found!");
  //delay(1000);  
}