#include <string>
#include <Controller.h>
#include <ControllerEvent.h>
#include <Logger.h>
#include <ViewImage.h>
#include <math.h> 
#include <stdio.h>

//Header files for using socket in the controller
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <unistd.h>
#include <netinet/in.h>
#include <errno.h>
//#include <fcntl.h>

#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) 
 
#define MAXPENDING 5    /* Max connection requests */
#define BUFFSIZE 512
#define MYPORT 9020
class RobotController : public Controller
{
 public:
   void onInit(InitEvent &evt);
   double onAction(ActionEvent &evt);
   void onRecvMessage(RecvMessageEvent &evt);
      
   void handleImageSending(int iClientSock); //accept connections, sending image, close connection
   int iImgIdx;
 private:  
 
   //?????(???????????????)
   bool move_eye; 
 
   //?????????
   double dvvx; 
 
   //?????????????
   int dy;
 
   //????
   double vel;
   
   //For handling sockets
   int iServerSock, iClientSock;
   struct sockaddr_in skaddrServer, skaddrClient;
   bool bConnectedFlag;
 };
 
 void RobotController::onInit(InitEvent &evt)
 {  
   move_eye = false;
   dvvx     =  0.1;                                                                
   dy       = 45;                                                                      
   vel      = 10.0;
   
/*Init socket for sending/receiving data*/
   if ( (iServerSock = socket(AF_INET,SOCK_STREAM,IPPROTO_IP)) <0)
   {	LOG_MSG(("Failed to create socket"));  }
   else
   	LOG_MSG(("Successful to create a socket\n"));
	
/* Construct the server sockaddr_in structure */
	memset(&skaddrServer, 0, sizeof(skaddrServer));       /* Clear struct */
	skaddrServer.sin_family = AF_INET;                  /* Internet/IP */
	skaddrServer.sin_addr.s_addr = htonl(INADDR_ANY);   /* Incoming addr */
	skaddrServer.sin_port = htons(MYPORT);       /* server port */

/* Bind the server socket */
	if (bind(iServerSock, (struct sockaddr *) &skaddrServer, sizeof(skaddrServer)) < 0)
	{	LOG_MSG(("Failed to bind the server socket: %s\n",strerror(errno))); }
	else
	{   	LOG_MSG(("Successful to bind the server socket\n")); }
	   	
/* Listen on the server socket */
	if (listen(iServerSock, MAXPENDING) < 0)
	{	LOG_MSG(("Failed to listen on server socket: %s\n",strerror(errno))); }
	else
	{   	LOG_MSG(("Listening to clients\n")); }
	iImgIdx = 0; //for sending images to client		
 }
 
 //?????????????????
 void RobotController::onRecvMessage(RecvMessageEvent &evt)
 {
  int n = evt.getSize();
  static int iImage = 0;

  LOG_MSG(("Number of messages : %d", n));
  if (n>0)
  {       
      std::string msg = evt.getString(0);
      LOG_MSG(("msg : %s", msg.c_str()));     
        
      if (strcmp(msg.c_str(), "capture") == 0)
      {
	  ViewImage *img = captureView(COLORBIT_ANY, IMAGE_320X240);
	  if (img)
	  {	      
	      LOG_MSG(("view%03d.jpg(%d, %d)", iImage, img->getWidth(), img->getHeight()));
 	            
	      char fname[256];
	      sprintf(fname, "../../SendImage_Tutorial/view%03d.jpg", iImage++);
	      img->saveAsWindowsBMP(fname);

	      delete img;
	  }
      }
      else if (strcmp(msg.c_str(), "open socket") == 0)
      {
	  unsigned int iAddrLen = sizeof(sockaddr);
	  if ((iClientSock = accept(iServerSock, (struct sockaddr *) &skaddrServer, &iAddrLen)) < 0) 
	  {	LOG_MSG(("Failed to accept client connection")); }
	  else
		LOG_MSG(("Accepted connection"));
      }
      else if (strcmp(msg.c_str(), "close socket") == 0)
      {	
	  close(iClientSock);
	  LOG_MSG(("Socket closed"));
      }
      else if (strcmp(msg.c_str(), "get image") == 0 )
      {      					
	  handleImageSending(iClientSock);
      }       
      else if (strcmp(msg.c_str(), "rotation") == 0)
      {
	  SimObj *my = getObj(myname());
 	 
	  // ???y??????????(???????)
	  double qy = my->qy();
 
	  //????????????(????)??????
	  double theta = 2*asin(qy);
 
	  //???????????
	  double y = theta + DEG2RAD(45);
	  if( y >= PI)
	  	y = y - 2 * PI;
	  
	  my->setAxisAndAngle(0, 1.0, 0, y);
      }
      //?????"move"??????????????????????
      else if (strcmp(msg.c_str(), "move") == 0)
      {
	  SimObj *my = getObj(myname());

	  //??????????
	  double x = my->x();
	  double y = my->y();
	  double z = my->z();
	
	  //y??????????????(???????)	
	  double qy = my->qy();
	
	  //???????????????????
	  double theta = 2*asin(qy);
	
	  //????????????
	  double dx = 0.0;
	  double dz = 0.0;
	
	  //?????????????
	  dx = sin(theta) * vel;
	  dz = cos(theta) * vel;
	
	  //??????
	  my->setPosition( x + dx, y , z + dz );	
      }
    }
 }
 
 void RobotController::handleImageSending(int iClientSock)
 { 	
 	FILE * fi = fopen("view000.jpg","rb");
	if (fi==NULL)
	{
		LOG_MSG(("Failed to open image file"));
		return;
	}
	/*Send the file size to client*/
	char sSize[10];
	long iSize;
		
	fseek(fi,0,SEEK_END);
	iSize = ftell(fi);
	fseek(fi,0,SEEK_SET);
	sprintf(sSize,"%d",iSize);
	write(iClientSock,sSize,sizeof(sSize));	
	
	/*Start to send an image to client*/
	LOG_MSG(("Start sending image"));
	char buf[512] = "";
	sleep(20); //wait a little for client to receive image size

	while (!feof(fi))
	{
		fread(buf,sizeof(char),512,fi);
		write(iClientSock,buf,sizeof(buf));		
	}    	
	fclose(fi);
    	LOG_MSG(("Finish sending image"));
 }

 //????????????
 double RobotController::onAction(ActionEvent &evt)
 {
  
  SimObj *my = getObj(myname()); 
 
  //???????x?????????
  double vvx = my->evx1();
 
  double dvvx_tmp;
  //???????????
  if (move_eye == false)
    {
      dvvx_tmp = -1 * dvvx;
    }
  //????????           
  else dvvx_tmp = dvvx;
 
  //??????????
  vvx = vvx + dvvx_tmp;
  my->evx1(vvx);  
 
 
  //???????x???-1.0??1.0?????????
  if(vvx >= 1.0)
    {
      move_eye = false;
    }
  if( vvx <= -1.0)
    {
      move_eye = true;
    } 
    
  return 0.5;
 }
 
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }