import com.shigeodayo.ardrone.processing.*;
import java.util.Timer;
import gab.opencv.*;
import java.awt.*; // for Rectangle

ARDroneForP5 ardrone;
OpenCV opencv;

boolean stop = false;
char state = 'n';
int start_time = 0;

void setup() {
  size(640, 360);

  ardrone=new ARDroneForP5("192.168.1.1");
  // connect to the AR.Drone
  ardrone.connect();
  // for getting sensor information
  ardrone.connectNav();
  // for getting video information
  ardrone.connectVideo();
  // start to control AR.Drone and get sensor and video data of it
  ardrone.start();
  
  // set OpenCV window
  opencv = new OpenCV(this, width, height);
  
  // set dataset for OpenCV
  opencv.loadCascade(OpenCV.CASCADE_FRONTALFACE); // face data
  //opencv.loadCascade(OpenCV.CASCADE_EYE); // eye data
  //opencv.loadCascade(OpenCV.CASCADE_MOUTH); // mouth data
}

void draw() {
  background(204);
  // getting image from AR.Drone
  // true: resizeing image automatically;
  // false: not resizing
  PImage img = ardrone.getVideoImage(false);
  if (img == null)
    return;

  // load an image ("img") to opencv 
  opencv.loadImage(img);
  
  // Image Processing
  //opencv.findCannyEdges(50, 200); // edge detection
  //opencv.blur(12); // shading off
  //img = opencv.getSnapshot();

  image(img, 0, 0); // display the processed image
  
  /**
  Rectangle[] faces = opencv.detect(); // detection
  noFill();
  stroke(0, 255, 0);
  strokeWeight(5);
  for (int i = 0; i < faces.length; i++) {
    rect(faces[i].x, faces[i].y, faces[i].width, faces[i].height);
  }
  **/

  // print out AR.Drone information
  // ardrone.printARDroneInfo();

  // getting sensor information of AR.Drone
  float pitch = ardrone.getPitch();
  float roll = ardrone.getRoll();
  float yaw = ardrone.getYaw();
  float altitude = ardrone.getAltitude();
  float[] velocity = ardrone.getVelocity();
  int battery = ardrone.getBatteryPercentage();

  String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude;
  text(attitude, 20, 85);
  String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1];
  text(vel, 20, 140);
  String bat = "battery:" + battery + " %";
  text(bat, 20, 170);

  float vx = velocity[0], vy = velocity[1];
  if(stop){
    int lr = (int)vy / 15;
    int fb = (int)vx / 15;
    ardrone.parallel(-lr, fb);
  }

  int ms = millis();
  String time = "time:" + ms;
  text(time, 20, 210);
  
  if(state=='w'){
    square(ms);
  }
}

//PC AR.Drone
// controlling AR.Drone through key input
void keyPressed() {
  stop = false;
  if (key == CODED) {
    if (keyCode == UP) {
      state='f';
      ardrone.forward(); // go forward
    } 
    else if (keyCode == DOWN) {
      state='b';
      ardrone.backward(); // go backward
    } 
    else if (keyCode == LEFT) {
      state='l';
      ardrone.goLeft(); // go left
    } 
    else if (keyCode == RIGHT) {
      state='r';
      ardrone.goRight(); // go right
    } 
    else if (keyCode == SHIFT) {
      ardrone.takeOff(); // take off, AR.Drone cannot move while landing
      stop = true;
    } 
    else if (keyCode == CONTROL) {
      state = 'n';
      ardrone.landing();
      // landing
    }
  }
  else {
    if (key =='w'){
      state='w';
      start_time = millis();

    }
    else if (key == 's') {
      state = 's';
      stop = true;
      ardrone.stop();
    } 
    else if (key == 'r') {
      ardrone.spinRight(); // spin right
    } 
    else if (key == 'l') {
      ardrone.spinLeft(); // spin left
    } 
    else if (key == 'u') {
      ardrone.up(); // go up
    }
    else if (key == 'd') {
      ardrone.down(); // go down
    }
    else if (key == '1') {
      ardrone.setHorizontalCamera(); // set front camera
    }
    else if (key == '2') {
      ardrone.setHorizontalCameraWithVertical(); // set front camera with second camera (upper left)
    }
    else if (key == '3') {
      ardrone.setVerticalCamera(); // set second camera
    }
    else if (key == '4') {
      ardrone.setVerticalCameraWithHorizontal(); //set second camera with front camera (upper left)
    }
    else if (key == '5') {
      ardrone.toggleCamera(); // set next camera setting
    }
    else if (key == 'e'){
      ardrone.parallel(-5,5);
    }
  }
}

void square(int ms){
  int time = ms - start_time;
  int speed = 10;
  if(time <1000){
    stop=true;
  }else  if(time < 4000){
    stop=false;
    ardrone.forward(speed);
  }else  if(time < 8000){
    stop = true;
  } else if (time < 11000){
    stop=false;
    ardrone.goRight(speed);
  } else if(time < 16000){
    stop = true;
  } else {
    stop = false;
    state = 'n';
    ardrone.landing();
  }
}