#include "definitions.h"
#include "vacuum.h"
#include "servo.h"
#include "bodytemp.h"
#include "behaviour.h"
#include "barometer.h"
#include "imu.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

  initBehaviour();

  initVacuum();
  initBodytemp();

  initBarometer();

  initIMU(7.54,-15.26,-1.41);


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

  pinMode(PIN_INPUTAUX,INPUT);

  initServos();
}




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

  loopVacuum(loopmillis);


  loopBarometer(loopmillis);
  loopIMU(loopmillis);
  
  bool bellyup=isBellyUp();

  
  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
  bool contact_peripheralL=digitalRead(PIN_INPUTARM2); //Sensor on tip of arm
  bool contact_peripheralR=digitalRead(PIN_INPUTARM3); //Sensor on tip of arm

  float pitch=-getAngleY();
  float roll=-getAngleX();

  float maxAccX=getMaxAccX();
  float maxAccY=getMaxAccY();
  float maxAccZ=getMaxAccZ();

  updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),maxAccX,maxAccY,maxAccZ,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
  updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
  updateServosByWeights(loopmillis,pitch,roll);

  loopServos(loopmillis);





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

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

    
    Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
    Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
    Serial.print("Height:");Serial.print(getBarometerHeight(),3); Serial.println();
    Serial.print("Contact:");Serial.print(contact_main); Serial.print(", "); Serial.print(contact_peripheralL); Serial.print(", "); Serial.print(contact_peripheralR);Serial.println();
    
    
    //Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
    float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
    Serial.print("Accel: ");Serial.print(maxAccX); Serial.print(", ");Serial.print(maxAccY); Serial.print(", ");Serial.print(maxAccZ); Serial.print(", max: ");Serial.print(maxAcc); Serial.println();
    printMood();
    
    printModes();
    
    
    Serial.println();
  }

}