//ardrone
ARDroneForP5 ardrone;

//artoolkit
NyARBoard nya;

//navdata
ARDroneNavP5 navdata;

PFont font;

PrintWriter output_pitch;
PrintWriter output_roll;
PrintWriter output_yaw;
PrintWriter output_altitude;

// global variable
int threshold=100;
int img_size_x = 320;
int img_size_y = 240;

// Graphing tool
int numberOfGraphPoints=1000; // match width for simplicity in demo
float[] tempVals1 = new float[numberOfGraphPoints]; // initialize 1st array
float[] tempVals2 = new float[numberOfGraphPoints]; // initialize 2nd array
float[] tempVals3 = new float[numberOfGraphPoints]; // initialize 3rd array
float[] tempVals4 = new float[numberOfGraphPoints]; // initialize 4th array
int currentPoint=0; // the marker used to travel through the array.
int currentPointA=0;
int maxDeg = 90;
int minDeg = -90;

Serial commPort;
int previousGrabTime=0, timeStep=1; // low timestep for fast demo
float tempEnvF;
float x,y1,y2,y3,y4,px=0,py1=0,py2=0,py3=0,py4=0;
float xa,ya,pxa=0,pya=0;

double Kp=1,Ki=1,Kd=1;
// PID controller for pitch
double setpoint1 = 0;
double integral1 = 0;
double pre_error1,error1,derivative1;
int output1;

// PID controller for toll
double setpoint2 = 0;
double integral2 = 0;
double pre_error2,error2,derivative2;
int output2;

// PID controller for yaw
double setpoint3 = 0;
double integral3 = 0;
double pre_error3,error3,derivative3;
int output3;

// PID controller for altitude
double setpoint4 = 700; // altitude setpoint
double integral4 = 0;
double pre_error4,error4,derivative4;
int output4;

// For Least Square Parameter Estimation
float t = 0;
float timestep = 0.05;

void setup() {
  size(320,900,OPENGL);
  hint(ENABLE_OPENGL_4X_SMOOTH);

  background(255); // white
  currentPoint=0; // reset array marker
  commPort = new Serial(this, Serial.list()[1], 9600);

  colorMode(RGB, 100);
  font=createFont("FFScala", 15);

  setUpARToolKit();
  setUpDrone();

  // Create a new file in the sketch directory
  output_pitch = createWriter("Position_pitch.txt");
  output_roll = createWriter("Position_roll.txt");
  output_yaw = createWriter("Position_yaw.txt");
  output_altitude = createWriter("Position_altitude.txt");
}

void setUpARToolKit() {
  nya=new NyARBoard(this,320,240/5,"camera_para.dat","patt.hiro",80);
  nya.gsThreshold=threshold;
  nya.cfThreshold=0.4;
}

void drawMarkerPos(int[][] points) {
  textFont(font,8.0);
  stroke(100,0,0);
  fill(10,0,0);
  for(int i=0;i<4;i++) {
    ellipse(nya.pos2d[i][0], nya.pos2d[i][1],5,5);
  }
  fill(150,150,150);
  for(int i=0;i<4;i++) {
    text("("+nya.pos2d[i][0]+","+nya.pos2d[i][1]+")",nya.pos2d[i][0],nya.pos2d[i][1]);
  }
}

String angle2text(float a) {
  int i=(int)degrees(a);
  i=(i>0?i:i+360);
  return (i<100?"  ":i<10?" ":"")+Integer.toString(i);
}
String trans2text(float i) {
  return (i<100?"  ":i<10?" ":"")+Integer.toString((int)i);
}

void setUpDrone() {
  ardrone=new ARDroneForP5();
  navdata = new ARDroneNavP5();
  ardrone.connect("192.168.1.1");
  ardrone.connectNav();
  //ardrone.enableDemoData(); // Sunghun (Oct 14th, 2011)
  ardrone.connectVideo();
  ardrone.start();
}

