#include <BleCombo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <TinyPICO.h>

/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
#define BAT_VOLTAGE 35

// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                    id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
TinyPICO tp = TinyPICO();

int acc_vec_count = 0;
int Xacc_vec[10] = {};
int Yacc_vec[10] = {};
int Zacc_vec[10] = {};

int buff_vec_count = 0;
int Xbuff_vec[20] = {};
int Ybuff_vec[20] = {};
int Zbuff_vec[20] = {};

boolean gesture = false;
boolean cursor = false;
boolean presenting = false;
int presentation_stop_count = 0;
int laser_count = 0;
int acc_total = 0;
int x_orient, y_orient, z_orient;
int x_acc, y_acc, z_acc;
float batt_voltage;


void displaySensorStatus(void)
{
  /* Get the system status values (mostly for debugging purposes) */
  uint8_t system_status, self_test_results, system_error;
  system_status = self_test_results = system_error = 0;
  bno.getSystemStatus(&system_status, &self_test_results, &system_error);

  /* Display the results in the Serial Monitor */
  Serial.println("");
  Serial.print("System Status: 0x");
  Serial.println(system_status, HEX);
  Serial.print("Self Test:     0x");
  Serial.println(self_test_results, HEX);
  Serial.print("System Error:  0x");
  Serial.println(system_error, HEX);
  Serial.println("");
  delay(500);
}

void displaySensorDetails(void)
{
  sensor_t sensor;
  bno.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

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

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  delay(1000);

  /* Display some basic information on this sensor */
  displaySensorDetails();

  /* Optional: Display current status */
  displaySensorStatus();

  bno.setExtCrystalUse(true);

  /* Set the values */
  Xacc_vec[9] = 100;
}

int findMax(int* array, int size){
  /*
  Finds maximum of an array
  :global array: Array of numbers
  :global size: Size of array
  */
  
  int max = array[0];
  for (int i=1; i<size; i++){
    if (max < array[i]){
      max = array[i];
    }
  }
  return max;
}

int findMin(int* array, int size){
  /*
  Finds minimum of an array
  :global array: Array of numbers
  :global size: Size of array 
  */
  
  int min = array[0];
  for (int i=1; i<size; i++){
    if (min > array[i]){
      min = array[i];
    }
  }
  return min;
}

void moveMouse() {
  /*
  Moves the cursor of mouse every 50 ms
  :global presenting: True/false of running the presentation
  :global cursor: True/false of mourse curson activity
  :global y_orient: Spatial orientation of the sensor in Y axis
  :global z_orient: Spatial orientation of the sensor in Z axis
  */
  
  if( (presenting == true) && (cursor == false) ){
    // Pohyb mysi neumoznen - prezentujes a neni zaply laser!
  }
  else{
    // Mouse move
    int y_move = y_orient / 1.5;
    int z_move = (z_orient + 60) / 1.8;
    Mouse.move(-y_move, -z_move, 0);
  }
}

void clickMouse() {
  /*
  Double-clicks the mouse left button
  :global Xacc_vec: Acceleration vector of 10 samples in X axis
  :global Yacc_vec: Acceleration vector of 10 samples in Y axis
  :global Zacc_vec: Acceleration vector of 10 samples in Z axis
  :global x_acc: Current acceleration in X axis
  :global y_acc: Current acceleration in Y axis
  :global z_acc: Current acceleration in Z axis
  :global acc_vec_count: Current position in acceleration vector of 10 samples
  :global presenting: True/false of running the presentation
  */
  
  Xacc_vec[acc_vec_count] = x_acc;
  Yacc_vec[acc_vec_count] = y_acc;
  Zacc_vec[acc_vec_count] = z_acc;
  acc_vec_count = acc_vec_count + 1;
  
  if(Xacc_vec[9] != 100) {
    int Xacc_vec_max = findMax(Xacc_vec, sizeof(Xacc_vec)/sizeof(Xacc_vec[0]));
    int Xacc_vec_min = findMin(Xacc_vec, sizeof(Xacc_vec)/sizeof(Xacc_vec[0]));
    int Xacc_vec_abs = abs(Xacc_vec_min - Xacc_vec_max);

    int Yacc_vec_max = findMax(Yacc_vec, sizeof(Yacc_vec)/sizeof(Yacc_vec[0]));
    int Yacc_vec_min = findMin(Yacc_vec, sizeof(Yacc_vec)/sizeof(Yacc_vec[0]));
    int Yacc_vec_abs = abs(Yacc_vec_min - Yacc_vec_max);

    int Zacc_vec_max = findMax(Zacc_vec, sizeof(Zacc_vec)/sizeof(Zacc_vec[0]));
    int Zacc_vec_min = findMin(Zacc_vec, sizeof(Zacc_vec)/sizeof(Zacc_vec[0]));
    int Zacc_vec_abs = abs(Zacc_vec_min - Zacc_vec_max);

    acc_vec_count = 0;
    Xacc_vec[9] = 100;

    int XYZ_abs = Xacc_vec_abs + Yacc_vec_abs + Zacc_vec_abs;

    if( (XYZ_abs > 35) && (presenting == false) ){
      // Left double click
      Serial.println("Levy dvojklik mysi - jeste neprezentujes!");
      Mouse.click(MOUSE_LEFT);
      Mouse.click(MOUSE_LEFT);
    }
    else if( (XYZ_abs > 35) && (presenting == true) ) {
      Serial.println("Levy dvojklik mysi neumoznen - prezentujes!");
    }
  }
}

