#include <CoDrone.h> // 코드론 라이브러리
#define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
#define buzzer 7 // 피에조 버저 핀번호
#define LeftVib 27 // 왼쪽 진동자 핀번호
#define RightVib 28 // 오른쪽 진동자 핀번호

// 변수 및 상수부
long int ac_x, ac_y, ac_z;
long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
const int mapping_value = 5000;
const int Emergency_value = 300; // 충격상태로 판단하는 값
boolean State_Parameter = false; // 충격상태 판단
long int shock_level;
const long int sum_count = 4; // 평균 내는 횟수
long shock_sum = 0; // 누적 충격값

//사용자지정 함수부
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; 
  }
}
// 가속도 연산 함수
void accel_calculate() 
{ 
  ac_x = 0; 
  ac_y = 0; 
  ac_z = 0;

  // Data SHIFT From CoDrone Accelerometer
  acceldata accel;
  accel = CoDrone.getAccelerometer(); // CoDrone에서 가속도 전달받음.
  ac_x = accel.x; // 넘겨 받은 x축 raw data
  ac_y = accel.y; // 넘겨 받은 y축 raw data
  ac_z = accel.z; // 넘겨 받은 z축 raw data
  
  // 맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
  normal_x = map(int(ac_x), -16384, 16383, -1 * mapping_value, mapping_value);
  normal_y = map(int(ac_y), -16384, 16383, -1 * mapping_value, mapping_value);
  normal_z = map(int(ac_z), -16384, 16383, -1 * mapping_value, mapping_value);

}

// 충격상태함수
void Emergency_state_()
{
  digitalWrite(ShockLed , HIGH); // LED를 True값으로 바꿔 12번 핀의 LED를 ON시킨다.
  digitalWrite(buzzer, HIGH); // buzzer를 True값으로 바꿔 
  if(ac_x < 0)
  {
    analogWrite(LeftVib ,map(int(shock_level), 0, 15000, 0, 255));
  }
  else
  {
    analogWrite(RightVib ,map(int(shock_level), 0, 15000, 0, 255));
  }
}

// 충격 감지함수
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); 
  }
  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); 
  }
  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];

  // deltha : 가속도 변화량
  if (deltha > Emergency_value)
  {
      State_Parameter=true;
  }
  // 충격상태체크
  if( State_Parameter == true )
  {
    Emergency_state_();
  }
  else 
  {
    digitalWrite(ShockLed, LOW);
    digitalWrite(buzzer, LOW);
  }
}

// 누적 충격량 계산
void ShockCal()
{
  Shock_Sensing();
  if(State_Parameter == true)
    {
      shock_sum += deltha;
    }
    if(State_Parameter == false && shock_sum != 0)
    {
      shock_level = shock_sum;  // 충격 누적값 전송
      shock_sum = 0;  // 충격의 지속이 끝났으니 원래값인 0으로 초기화
    }
}

// Main 함수부
void setup() 
{
  // 부속 하드웨어 제어
  pinMode(LeftVib,OUTPUT); // 핀모드사용, 왼쪽 진동자
  pinMode(RightVib,OUTPUT); // 핀모드사용, 오른쪽 진동자
  pinMode(ShockLed,OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
  pinMode(buzzer,OUTPUT); // 핀모드사용, 버저
  
  digitalWrite(ShockLed,LOW); // HIGH - LED on, LOW - LED off   (DEFAULT : OFF)
  digitalWrite(buzzer,LOW); // HIGH - buzzer on, buzzer - LED off   (DEFAULT : OFF)
  
  CoDrone.begin(115200);  // 드론 플러그의 통신 개시 (115200bps)
  CoDrone.AutoConnect(NearbyDrone); // 가장 가까운 위치의 드론과 연결
  CoDrone.DroneModeChange(Flight);  // 드론을 플라이트 모드로 설정
}

void loop() 
{
  value_init(); //가속도-각도 관련 초기값 선언

  byte bt1 = digitalRead(11);       // ■ □ □ □ □ □ □ 밑면 적외선 센서를 입력으로 사용
  byte bt4 = digitalRead(14);       // □ □ □ ■ □ □ □ 밑면 적외선 센서를 입력으로 사용
  byte bt8 = digitalRead(18);       // □ □ □ □ □ □ ■ 밑면 적외선 센서를 입력으로 사용
  if (bt1 && !bt4 && !bt8)          // 밑면 센서 가장 끝 11번 센서에 손을 대면 실행합니다.
  {
    CoDrone.FlightEvent(Stop);      // 드론을 정지시킵니다.
  }
  if (!bt1 && !bt4 && bt8)          // 밑면 센서 가장 끝 18번 센서에 손을 대면 실행합니다.
  {
    CoDrone.FlightEvent(Landing);   // 드론을 착륙시킵니다.
  }

    
  if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행
  {
    
    ShockCal();
    YAW = -1 * CoDrone.AnalogScaleChange(analogRead(A3)); // 아날로그 3번 핀의 값을 YAW 값으로 사용합니다. - 좌우회전
    THROTTLE = CoDrone.AnalogScaleChange(analogRead(A4)); // 아날로그 4번 핀의 값을 THROTTLE 값으로 사용합니다. - 승하강
    ROLL = -1 * CoDrone.AnalogScaleChange(analogRead(A5)); // 아날로그 5번 핀의 값을 ROLL 값으로 사용합니다. - 좌우이동
    PITCH = CoDrone.AnalogScaleChange(analogRead(A6)); // 아날로그 6번 핀의 값을 PITCH 값으로 사용합니다. - 전후진
    CoDrone.Control(1);
  }
}