#include "definitions.h"
#include "vacuum.h"
#include "servo.h"
#include "bodytemp.h"
#include "behaviour.h"

void setup() {
  Serial.begin(115200);

  pinMode(PIN_VIBRATION,OUTPUT);
  analogWrite(PIN_VIBRATION,0);

  pinMode(PIN_BUTTON,INPUT_PULLDOWN);
  pinMode(PIN_LED,OUTPUT);
  digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off

  initVacuum();
  initBodytemp();


  pinMode(PIN_INPUTARM1,INPUT);
  pinMode(PIN_INPUTARM2,INPUT);
  pinMode(PIN_INPUTARM3,INPUT);

  pinMode(PIN_INPUTAUX,INPUT);

  initServos();
}




void loop() {
  unsigned long loopmillis=millis();
 
  
  bool body_present=checkBodypresence(loopmillis);
  digitalWrite(PIN_LED,!body_present);

  loopVacuum(loopmillis);

  
  bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed
  if (buttonstate) { //Button pressed
    setVacuum(false); //release
  }else{
    setVacuum(body_present);
  }

  bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm
  
  updateMatrixWeights(loopmillis,body_present,contact_main);
  updateServosByWeights(loopmillis);

  loopServos(loopmillis);





  //Print Debug Information
  static unsigned long last_print=0;
  if (loopmillis - last_print >100) {
    last_print=loopmillis;
    //printVacuumValues();
    //Serial.print(" Vac="); Serial.print(vac);

    printServoDebug();
    Serial.println();
    //printBodytempDebug();
    
    
    
    Serial.println();
  }

}