import com.shigeodayo.ardrone.processing.*;
import java.util.Timer;

ARDroneForP5 ardrone;

//OpenCV opencv;
boolean stop = false;
char state = 'n';
int start_time = 0;
float yaw_stop = 0;

void setup() {
  size(640, 360);
  stop = false;
  ardrone = new ARDroneForP5("192.168.1.1");
  // connect to the AR.Drone
  ardrone.connect();
  // for getting sensor information
  ardrone.connectNav();
  // for getting video informationp
  ardrone.connectVideo();
  // start to control AR.Drone and get sensor and video data of it
  ardrone.start();
}

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;
  image(img, 0, 0);

  // 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 + "\nyaw_stop:" + yaw_stop;
  text(attitude, 20, 85);
  String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1];
  text(vel, 20, 160);
  String bat = "battery:" + battery + " %";
  text(bat, 20, 190);
  String is_stop = "stop:" + stop;
  text(is_stop, 20, 210);

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

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

//PC AR.Drone
// controlling AR.Drone through key input
void keyPressed() {
  stop = false;
  if (key == CODED) {
    if (keyCode == UP) {
      ardrone.forward(); // go forward
    } 
    else if (keyCode == DOWN) {
      ardrone.backward(); // go backward
    } 
    else if (keyCode == LEFT) {
      ardrone.goLeft(); // go left
    } 
    else if (keyCode == RIGHT) {
      ardrone.goRight(); // go right
    } 
    else if (keyCode == SHIFT) {
      ardrone.takeOff(); // take off, AR.Drone cannot move while landing
      delay(2500);
      stop = true;
      yaw_stop = ardrone.getYaw();
    } 
    else if (keyCode == CONTROL) {
      ardrone.landing(); // landing
      state = 'n';
      stop = false;
    }
  }
  else {
    if (key =='w'){
      state = 'w';
      start_time = millis();
    }
    else if (key == 's') { // hovering
      ardrone.stop();
      state = 's';
      stop = true;
      yaw_stop = ardrone.getYaw();
    } 
    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 = 5;
  if(time < 1000){ //hitsuyou??
    yaw_stop = ardrone.getYaw();
    stop = true;
  } else if(time < 4000){ //0~4s go forward for 4s
    stop = false;
    ardrone.forward(speed);
  } else if(time < 7000){ //4~7s stop for 3s
    //yaw_stop = ardrone.getYaw();
      
      
    stop = true;
  } else if (time < 11000){ //7~11s go right for 4s
    stop = false;
    ardrone.goRight(speed+2);
  } else if(time < 14000){ //11~14s stop for 3s
    //yaw_stop = ardrone.getYaw();
    stop = true;
  } else if(time < 18000){ //14~18s go backward for 4s
    stop = false;
    ardrone.backward(speed+2);
  } else if(time < 21000){ //18~21s stop for 3s
    //yaw_stop = ardrone.getYaw();
    stop = true;
  } else if(time < 25000){ //21~25s go left for 4s
    stop = false;
    ardrone.goLeft(speed+2);
  } else if(time < 28000){ //25~28s stop for 3s
    //yaw_stop = ardrone.getYaw();
    stop = true;
  } else { //28s~ landing
    stop = false;
    ardrone.landing();
  }
}