#include "ardrone/ardrone.h"

#include <vector>

#include <iostream>

// --------------------------------------------------------------------------

// main(Number of arguments, Argument values)

// Description  : This is the entry point of the program.

// Return value : SUCCESS:0  ERROR:-1

// --------------------------------------------------------------------------

int main(int argc, char *argv[])

{

// AR.Drone class

ARDrone ardrone;

// Initialize

if (!ardrone.open()) {

std::cout << "Failed to initialize." << std::endl;

return -1;

}

// Battery

std::cout << "Battery = " << ardrone.getBatteryPercentage() << "[%]" << std::endl;

// Instructions

std::cout << "***************************************" << std::endl;

std::cout << "*       CV Drone sample program       *" << std::endl;

std::cout << "*           - How to play -           *" << std::endl;

std::cout << "***************************************" << std::endl;

std::cout << "*                                     *" << std::endl;

std::cout << "* - Controls -                        *" << std::endl;

std::cout << "*    'Space' -- Takeoff/Landing       *" << std::endl;

std::cout << "*    'Up'    -- Move forward          *" << std::endl;

std::cout << "*    'Down'  -- Move backward         *" << std::endl;

std::cout << "*    'Left'  -- Turn left             *" << std::endl;

std::cout << "*    'Right' -- Turn right            *" << std::endl;

std::cout << "*    'Q'     -- Move upward           *" << std::endl;

std::cout << "*    'A'     -- Move downward         *" << std::endl;

std::cout << "*                                     *" << std::endl;

std::cout << "* - Others -                          *" << std::endl;

std::cout << "*    'C'     -- Change camera         *" << std::endl;

std::cout << "*    'Esc'   -- Exit                  *" << std::endl;

std::cout << "*                                     *" << std::endl;

std::cout << "***************************************" << std::endl;

while (1) {

// Key input

int key = cv::waitKey(33);

if (key == 0x1b) break;

// Get an image

cv::Mat image = ardrone.getImage();

cv::Mat frame = image.clone();

//방법1.

//반복문으로 각 화소 모두 비교하는 방법

//컬러 공간 변환 BGR->YCrCb

cv::Mat YCrCb;

cvtColor(frame, YCrCb, CV_BGR2YCrCb);

//각 채널별로 분리

std::vector<cv::Mat> planes;

cv::split(YCrCb, planes);

cv::Mat mask = (128 < planes[1]) & (planes[1] < 170) & (73 < planes[2]) & (planes[2] < 158);

erode(mask, mask, cv::Mat(3, 3, CV_8U, cv::Scalar(1)), cv::Point(-1, -1), 2);

// Take off / Landing 

if (key == ' ') {

if (ardrone.onGround()) ardrone.takeoff();

else                    ardrone.landing();

}

// Move

double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;

if (key == 'i' || key == CV_VK_UP)    vx = 1.0;

if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;

if (key == 'u' || key == CV_VK_LEFT)  vr = 1.0;

if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;

if (key == 'j') vy = 1.0;

if (key == 'l') vy = -1.0;

if (key == 'q') vz = 1.0;

if (key == 'a') vz = -1.0;

ardrone.move3D(vx, vy, vz, vr);

// Change camera

static int mode = 0;

if (key == 'c') ardrone.setCamera(++mode % 4);

// Display the image

cv::imshow("camera", image);

cv::imshow("humancamera", mask);

}

// See you

ardrone.close();

return 0;

}