#include "../include/Djitellowhite.h"
#include <iostream>
#include <opencv2/opencv.hpp>


#include<stdio.h>
#include<sys/socket.h>
#include<string.h>

#include<arpa/inet.h>
#include<sys/types.h>

using namespace cv;
using namespace std;

Tello::Tello(const char* IP) {
	this->stream_on=false;
	this->host = inet_addr(IP);
	cout << "create successfully!" << endl;
	
}
Tello::~Tello() {}
void Tello::set_conf(int socket){
	this->sock = socket;
}
void Tello::get_connect(sockaddr_in serveraddr) {
	char tmp[10] = {"command"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "connect successfully!" <<endl;
}
unsigned long Tello::gethost(){
	return this->host;
}
void Tello::takeoff(sockaddr_in serveraddr) {
	char tmp[10] = {"takeoff"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "takeoff successfully!" <<endl;
}
void Tello::land(sockaddr_in serveraddr) {
	char tmp[10] = {"land"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "land successfully!" <<endl;
}

bool Tello::send_message(char* msg,sockaddr_in server_addr,int timeout){
	char buffer[BUFFER_SIZE];
	memset(buffer,0,sizeof(buffer));
	strcpy(buffer,msg);
	//cout << buffer;
	sendto(this->sock,buffer,strlen(buffer),0,(struct sockaddr *)&server_addr, sizeof(server_addr));
	return true;
}
void* Tello::udp_response_receiver(void* arg){

	//int m_SockServer;            //创建socket对象           
	//sockaddr_in serveraddr;         //创建sockaddr_in对象储存自身信息(当有多个端口，可以多个绑定)
	//sockaddr_in serveraddrfrom;

	//serveraddr.sin_family = AF_INET;             //设置服务器地址家族
	//serveraddr.sin_port = htons(8889);           //设置服务器端口号
	//serveraddr.sin_addr.s_addr = inet_addr("0.0.0.0");
	//m_SockServer = socket(AF_INET, SOCK_DGRAM, 0);    //创建一个临时变量并赋值给m_SockServer
	//int i = bind(m_SockServer, (sockaddr*)&serveraddr, sizeof(serveraddr));    //把名字和套接字绑定
	//std::cout << "bind:" << i << std::endl;

	int socket = *(int *)arg;	
	
	cout << "receive start" << endl;
	while(1){
		char buffer[20];
		int iret;
		memset(buffer,0,sizeof(buffer));
		
		iret = recv(socket, buffer,20,MSG_WAITALL);
		if(iret > 0 && iret <10){
			printf("Message from tello: %s\n", buffer);	
		}
	}
}
void Tello::forward(sockaddr_in serveraddr){
	char tmp[15] = {"forward 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "forward successfully!" <<endl;
}
void Tello::back(sockaddr_in serveraddr){
	char tmp[15] = {"back 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "back successfully!" <<endl;
}
void Tello::left(sockaddr_in serveraddr){
	char tmp[15] = {"left 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "left successfully!" <<endl;
}
void Tello::right(sockaddr_in serveraddr){
	char tmp[15] = {"right 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "right successfully!" <<endl;
}
void Tello::up(sockaddr_in serveraddr){
	char tmp[15] = {"up 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "up successfully!" <<endl;
}
void Tello::down(sockaddr_in serveraddr){
	char tmp[15] = {"down 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "down successfully!" <<endl;
}
void Tello::cw(sockaddr_in serveraddr){
	char tmp[15] = {"cw 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "cw successfully!" <<endl;
}
void Tello::ccw(sockaddr_in serveraddr){
	char tmp[15] = {"ccw 20"};
	send_message(tmp,serveraddr,0);// 0 -> timeout
	cout << "ccw successfully!" <<endl;
}
void Tello::get_video(sockaddr_in serveraddr){
	
	char tmp[15] = {"streamon"};
	send_message(tmp,serveraddr,0);
	cout << "stream_on successfully!" <<endl;
	this->stream_on = true;
	
	cout << "vid";
	VideoCapture cap("udp://@0.0.0.0:11111");
	cout << "cap";
	if(!cap.isOpened()){
		cout << "fuck off";
		return;
	}

	Mat frame;
	cout << "continue!";
	while(1){
		cap>>frame;
		if(frame.empty())
			break;
		imshow("video",frame);
		waitKey(20);
		
	}
	cap.release();
	
	
}