int gestureDetection() {
  /*
  Detects movement of the hand (dynamic gesture)
  :global x_acc: Current acceleration in X axis
  :global y_acc: Current acceleration in Y axis
  :global z_acc: Current acceleration in Z axis
  :global acc_vec_count: Current position in acceleration vector of 10 samples
  :global Xbuff_vec: Position data vector of 20 samples in X axis
  :global Ybuff_vec: Position data vector of 20 samples in Y axis
  :global Zbuff_vec: Position data vector of 20 samples in Z axis
  :global buff_vec_count: Current position in position data vector of 20 samples
  :global acc_total: Total acceleration in X, Y and Z axis
  :global presenting: True/false of running the presentation
  :global gesture: True/false of making the gesture
  */
 
  int acc_total = abs(x_acc) + abs(y_acc) + abs(z_acc);
  
  if(gesture == true){
    Xbuff_vec[buff_vec_count] = x_orient;
    Ybuff_vec[buff_vec_count] = y_orient;
    Zbuff_vec[buff_vec_count] = z_orient;
    buff_vec_count += 1;
  }
  if(acc_total < 1){
    gesture = false;
    buff_vec_count = 0;      
  }
  else{
    gesture = true;
  }
}

void stopPresentation() {
  /*
  Detects closing the presentation
  :global y_orient: Spatial orientation of the sensor in Y axis
  :global z_orient: Spatial orientation of the sensor in Z axis
  :global acc_total: Total acceleration in X, Y and Z axis
  :global presentation_stop_count: Cumulative counter for counting the presentation stop treshold
  :global presenting: True/false of running the presentation
  :global cursor: True/false of running the cursor
  */
 
  if( (presenting == true) && (acc_total < 3) && (z_orient < -110) ){
    if( (z_orient > -155) && (y_orient < 10) && (y_orient > -30) ){
      presentation_stop_count += 1;
    }
  }
  else{
    presentation_stop_count = 0;
  }
  if( (presentation_stop_count == 15) && (cursor == false) ){
    Serial.println("Zavri prezentaci!");
    Keyboard.press(KEY_ESC);
    Keyboard.releaseAll();
    presentation_stop_count = 0;
    presenting = false;
  }
  else if( (presentation_stop_count == 15) && (cursor == true) ){
    Serial.println("Zahod laser a zavri prezentaci!");
    Keyboard.press(KEY_LEFT_CTRL);
    Keyboard.press('l');
    Keyboard.releaseAll();
    delay(20);
    Keyboard.press(KEY_ESC);
    Keyboard.releaseAll();
    presentation_stop_count = 0;
    presenting = false;
  }
}

void getLaser() {
  /*
  Detects activating/deactivang the laser
  :global y_orient: Spatial orientation of the sensor in Y axis
  :global z_orient: Spatial orientation of the sensor in Z axis
  :global acc_total: Total acceleration in X, Y and Z axis
  :global laser_count: Cumulative counter for counting the laser detection treshold
  :global cursor: True/false of running the cursor
  */
  
  if( (presenting == true) && (acc_total < 3) && (z_orient < 25) ){
    if( (z_orient > -20) && (y_orient < 25) && (y_orient > -25) ){
      laser_count += 1;
    }
  }
  else{
    laser_count = 0;
  }
  if( laser_count == 15 ){
    if(cursor == false){
        Serial.println("Podej laser!");
        Keyboard.press(KEY_LEFT_CTRL);
        Keyboard.press('l');
        Keyboard.releaseAll();
        delay(1000);
        cursor = true;
        laser_count = 0;
    }
      else if(cursor == true){
        Serial.println("Zahod laser a skryj kurzor!");
        Keyboard.press(KEY_LEFT_CTRL);
        Keyboard.press('l');
        Keyboard.releaseAll();
        delay(20);
        Keyboard.press(KEY_LEFT_CTRL);
        Keyboard.press('h');
        Keyboard.releaseAll();
        delay(1000);
        cursor = false;
        laser_count = 0;
    }
  }
}

