// Libraries
#include <PIDController.h>
#include <AccelStepper.h>
#include <SparkFun_MicroPressure.h>
#include <Wire.h>

// Defines
#define POMIK 1
#define TLAK 2
#define POLNJENJE 1
#define PRAZNJENJE 2
#define dirPin 12
#define stepPin 13
#define motorInterfaceType 1
#define FULLSTEP 1
#define HALFSTEP 2

// Pins
const int endSwitch1 = 6;
const int endSwitch2 = 5;
const int LED1 = 3;
const int LED2 = 4;
const int MS1 = 7;
const int MS2 = 8;
const int MS3 = 9;

// Spremenljivke
int sprdef = 200; // steps per rotation
int spr;
int izbira; // izbira načina uporabe
int onSw1;
int onSw2;
int dirState; // določa smer vrtenja motorja
int speed, speedSet;
int pomik;
int pomikSet;
int step = 1; // število korakov motorja, ki jih naredi motor med vsakim preračunom PID
float gonilna = 24.; // št. zob gonilne jermenice
float gnana = 10.; // št. zob gnane jermenice
float navoj = 2; // korak navoja navojne palice
float tlakSet;
float tlakRead;
bool runPolnjenje = true; // omogočanje vrtenja motorja v načinu tlaka
bool runPraznjenje = true; // omogočanje vrtenja motorja v načinu pomika
bool runProgram = true; // omogoča izvajanje celotnega programa

// AccelStepper
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

// Pressure sensor
SparkFun_MicroPressure mpr;

// Spremenljivke za PID krmiljenje
PIDController pid;
float kp = 1;
float ki = 1;
float kd = 1;

/****************************************************
  PROGRAM
****************************************************/

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(endSwitch1, INPUT_PULLUP);
  pinMode(endSwitch2, INPUT_PULLUP);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  stepper.setMaxSpeed(1000);
  startup();
}

void loop()
{
  stepMode();
  while (runProgram == true) {
    bool stateMode;
    bool stateParam;
    masterMode();
    switch (izbira) {
      case POMIK: // pomik
        stateMode = pomikMode();
        break;
      case TLAK: // tlak
        tlakMode();
        break;
      default:
        Serial.print("ERROR (izbira: ");
        Serial.print(izbira);
        Serial.println(")");
        delay(10000);
        break;
    }
  }
}

/****************************************************
  FUNKCIJE
****************************************************/

// STARTUP MESSAGE V SERIJSKI KOMUNIKACIJI
void startup()
{
  Serial.println("PRIPRAVLJEN NA UPORABO");
  Serial.println();
  delay(1000);
}

// IZBIRA FULLSTEP, HALFSTEP
void stepMode()
{
  bool state = true;
  while(state == true)
  {
    Serial.println("IZBIRA KORAČNEGA NAČINA MOTORJA");
    Serial.println("1 - FULLSTEP");
    Serial.println("2 - HALFSTEP");
    Serial.println();
    while(!Serial.available()){}
    int chooseVal = Serial.parseInt();
    Serial.read();
    switch(chooseVal)
    {
      case FULLSTEP:
        digitalWrite(MS1, LOW);
        digitalWrite(MS2, LOW);
        digitalWrite(MS3, LOW);
        spr = sprdef;
        Serial.println("IZBRANO: FULLSTEP");
        Serial.println();
        delay(500);
        state = false;
        break;
      case HALFSTEP:
        digitalWrite(MS1, HIGH);
        digitalWrite(MS2, LOW);
        digitalWrite(MS3, LOW);
        spr = 2 * sprdef;
        Serial.println("IZBRANO: HALFSTEP");
        Serial.println();
        delay(500);
        state = false;
        break;
      default:
        state = true;
        break;
    }
  }    
}

// IZBIRA NAČINA UPORABE
void masterMode()
{
  Serial.println("Izberi način uporabe: ");
  Serial.println("1 - Nastavitev pomika");
  Serial.println("2 - Nastavitev tlaka");
  Serial.println();
  Serial.read();
  while (!Serial.available()) {}
  izbira = Serial.parseInt();
}

// NASTAVITEV PARAMETROV
void parametri()
{
  bool out = true;
  while(out == true)
  {
    switch (izbira)
    {
      case POMIK:
        out = false;
        pomikSet = pomikIn();
        speedSet = hitrostIn();
        break;
      case TLAK:
        out = false;
        tlakSet = tlakIn();
        break;
      default:
        out = true;
        break;
    }
  }
}