void draw() {
  PImage img=ardrone.getVideoImageAsPImage();
  if(img==null)
    return;

  hint(DISABLE_DEPTH_TEST);
  image(img, 0, 0);
  hint(ENABLE_DEPTH_TEST);

  float pitch=ardrone.getPitch();
  float roll=ardrone.getRoll();
  float yaw=ardrone.getYaw();
  float altitude=ardrone.getAltitude();
  float[] velocity=ardrone.getVelocity();
  int battery=ardrone.getBatteryPercentage();
  //ardrone.printARDroneInfo(); // prints Navdata info in the processing output screen
  navdata.setControlAck();
 
 

  textFont(font,12);
  fill(0, 255, 0);
  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);
  text(threshold, 20, 190);
  String time="time:"+round(t)+" s";
  text(time, 20, 210);

  if(nya.detect(img)) {
    hint(DISABLE_DEPTH_TEST);

    textFont(font,25.0);
    fill((int)((1.0-nya.confidence)*100),(int)(nya.confidence*100),0);
    text((int)(nya.confidence*100)+"%",320-60,240-20);

    pushMatrix();
    textFont(font,10.0);
    fill(0,100,0,80);
    int x_width;
    int y_width;
    x_width = nya.pos2d[0][0]+nya.pos2d[1][0]+nya.pos2d[2][0]+nya.pos2d[3][0];
    y_width = nya.pos2d[0][1]+nya.pos2d[1][1]+nya.pos2d[2][1]+nya.pos2d[3][1];
    translate(x_width/4+50,y_width/4+50);
    int CenterPos_x;
    int CenterPos_y;
    CenterPos_x = (nya.pos2d[0][0]+nya.pos2d[1][0])/2;
    CenterPos_y = (nya.pos2d[0][1]+nya.pos2d[1][1])/2;
    text("CenterPos  "+CenterPos_x+","+CenterPos_y,0,0);
    text("TRANS "+trans2text(nya.trans.x)+","+trans2text(nya.trans.y)+","+trans2text(nya.trans.z),0,15);
    text("ANGLE "+angle2text(nya.angle.x)+","+angle2text(nya.angle.y)+","+angle2text(nya.angle.z),0,30);

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // Sunghun's ARDrone Hiro Tracking
    ardrone.setSpeed(50);

    int x_dist;
    int y_dist;
    x_dist = img_size_x/2 - CenterPos_x;
    y_dist = img_size_y/2 - CenterPos_y;
    text("x_dist  "+x_dist,0,45);
    text("y_dist  "+y_dist,0,60);

    // UAV position control
    int r = round(sqrt(sq(x_dist) + sq(y_dist)));
    if (r < 50) {
      ardrone.stop();
    }
    else if (r > 50) {
      if (x_dist > 0 && y_dist > 0) { // Region 1
        ardrone.forward();
        ardrone.goLeft();
      }
      else if (x_dist < 0 && y_dist > 0) { // Region 2
        ardrone.forward();
        ardrone.goRight();
      }
      else if (x_dist > 0 && y_dist < 0) { // Region 3
        ardrone.backward();
        ardrone.goLeft();
      }
      else if (x_dist < 0 && y_dist < 0) { // Region 4
        ardrone.backward();
        ardrone.goRight();
      }
    }

    popMatrix();
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    drawMarkerPos(nya.pos2d);
    hint(ENABLE_DEPTH_TEST);

    PGraphicsOpenGL pgl = (PGraphicsOpenGL) g;
    nya.beginTransform(pgl);

    stroke(255,200,0);
    translate(0,0,20);
    box(40);
    nya.endTransform();
  }

  output_pitch.println(pitch); // Write the coordinate to the file
  output_roll.println(roll);
  output_yaw.println(yaw);
  output_altitude.println(altitude);

  textFont(font,12);
  fill(0, 0, 0);
  text("Pitch", 320/2-20, 3*height/6);
  text("Roll", 320/2-20, 4*height/6);
  text("Yaw", 320/2-20, 5*height/6);
  text("Altitude", 320/2-20, 6*height/6);

  if (currentPoint < numberOfGraphPoints) { // don't exceed your arrays 
    if (millis() - previousGrabTime > timeStep) {
      // if it's time to grab the values for A and B
      // store them in the two float[] arrays

      float x = map(currentPoint,0,numberOfGraphPoints,0,width);  
      float y1 = map(pitch,minDeg,maxDeg,height/5,0);
      float y2 = map(roll,minDeg,maxDeg,height/5,0);
      float y3 = map(yaw,-270,270,height/5,0);
      float y4 = map(altitude,0,2000,height/5,0);
      float xa = map(currentPointA,0,numberOfGraphPoints,0,width);  
      float ya = map(tempEnvF,minDeg,maxDeg,height/5,0);    

      // plug the values into the arrays
      tempVals1[currentPoint] = pitch;
      tempVals2[currentPoint] = roll;
      tempVals3[currentPoint] = yaw;
      tempVals4[currentPoint] = altitude;

      currentPoint++; // add 1 to the current point 
      currentPointA++;
      previousGrabTime = millis(); // reset the wait time 

      stroke(0); 
      line (px,3*pya+py1,x,3*pya+y1);
      line (px,5*pya+py2,x,5*pya+y2);
      line (px,6.5*pya+py3,x,6.5*pya+y3);
      line (px,7.5*pya+py4,x,7.5*pya+y4);
      px = x; 
      py1 = y1;
      py2 = y2;
      py3 = y3;
      py4 = y4;
      stroke(255,0,0);  
      line (pxa,3*pya+pya,xa,3*pya+ya);
      line (pxa,5*pya+pya,xa,5*pya+ya);
      line (pxa,6.5*pya+pya,xa,6.5*pya+ya);
      line (pxa,8.5*pya+pya,xa,8.5*pya+ya);
      pxa = xa;  
      pya = ya;
    }

    if (numberOfGraphPoints == currentPoint) {
      size(320,900,OPENGL);
      hint(ENABLE_OPENGL_4X_SMOOTH);

      background(100); // white
      currentPoint=0; // reset array marker

      x=0;
      y1=0;
      y2=0;
      y3=0;
      y4=0;
      px=0;
      py1=0;
      py2=0;
      py3=0;
      py4=0;
      xa=0;
      ya=0;
      pxa=xa;
      pya=ya;
      currentPoint=0;
      currentPointA=0;
    }
  }

  // PID control - horizontal positioning of ARDrone
  // Pitch control
  pre_error1 =  setpoint1 - pitch;
  error1 = setpoint1 - pitch;
  integral1 = integral1 + (error1*timeStep);
  derivative1 = (error1 - pre_error1)/timeStep;
  output1 = int("(Kp*error1) + (Ki*integral1) + (Kd*derivative1)");
  if (output1 < -10) {
    output1 = -10;
  }
  else if (output1 > 10) {
    output1 = 10;
  }
  if (output1 >= 0) {
    ardrone.forward(output1);
  }
  else if (output1 < 0) {
    ardrone.backward(output1);
  }
  pre_error1 = error1;

  // Roll control
  pre_error2 =  setpoint2 - roll;
  error2 = setpoint2 - roll;
  integral2 = integral2 + (error2*timeStep);
  derivative2 = (error2 - pre_error2)/timeStep;
  output2 = int("(Kp*error2) + (Ki*integral2) + (Kd*derivative2)");
  if (output2 < -20) {
    output2 = -20;
  } else if (output2 > 20) {
    output2 = 20;
  }
  if (output2 >= 0) {
    ardrone.goLeft(output2);
  } else if (output2 < 0) {
    ardrone.goRight(output2);
  }
  pre_error2 = error2;

