/*
 * 
 Authors: P.K.Iosifidis, I. Kyprakis
 * 
 * 
 */
//libs
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include "model_header.h" //include model.h file

//definitions
#define NUM_AXES 3
#define ACCEL_THRESH 0.2 
#define samples 100 //init samples from imu
#define channels 24
#define sound_limit 20

//buttons & other stuff
const int button_power=27;
const int button_mute=14;
const int led_power=13;
const int led_lr=12;
bool power_state = false;
bool mute_state = false;

//global vars
int pred=0;
long timer = 0;
int volume=10;
int channel=12;
float acc_stats[NUM_AXES]={0.0, 0.0, 0.0};
float offsets[NUM_AXES]={0.0, 0.0, 0.0}; //initialize offsets as GLOBALS
float movement[NUM_AXES]={0.0, 0.0, 0.0};
MPU6050 mpu6050(Wire, 0.99, 0.01);
Eloquent::ML::Port::RandomForest model; //init model

//MQTT dawg

//hotspot pier (change with your creds)
//const char* ssid = "krotalias";
//const char* password = "savrespantou123";

// MQTT broker IP address change it!
const char* host = "192.168.87.5";

WiFiClient espClient3;
PubSubClient client3(espClient3);

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu6050.begin();
  mpu_init(mpu6050); //init mpu
  pinMode(led_power, OUTPUT);
  digitalWrite(led_power, LOW);
  pinMode(button_power, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(button_power), powerISR, FALLING);
  pinMode(button_mute, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(button_mute), muteISR, FALLING);
  WiFi.begin(ssid, password);
  Serial.print("Connecting.");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nWiFi connected - IP address: ");
  Serial.println(WiFi.localIP());
  client3.setServer(host, 1883);
}

void loop() {
  //connect
  client3.loop();
  if (!client3.connected()) {
      reconnect();
  }
  update_state(mpu6050);
  
  if (power_state){
    digitalWrite(led_power, HIGH);
    if (motion_detect()){
      mov_grab(mpu6050);
      //print_lx(movement); 
      //gen_data(movement); //ONLY TO FILL THE DATASET

      //classify
      pred = model.predict(movement);
      invoke_action(pred);
      
      //Serial.print(model.idxToLabel(pred));Serial.print(".\n"); //debug
      for (int i=0;i<3;i++) //reset movement vectorx
        movement[i]=0.0; 
      if (!pred || pred==2) //volume
        delay(200);
      else if (pred==1 || pred==3)
        delay(1000); //1sec to see whats playing hehe
      }
  }else
    pred = NULL;
    digitalWrite(led_power, LOW);
  
  //publish state every second or so
  if(millis() - timer > 1000){ 
    String formattedString = String(power_state ? 1 : 0) + "_" + String(pred) + "_" + String(channel) + "_" + String(volume) + "_" + String(mute_state ? 1 : 0);
    client3.publish("remote", formattedString.c_str());
    //Serial.println(formattedString);//debug
    timer = millis(); //reset timer
  }
}

void mpu_init(MPU6050 obj){
  Serial.println("DO NOT MOVE THE SYSTEM! CALIBRATING...");
  int i;
  for (i=0; i<samples; i++){
    obj.update(); //update to get new values and get xyz values
    offsets[0] += obj.getAccX();
    offsets[1] += obj.getAccY();
    offsets[2] += obj.getAccZ(); 
  }
  for (i=0; i<3;i++){
    offsets[i] = offsets[i]/100; //get mean values for each axis if you want you can have the whole vector
  }
  Serial.print("OFFSETS: ");
  print_lx(offsets);
}

//motion detection if > than given threshold (adapt it in your case)
bool motion_detect(){
  return (abs(acc_stats[0]) + abs(acc_stats[1])+ abs(acc_stats[2])) > ACCEL_THRESH;
}

void invoke_action(int p){ //doesn't matter change it
  switch (p){
    case 0:{ //volume increase
      if (mute_state)
        Serial.println("Volume: MUTE");
      else{
        if (volume <20)
          volume++;
        if (volume==20)
          Serial.println("Volume: MAX");
        else
          {Serial.print("Volume: ");Serial.print(volume);Serial.print('\n');}
      }
      break;
    }
    case 2:{ //volume decrease  
      if (!volume || mute_state)
        Serial.println("Volume: MUTE");
      else{
        if (volume>0)
          volume--;
        Serial.print("Volume: ");Serial.print(volume);Serial.print('\n');
        }
      break;
    }
    case 1:{ //channel reduction, reset to 24 if it reaches 0
      channel--;
      if (channel==0)
        channel = 24;
      Serial.print("Channel: ");Serial.print(channel);Serial.print('\n');
      break;
    }
    case 3:{
      channel++;
      if (channel==25)
        channel = 1;
      Serial.print("Channel: ");Serial.print(channel);Serial.print('\n'); 
      break;
    }
    default:{
      Serial.println("FATAL ERROR, XOF");
      break;
    }
  }
}

void mov_grab(MPU6050 obj){
  int i;
  for (i=0; i<samples; i++){
    update_state(obj);
    movement[0] += acc_stats[0];
    movement[1] += acc_stats[1];
    movement[2] += acc_stats[2]; 
  }
  for (i=0; i<3;i++){
    movement[i] = movement[i]/100; //get mean movement (again change)
  }
}

//print acceleration stats
void print_lx(float *x){
    Serial.print("accX : ");Serial.print(*x);
    Serial.print("\taccY : ");Serial.print(*(x+1));
    Serial.print("\taccZ : ");Serial.println(*(x+2));
}

//generate data
void gen_data(float *x){
  Serial.print(*x);     Serial.print(","); //x
  Serial.print(*(x+1)); Serial.print(","); //y
  Serial.print(*(x+2));Serial.print("\n"); //z
}

//update mpu state and subtract any offeset if you have
void update_state(MPU6050 obj){
  obj.update();
  //values here are calculated by environmental measuring
  acc_stats[0] = obj.getAccX() - offsets[0];
  acc_stats[1] = obj.getAccY() - offsets[1];
  acc_stats[2] = obj.getAccZ() - offsets[2]; 
}

void reconnect() {
  while (!client3.connected()) {
    Serial.println("Attempting MQTT connection...");
    if (client3.connect("ESP32 client")) {
      Serial.println("Connected");
    }
    else {
      Serial.print(client3.state());
      Serial.println("Failed - Try again in 5 seconds...");
      delay(5000);
    }
  }
}

void powerISR(){ //flag change remove (interrupts)
  power_state = !power_state;
}
  
void muteISR(){
  if (power_state)
    mute_state = !mute_state;
}