// NASTAVITEV TLAKA
float tlakIn()
{
  Serial.print("Nastavi željeni tlak (vrednost v Pa): ");
  Serial.read();
  while (!Serial.available()) {}
  float tlak = Serial.parseFloat();
  Serial.println(tlak);
  return tlak;
}

// NASTAVITEV POMIKA
float pomikIn()
{
  Serial.print("Nastavi željeni pomik (vrednost v mm): ");
  Serial.read();
  while (!Serial.available()) {}
  pomik = Serial.parseFloat();
  float rotacijaMotorja = prestava(pomik);
  int steps = funPomik(rotacijaMotorja);
  Serial.print(pomik);
  Serial.println(" mm");
  return steps;
}

// NASTAVITEV HITROSTI
float hitrostIn()
{
  Serial.print("Nastavi željeno hitrost (vrednost v RPM): ");
  Serial.read();
  while (!Serial.available()) {}
  speed = Serial.parseFloat();
  int sps = funHitrost(speed);
  if(speed < 60)
  {
    Serial.println("Nastavljena privzeta vrednost - 60 RPM");
  }
  else
  {
    Serial.print(speed);
    Serial.println(" RPM");
  }
  return sps;
}

// PRERAČUN PRESTAVE
float prestava(float x)
{
  float razmerje = (gnana / gonilna);
  float pomiksprestavo = x * razmerje;
  return pomiksprestavo;
}

// PRERAČUN POMIKANJA BATA V ŠTEVILO KORAKOV
int funPomik(float x)
{
  float stRotacij = x / navoj;
  float steps = stRotacij * spr;
  int rsteps = round(steps);
  return rsteps;
}

// PRERAČUN HITROSTI VRTENJA V STEPS/SECOND
int funHitrost(int v)
{
  float rps = (v * sprdef) / 60;
  if(v < 60)
  {
    rps = 10;
  }
  int rrps = round(rps);
  int sps = rrps * sprdef;
  return sps;
}

// FUNKCIJA POLNJENJA MOTORJA
void polnjenje(float varSet, float setSpeed)
{
  runPolnjenje = true;
  Serial.println("POLNJENJE");
  Serial.println();
  bool statusRotor = true;
    while(runPolnjenje == true)
    {
      bool status = rotacija(varSet, setSpeed);
      if(status == false)
      {
      runPolnjenje = false;
      }
    }  
  Serial.println("KONčANO");
}

// FUNKCIJA PRAZNJENJA MOTORJA
void praznjenje(float varSet, float setSpeed)
{
  runPraznjenje = true;
  Serial.println("PRAZNJENJE");
  Serial.println();
  while(runPraznjenje == true)
  {
    bool status = rotacija(-varSet, -setSpeed);
    if(status == false)
    {
      runPraznjenje = false;
    }
  }
  Serial.println("KONčANO");  
}

// NADALJUJ V IZBRANEM NAČINU ALI NE
bool nadaljuj()
{
  bool out;
  Serial.println("1 - nadaljuj v izbranem načinu");
  Serial.println("2 - zamenjaj način");
  Serial.println();
  Serial.read();
  while(!Serial.available()){}
  int a = Serial.parseInt();
  switch(a)
  {
    case 1:
      out = true;
      break;
    case 2:
      out = false;
      break;
    default:
      out = false;
      break;
  }
  return out;
}

// NAČIN KRMILJENJA Z NASTAVITVIJO POMIKA
bool pomikMode()
{
  bool state = true;
  while(state == true)
  {
    Serial.println();
    Serial.println("KRMILJENJE Z NASTAVITVIJO POMIKA");
    parametri();
    Serial.println("1 - POLNJENJE");
    Serial.println("2 - PRAZNJENJE");
    Serial.println("3 - SPREMENI POMIK/HITROST");
    Serial.println("4 - POLNI IZVLEK");
    Serial.println("5 - MENJAVA NAČINA");
    Serial.println();

    Serial.read();
    while (!Serial.available()) {}
    int dane = Serial.parseInt();

    switch (dane){
      // polnjenje
      case 1:
        polnjenje(pomikSet, speedSet);
        state = nadaljuj();
        break;
      
      // praznjenje
      case 2:
        praznjenje(pomikSet, speedSet);
        state = nadaljuj();
        break;

      // menjava nastavljene vrednosti
      case 3:
        state = true;
        break;

      case 4:
        int pomikFull = funPomik(100);
        int speedDef = funHitrost(200);
        praznjenje(pomikFull, speedDef);
        state = true;
        break;

      // menjava načina uporabe
      case 5:
        state = false;
        break;

      default:
        Serial.println("NAPAČEN VNOS");
        delay(1000);
        state = true;
        break; 
    }
  }
  return state; 
}

