#include "Arduino.h"
#include "vacuum.h"


double vacuumPressureRelease=8.5; //If pressure is below this value and vacuum is turned off, valves will open
double vacuumPressureReleasehysteresis=0.5; //hysteresis for release pressure
double vacuumPressureSuck=2.0;

double vacuum1=0; //Input
double vacuumPump1=0; //Output
double vacuumSetpoint1=vacuumPressureSuck;

double vacuum2=0; //Input
double vacuumPump2=0; //Output
double vacuumSetpoint2=vacuumSetpoint1;

bool vacuumEnabled=false;

double Kp=0.8, Ki=0.01, Kd=0.0;
PID pump1PID(&vacuum1, &vacuumPump1, &vacuumSetpoint1, Kp, Ki, Kd, REVERSE);
PID pump2PID(&vacuum2, &vacuumPump2, &vacuumSetpoint2, Kp, Ki, Kd, REVERSE);

void initVacuum() {
  pinMode(PIN_PRESSURE1_OUT, INPUT);   // Connect HX710 OUT
  pinMode(PIN_PRESSURE2_OUT, INPUT);   // Connect HX710 OUT
  pinMode(PIN_PRESSURE_SCK, OUTPUT);  // Connect HX710 SCK

  pinMode(PIN_PUMP1, OUTPUT);
  digitalWrite(PIN_PUMP1, LOW);

  pinMode(PIN_PUMP2, OUTPUT);
  digitalWrite(PIN_PUMP2, LOW);

  pinMode(PIN_VALVE1,OUTPUT);
  pinMode(PIN_VALVE2,OUTPUT);

  pump1PID.SetOutputLimits(0.0, 1.0);
  pump1PID.SetMode(AUTOMATIC);

  pump2PID.SetOutputLimits(0.0, 1.0);
  pump2PID.SetMode(AUTOMATIC);

}


void loopVacuum(unsigned long millis) {
  static unsigned long last_pressurereading = 0;
  if (millis - last_pressurereading > VACUUM_UPDATE_INTERVAL)
  {
    last_pressurereading=millis;
    vacuum1 = readHX710B(PIN_PRESSURE1_OUT) / 1000000.0;
    vacuum2 = readHX710B(PIN_PRESSURE2_OUT) / 1000000.0;

    
    pump1PID.Compute();

    if (vacuumEnabled) {
      digitalWrite(PIN_VALVE1,LOW); //close valve
    }else{
      vacuumPump1=0.0; //turn off pump
      if (!digitalRead(PIN_VALVE1) && vacuum1<vacuumPressureRelease-vacuumPressureReleasehysteresis) {
        digitalWrite(PIN_VALVE1,HIGH); //open valve
      }else if (digitalRead(PIN_VALVE1) && vacuum1>vacuumPressureRelease+vacuumPressureReleasehysteresis) {
        digitalWrite(PIN_VALVE1,LOW); //close valve
      }
      
    }

    if (vacuumEnabled) {
      pump2PID.Compute();
      digitalWrite(PIN_VALVE2,LOW); //close valve
    }else{
      vacuumPump2=0.0; //turn off pump
      if (!digitalRead(PIN_VALVE2) && vacuum2<vacuumPressureRelease-vacuumPressureReleasehysteresis) {
        digitalWrite(PIN_VALVE2,HIGH); //open valve
      }else if (digitalRead(PIN_VALVE2) && vacuum2>vacuumPressureRelease+vacuumPressureReleasehysteresis) {
        digitalWrite(PIN_VALVE2,LOW); //close valve
      }
      
    }
    
    
    analogWrite(PIN_PUMP1,constrain(vacuumPump1,0.0,1.0)*255);
    analogWrite(PIN_PUMP2,constrain(vacuumPump2,0.0,1.0)*255);
  }
}

float getAveragePumpspeed(){
  return (vacuumPump1+vacuumPump2)/2.0;
}

void setVacuum(bool val) {
  if (val) { //True = turn off, False = Release
    pump2PID.SetMode(AUTOMATIC);
    vacuumSetpoint1=vacuumPressureSuck;    
    vacuumSetpoint2=vacuumSetpoint1;
    vacuumEnabled=true;
  }else{
    pump2PID.SetMode(MANUAL);
    vacuumEnabled=false;
  }
}

void printVacuumValues() {
  Serial.print(vacuum1, 3);
    Serial.print(" -> ");
    Serial.print(vacuumSetpoint1, 1);
    Serial.print(" vacuumPump1="); Serial.print(vacuumPump1);
    
    
    Serial.println();
    Serial.print(vacuum2, 3);
    Serial.print(" -> ");
    Serial.print(vacuumSetpoint2, 1);
    Serial.print(" vacuumPump2="); Serial.print(vacuumPump2);
}

long readHX710B(uint8_t outpin) {
  //This function takes 95 microseconds
  // pulse the clock line 3 times to start the next pressure reading
  for (char i = 0; i < 3; i++) {
    digitalWrite(PIN_PRESSURE_SCK, HIGH);
    digitalWrite(PIN_PRESSURE_SCK, LOW);
  }
  while (digitalRead(outpin)) {}  //TODO: add timeout


  // read 24 bits
  long result = 0;
  for (int i = 0; i < 24; i++) {
    digitalWrite(PIN_PRESSURE_SCK, HIGH);
    digitalWrite(PIN_PRESSURE_SCK, LOW);
    result = result << 1;
    if (digitalRead(outpin)) {
      result++;
    }
  }

  // get the 2s compliment
  result = result ^ 0x800000;
  return result;
}