// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
// 전처리부
#include "I2Cdev.h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
#define mpu_add 0x68
#define Pin_Relay 13
MPU6050 mpu;


// 변수 및 상수부
long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z; //acc, gyro data (acc, gyro 계산 수식)
double angle = 0, deg; // angle, deg data (각도계산)
double dgy_x; // double type acc data
long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
long int angle_value;
const int mapping_value = 1000;
const int Emergency_value = 1300; // 충격상태로 판단하는 값
const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
boolean State_Parameter = false; // 충격상태
const long int sum_count = 2; // 평균 내는 횟수
const long interval = 1000; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
unsigned long previousMillis = 0; // 최초 시작시간
 

/* 사용자지정함수부
*/
// 모듈초기화
void mpu6050_init(){ 
  Wire.begin(); //I2C통신 시작
  Wire.beginTransmission(mpu_add) ; // 0x68(주소) 찾아가기
  Wire.write(0x6B) ; // 0x6B의 레지스트를 0으로 함으로써 Sleep에서 깨움
  Wire.write(0); // set to ZERO(WAKE UP THE MPU6050)
  Wire.endTransmission(true) ;
}

// 연산에 필요한 변수들을 초기화
void value_init(){ 
  normal_x = 0; 
  normal_y = 0; 
  normal_z = 0;
  
  for(int i = 0; i < 3; i++){ 
     deltha_x[i] = 0; 
     deltha_y[i] = 0; 
     deltha_z[i] = 0; 
     angle = 0; 
     angle_value = 0;
  }
}

// 가속도 연산 함수
void accel_calculate() { 
  ac_x = 0; 
  ac_y = 0; 
  ac_z = 0;
  normal_x = 0; 
  normal_y = 0; 
  normal_z = 0;

  Wire.beginTransmission(mpu_add) ; // 번지수 찾기
  Wire.write(0x3B) ; // 가속도 데이터 보내달라고 컨트롤 신호 보내기
  Wire.endTransmission(false) ; // 기달리고,
  Wire.requestFrom(mpu_add, 6, true) ; // 데이터를 받아 처리 

  // Data SHIFT
  ac_x = Wire.read() << 8 | Wire.read() ;
  ac_y = Wire.read() << 8 | Wire.read() ;
  ac_z = Wire.read() << 8 | Wire.read() ;
 
  //맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
  normal_x = map(int(ac_x), -16384, 16384, 0, mapping_value);
  normal_y = map(int(ac_y), -16384, 16384, 0, mapping_value);
  normal_z = map(int(ac_z), -16384, 16384, 0, mapping_value);
 
  //각도계산 deg -> 각도
  deg = atan2(ac_x, ac_z) * 180 / PI ; //rad to deg
  dgy_x = gy_y / 131. ; //16-bit data to 250 deg/sec
  angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ;
}

// 충격상태함수
void Emergency_state_(){
  digitalWrite(Pin_Relay , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
  Serial.println("SHOCK!!!!!!!!!!!!!!!!!"); // Serial 모니터로 확인
}

void Shock_Sensing(){
  State_Parameter = false; // 충격상태 초기화
  
  //첫번째 센싱
  for (int i=0; i < sum_count; i++){ 
    accel_calculate();
    deltha_x[1] = deltha_x[1]+(normal_x); 
    deltha_y[1] = deltha_y[1]+(normal_y); 
    deltha_z[1] = deltha_z[1]+(normal_z); 
    angle_value = angle_value + angle;
  }
  deltha_x[1] = int(deltha_x[1]/sum_count); 
  deltha_y[1] = int(deltha_y[1]/sum_count); 
  deltha_z[1] = int(deltha_z[1]/sum_count);

  //두번째 센싱
  for (int i=0; i < sum_count; i++){ 
    accel_calculate();
    deltha_x[2] = deltha_x[2]+(normal_x); 
    deltha_y[2] = deltha_y[2]+(normal_y); 
    deltha_z[2] = deltha_z[2]+(normal_z); 
    angle_value = angle_value + angle;
  }
  deltha_x[2] = int(deltha_x[2]/sum_count); 
  deltha_y[2] = int(deltha_y[2]/sum_count); 
  deltha_z[2] = int(deltha_z[2]/sum_count);
 
  //3축 변화량 비교 - 가속도 변화량, 각도 평균 값
  deltha_x[0] = abs(deltha_x[1]-deltha_x[2]); 
  deltha_y[0] = abs(deltha_y[1]-deltha_y[2]); 
  deltha_z[0] = abs(deltha_z[1]-deltha_z[2]);
  deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
  angle_value = abs(int(angle_value/(sum_count)));
 
  // deltha : 가속도 변화량
  // angle_value : 각도 값(현재 각도 값)
   if (deltha > Emergency_value){
       State_Parameter=true;
   }
  /*if (angle_value > Emergency_angle){
    State_Parameter=true;
    }
  if ((deltha > Emergency_value2)&&(angle_value > Emergency_angle2)){
    State_Parameter=true;
    }*/
 
  // 충격상태체크
  if( State_Parameter == true ){
    Emergency_state_();
  }
  else {
    digitalWrite(Pin_Relay , LOW);
  }
}



/*main 함수부*/
void setup() {
  Serial.begin(9600); // 시리얼 속도 설정
  Serial.println("Initializing I2C devices...");
  mpu6050_init(); // 가속도 센서 초기화 
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  pinMode(Pin_Relay, OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
  digitalWrite(Pin_Relay , LOW); // HIGH - LED on, LOW - LED off   (DEFAULT : OFF)
}

void loop() {
  value_init(); //가속도-각도 관련 초기값 선언
  unsigned long currentMillis = millis();
  if(currentMillis - previousMillis >= interval){
  Shock_Sensing();
  if(State_Parameter == true){
    previousMillis = currentMillis;
   }
 }
}