// NAČIN KRMILJENJA Z NASTAVITVIJO TLAKA
void tlakMode()
{
  bool state = true;
  while(state == true)
  {
    Serial.println();
    Serial.println("KRMILJENJE Z NASTAVITVIJO TLAKA");
    Serial.println("1 - POLNJENJE");
    Serial.println("2 - IZVLEK");
    Serial.println("3 - POLNI IZVLEK");
    Serial.println("4 - MENJAVA NAČINA");
    Serial.println();
    Serial.read();
    while (!Serial.available()) {}
    int dane = Serial.parseInt();
    switch (dane){
      // polnjenje
      case 1:
        parametri();
        krmiljenjePID(tlakSet);
        state = nadaljuj();
        state = true;
        break;
      
      // praznjenje
      case 2:
        pomikSet = pomikIn();
        speedSet = hitrostIn();
        praznjenje(pomikSet, speedSet);
        state = nadaljuj();
        state = true;
        break;

      // polni izvlek bata
      case 3:
        int pomikFull = funPomik(100);
        int speedDef = funHitrost(200);
        praznjenje(pomikFull, speedDef);
        state = true;
        break;

      // menjava načna uporabe
      case 4:
        state = false;
        break;

      default:
        Serial.println("NAPAČEN VNOS");
        delay(1000);
        state = false;
        break;
    }
  }
}

// MOTOR SE VRTI
bool rotacija(int x, int y)
{
  stepper.setCurrentPosition(0);
  bool out;
  while(stepper.currentPosition() != x)
  {
    // SAFETY
    bool stop1 = stopSwitch();
    // SAFETY

    if(stop1 == false)
    {
      stepper.stop();
      out = false;
      break;
    } 
    else 
    {
      stepper.setSpeed(y);
      stepper.runSpeed();
      out = true;
    }
  }
  out = false;
  return out;
}

// USTAVLJANJE S STIKALI
bool stopSwitch()
{
  bool out = true;
  onSw1 = digitalRead(endSwitch1);
  onSw2 = digitalRead(endSwitch2);

  if (onSw1 == LOW || onSw2 == LOW)
  {
    out = false;
  }
  return out;
}

// // TLAČNI SENZOR
// float pressureSensor()
// {
//   const float BETA = 3950; // should match the Beta Coefficient of the thermistor
//   int analogValue = analogRead(tin);
//   float celsius = 1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
//   return celsius;
// }

// TLAČNI SENZOR
float pressureSensor()
{
  float pressure = mpr.readPressure(PA);
  return pressure;
}

// PID enable
void pidSetup(float goal)
{
  pid.begin();
  pid.setpoint(goal);
  pid.tune(kp, ki, kd);
  pid.limit(-2500, 2500);
}

// PID control
float pidControl()
{
  float sensorVal = pressureSensor();
  float output = pid.compute(sensorVal);
  return output;
}

// PID rotacija motorja
void rotacijaPID(int y)
{
  stepper.setCurrentPosition(0);
  if (y > 0)
  {
    while(stepper.currentPosition() != step)
    {
      stepper.setSpeed(y);
      stepper.runSpeed();
    }
  } 
  else if (y < 0)
  {
    while(stepper.currentPosition() != -step)
    {
      stepper.setSpeed(y);
      stepper.runSpeed();
    }
  }
}

// PID krmiljenje motorja
void krmiljenjePID(float x)
{
  pidSetup(x);
  bool state = true;
  float outVal;
  while(state == true)
  {
    // VARNOST
    bool varnost = stopSwitch();
    if(varnost == false)
    {
      state = false;
    }
    // VARNOST

    outVal = pidControl();
    rotacijaPID(outVal);
  }
}