//  // Yaw control
//  pre_error3 =  setpoint3 - yaw;
//  error3 = setpoint3 - yaw;
//  integral3 = integral3 + (error3*timeStep);
//  derivative3 = (error3 - pre_error3)/timeStep;
//  output3 = int("(Kp*error3) + (Ki*integral3) + (Kd*derivative3)");
//  if (output3 < -30) {
//    output3 = -30;
//  } else if (output3 > 30) {
//    output3 = 30;
//  }
//  if (output3 >= 0) {
//    ardrone.spinLeft(output3);
//  } else if (output3 < 0) {
//    ardrone.spinRight(output3);
//  }
//  pre_error3 = error3;

//  // Altitude control
//  pre_error4 =  setpoint4 - altitude;
//  error4 = setpoint4 - altitude;
//  integral4 = integral4 + (error4*timeStep);
//  derivative4 = (error4 - pre_error4)/timeStep;
//  output4 = int("(Kp*error4) + (Ki*integral4) + (Kd*derivative4)");
//  if (output4 < -50) {
//    output4 = -50;
//  } else if (output4 > 50) {
//    output4 = 50;
//  }
//  if (output4 >= 0) {
//    ardrone.down(output4);
//  } else if (output4 < 0) {
//    ardrone.up(output4);
//  }
//  pre_error4 = error4;

  // For Least Square Parameter Estimation
  t = t + timestep;

  //  // For data collect
  //  if (t >= 15) {
  //    ardrone.landing();
  //    output_pitch.flush(); // Writes the remaining data to the file
  //    output_roll.flush();
  //    output_yaw.flush();
  //    output_altitude.flush();
  //    output_pitch.close(); // Finishes the file
  //    output_roll.close();
  //    output_yaw.close();
  //    output_altitude.close();
  //    exit(); // Stops the program
  //  }
}

// keyboard
void keyPressed() {
  t = 0;
  ardrone.enableDemoData();
  if(key==CODED) {
    if(keyCode==UP) {
      ardrone.forward();
    }
    else if(keyCode==DOWN) {
      ardrone.backward();
    }
    else if(keyCode==LEFT) {
      ardrone.goLeft();
    }
    else if(keyCode==RIGHT) {
      ardrone.goRight();
    }
    else if(keyCode==SHIFT) {
      ardrone.takeOff();
    }
    else if(keyCode==CONTROL) {
      ardrone.landing();
      output_pitch.flush(); // Writes the remaining data to the file
      output_roll.flush();
      output_yaw.flush();
      output_altitude.flush();
      output_pitch.close(); // Finishes the file
      output_roll.close();
      output_yaw.close();
      output_altitude.close();
      exit(); // Stops the program
    }
  }
  else {
    if(key=='s') {
      ardrone.stop();
    }
    else if(key=='r') {
      ardrone.spinRight();
    }
    else if(key=='l') {
      ardrone.spinLeft();
    }
    else if(key=='u') {
      ardrone.up();
    }
    else if(key=='d') {
      ardrone.down();
    }   
    if(key=='1') {
      ardrone.setFrontCameraStreaming();
    }
    else if(key=='2') {
      ardrone.setFrontCameraWithSmallBellyStreaming();
    }
    else if(key=='3') {
      ardrone.setBellyCameraStreamingResize();
    }
    else if(key=='4') {
      ardrone.setBellyCameraWithSmallFrontStreamingResize();
    }
    else if(key=='5') {
      ardrone.setNextCameraResize();
    }
    else if(key=='q') {
      threshold+=10;
      nya.gsThreshold=threshold;
    }
    else if(key=='w') {
      if(threshold>10) {
        threshold-=10;
        nya.gsThreshold=threshold;
      }
    }
  }
}