// Group 50 Project A
#include "Vincent.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "U8glib.h"
#include <MemoryUsage.h>
U8GLIB_SSD1306_128X64 u8g; // initializing the OLED screen

//Global variables
char Password[8]{"EEE20003"}; //The password string
bool pass{false};
float * AccVec();
float * Vadd{};
float forceVec[3]{};
const int gyroLength = 20;
float gyrosD[gyroLength][3]{};
int gyrocycle = 0; 
float currVec[3]{};
unsigned long lastmillis=0;
bool Falling= false;

void setup() 
{
  Serial.begin(115200);
  u8g.setFont(u8g_font_tpss); //setting the text font
  u8g.setColorIndex(1); // setting colour to white

  WritePassword(Password); // Password should only need to be written the first time the board is programmed, I haven't tried to test this in the simulation.




  while(!pass)
  {
  pass = LogOn(u8g);
  } 
  //Serial.println(pass);


    //Serial.println(pass);
}

void loop() {
//SRamDisplay();
Vadd=AccVec();
forceVec[0]=*Vadd;
forceVec[1]=*(Vadd+1);
forceVec[2]=*(Vadd+2);
gyroCheck();
/*
Serial.print("x= "); 
Serial.println(*Vadd);
Serial.print("y= ");
Serial.println(forceVec[1]);
Serial.print("z= ");
Serial.println(forceVec[2]);
*/

  u8g.firstPage();  
  do {
    //changing screen colour if ACC is too high (possible fall) doesnt work
    if(abs(forceVec[0]) > 19 || abs(forceVec[1]) > 19 || abs(forceVec[2]) > 19 ||Falling){
      u8g.drawStr(20, 20, "FALL DETECTED!!!");
      u8g.drawStr(22, 50, "DAILING 000 NOW");
    }
    else{
    u8g.setColorIndex(1);
    u8g.drawFrame(64, 0, 64, 64);
    u8g.drawStr(0, 10, "X =");
    String strX = String(*Vadd);
    char charX[7];
    strX.toCharArray(charX, 7);
    u8g.drawStr(30, 10, charX);

    u8g.drawStr(0,38, "Y =");
    String strY = String(*(Vadd+1));
    char charY[7];
    strY.toCharArray(charY, 7);
    u8g.drawStr(30, 38, charY);

    u8g.drawStr(0, 64, "Z =");
    String strZ = String(*(Vadd+2));
    char charZ[7];
    strZ.toCharArray(charZ, 7);
    u8g.drawStr(30, 64, charZ);
    
    u8g.drawDisc(96 + forceVec[0] * 0.8, 32 + forceVec[1] * 0.8, (22 + forceVec[2]) * 0.3);
    }
    
    

    

  } while( u8g.nextPage() );

}

////////////////////////////
int16_t ax, ay, az;
int16_t gx, gy, gz;


float * AccVec()
{
  Adafruit_MPU6050 gyro;
    while (!gyro.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }


  //sensors_event_t event;
  sensors_event_t event, g, temp;
  gyro.getEvent(&event, &g, &temp);

  float vector[6]{};
  float * vecPt{vector};

  //gyro.getAccelerometerSensor()->getEvent(&event);

  vector[0]=static_cast<float>(event.acceleration.x);
  vector[1]=static_cast<float>(event.acceleration.y);
  vector[2]=static_cast<float>(event.acceleration.z);
  vector[3]=static_cast<float>(g.gyro.x);
  vector[4]=static_cast<float>(g.gyro.y);
  vector[5]=static_cast<float>(g.gyro.z);
  currVec[0]=static_cast<float>(g.gyro.x);
  currVec[1]=static_cast<float>(g.gyro.y);
  currVec[2]=static_cast<float>(g.gyro.z);


    currVec[0] = static_cast<float>(g.gyro.x);

  //Serial.println(pass);



  return vecPt;
  
}


///////////////////////////
void gyroCheck(){
  if(millis()-lastmillis>100){
    lastmillis = millis();
    Falling = false;
    for(int i = 0;i<3;i++){
      gyrosD[gyrocycle][i]=currVec[i];
    }
    for(int i = 0;i<gyroLength;i++){
      Serial.print("X Angle: ");
      Serial.println(gyrosD[(gyrocycle+i)%gyroLength][0]);
    }
      Serial.print("Cycle Count: ");
      Serial.println(gyrocycle);
      
      /*float Diffr[3]{};
      for(int i = 0; i<3;i++){
        Diffr[i] = gyrosD[(gyrocycle+5)%6][i]-gyrosD[(gyrocycle)%6][i];
        //Serial.println(Diffr[i]);
      }*/
      float gyroSum[3]{};
      for(int x = 0;x<3;x++){
        for(int i = 0;i<gyroLength;i++){
          gyroSum[x] += gyrosD[i][x];//gives approx angular displacement
        }
        Serial.println(gyroSum[x]);
      }
      for(int i = 0;i<3;i++){
        if(abs(gyroSum[i])>=1||(abs(gyrosD[gyrocycle][i])>=0.5&&abs(gyrosD[(gyrocycle+1)%gyroLength][i])>=0.5)){
        Falling= true;
        Serial.print("Fall on ");
        Serial.print(i);
        Serial.println(" axis.");
        }
      }





      /*if(
        (
      
      
        )
      )*/


    gyrocycle = (gyrocycle+=1)%gyroLength;
  }
  
  //Serial.println(pass);

}