void gestureAnalysis() {
  /*
  Analyses the buffers and detects the gestures
  :global Xbuff_vec: Position data vector of 20 samples in X axis
  :global Ybuff_vec: Position data vector of 20 samples in Y axis
  :global Zbuff_vec: Position data vector of 20 samples in Z axis
  :global buff_vec_count: Current position in position data vector of 20 samples
  :global presenting: True/false of running the presentation
  */
  
  if(buff_vec_count == 20){
 
    // X has to start at 180dg (signal can fluctuate e.g. within <340; 30> which is not suitable for further analysis of buffer)
    if(Xbuff_vec[0]-180 > 0){
      int diff = Xbuff_vec[0]-180;
      for(int i=0; i<20; i++){
        if(Xbuff_vec[i] - diff <= 0){
          Xbuff_vec[i] = (360 + Xbuff_vec[i]) - diff;
        }
        else{
          Xbuff_vec[i] = Xbuff_vec[i] - diff;
        }
      }
    }
    else if(Xbuff_vec[0]-180 < 0){
      int diff = Xbuff_vec[0]-180;
      diff = abs(diff);
      for(int i=0; i<20; i++){
        if(Xbuff_vec[i] - diff >= 0){
          Xbuff_vec[i] = Xbuff_vec[i] + diff;
          if(Xbuff_vec[i] > 360){
            Xbuff_vec[i] = (Xbuff_vec[i] - 360) + diff;
          }
        }
        else{
          Xbuff_vec[i] = Xbuff_vec[i] + diff;
        }
      }
    }
    
    // Find max & min in vectors, calculate abs
    // X
    int Xbuff_vec_max = findMax(Xbuff_vec, sizeof(Xbuff_vec)/sizeof(Xbuff_vec[0]));
    int Xbuff_vec_min = findMin(Xbuff_vec, sizeof(Xbuff_vec)/sizeof(Xbuff_vec[0]));
    int Xbuff_vec_abs = abs(Xbuff_vec_min - Xbuff_vec_max);
    // Y
    int Ybuff_vec_max = findMax(Ybuff_vec, sizeof(Ybuff_vec)/sizeof(Ybuff_vec[0]));
    int Ybuff_vec_min = findMin(Ybuff_vec, sizeof(Ybuff_vec)/sizeof(Ybuff_vec[0]));
    int Ybuff_vec_abs = abs(Ybuff_vec_min - Ybuff_vec_max);
    // Z
    int Zbuff_vec_max = findMax(Zbuff_vec, sizeof(Zbuff_vec)/sizeof(Zbuff_vec[0]));
    int Zbuff_vec_min = findMin(Zbuff_vec, sizeof(Zbuff_vec)/sizeof(Zbuff_vec[0]));
    int Zbuff_vec_abs = abs(Zbuff_vec_min - Zbuff_vec_max);
    
    // Analysis (condition based) + key press
    if( (Xbuff_vec_abs < 30) && (Ybuff_vec_abs < 25) && (Zbuff_vec_abs > 70) && (Zbuff_vec[0] < Zbuff_vec[10]) ){
      Serial.println("Spust prezentaci a skryj kurzor!");
      Keyboard.press(KEY_F5);
      Keyboard.releaseAll();
      delay(20);
      Keyboard.press(KEY_LEFT_CTRL);
      Keyboard.press('h');
      Keyboard.releaseAll();
      presenting = true;
    }
    else if( (Xbuff_vec_abs > 50) && (Xbuff_vec_abs < 100) && (Ybuff_vec_abs < 50) && (Ybuff_vec_abs > 0) && (Zbuff_vec_min > -110) && (Zbuff_vec_min < -30) && (Xbuff_vec[0] > Xbuff_vec[10]) && (Ybuff_vec[0] > Ybuff_vec[10]) && (presenting == true) ){
      Serial.println("Posun prezentace vpred!");
      Keyboard.press('n');
      Keyboard.releaseAll();
    }
    else if( (Xbuff_vec_abs > 100) && (Xbuff_vec_abs < 170) && (Ybuff_vec_abs > 15) && (Ybuff_vec_abs < 65) && (Zbuff_vec_min < -90) && (Xbuff_vec[0] < Xbuff_vec[10]) && (Ybuff_vec[0] < Ybuff_vec[10]) && (Zbuff_vec[0] > Zbuff_vec[10]) && (presenting == true) ){
      Serial.println("Posun prezentace vzad!");
      Keyboard.press('p');
      Keyboard.releaseAll();
    }

    buff_vec_count = 0;
  }
}
  

void loop() { 

  batt_voltage = tp.GetBatteryVoltage();
  
  if((Keyboard.isConnected()) && (batt_voltage > 2.75)) {

    // Get a new sensor event
    sensors_event_t event;
    bno.getEvent(&event);
    x_orient = event.orientation.x;
    y_orient = event.orientation.y;
    z_orient = event.orientation.z;

    // Get linear acceleration vector
    imu::Vector<3> acc = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
    x_acc = round(acc.x());
    y_acc = round(acc.y());
    z_acc = round(acc.z());
    
    moveMouse();
    clickMouse();
    gestureDetection();
    stopPresentation();
    getLaser();
    gestureAnalysis();
   
  }
 
  delay(50);

}
