/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<iomanip>
#include<opencv2/core/core.hpp>
#include"System.h"

#include "PracticalSocket.h"                 // For UDPSocket and SocketException
#include <iostream>                          // For cout and cerr
#include <cstdlib>                           // For atoi()
#include "opencv2/opencv.hpp"
#include <pthread.h>

#define PACK_SIZE 4096                       // Udp pack size
#define BUF_LEN 65540                        // Larger than maximum UDP packet size
#define BUF_STRING_LEN 128                   // Size of string buffer
#define T_WAIT 1/10                          // Waiting time to load a frame 
#define NAMEFILE "KeyFrameTrajectory.txt"    // File with camera points
#define LOCAL_PORT 8888                      // UDP port
#define STRINGPORT 9999                      // TCP port

using namespace std;

int wait_TCP;                                // Wait until the thread for TCP finish

struct thread_args {
    FILE *fin;
    TCPSocket* TCPServer;
};

bool ReadFrames(cv::Mat &frame);             // Read and decode frames received by the client
void HandleTCPClient(TCPSocket *sock);       // TCP client handling function
void *SendFile(void* arg);                   // Send points of the camera to the client

bool ReadFrames(cv::Mat &frame) {
   unsigned short servPort = LOCAL_PORT;
   UDPSocket sock(servPort);

   char buffer[BUF_LEN];                     // Buffer of received frame
   int recvMsgSize;                          // Size of received message
   string sourceAddress;                     // Address of datagram source
   unsigned short sourcePort;                // Port of datagram source
      
   try{
      do {
         recvMsgSize = sock.recvFrom(buffer, BUF_LEN, sourceAddress, sourcePort);
      } while ((unsigned)recvMsgSize > sizeof(int));
      
      int total_pack = ((int * ) buffer)[0];
      char * longbuf = new char[PACK_SIZE * total_pack];
      
      for (int i = 0; i < total_pack; i++) {
         recvMsgSize = sock.recvFrom(buffer, BUF_LEN, sourceAddress, sourcePort);
         if (recvMsgSize != PACK_SIZE) {
            continue;
         }
         memcpy( & longbuf[i * PACK_SIZE], buffer, PACK_SIZE);
      }
 
      cv::Mat rawData = cv::Mat(1, PACK_SIZE * total_pack, CV_8UC1, longbuf);
      frame = cv::imdecode(rawData, CV_LOAD_IMAGE_COLOR);
      
      if(frame.empty()){
         cerr << endl << "Failed to load image" << endl;
      }
      
      if (frame.size().width == 0) {
         cerr << "decode failure!" << endl;
         return false;
      }
      
      free(longbuf);
             
      return true;
   }
   catch (SocketException & e) {
      cerr << e.what() << endl;
      return false;
   }   
	
}


void HandleTCPClient(TCPSocket *sock) {
   cout << "Handling client ";
   try {
      cout << sock->getForeignAddress() << ":";
   } catch (SocketException e) {
      cerr << "Unable to get foreign address" << endl;
   }
   try {
      cout << sock->getForeignPort();
   } catch (SocketException e) {
      cerr << "Unable to get foreign port" << endl;
   }
   cout << endl;
}


void *SendFile(void* arg) {
   struct thread_args *my_arg = (struct thread_args*) arg;
   char BufferString[BUF_STRING_LEN];  
   char *res;

   while(1) {
      res=fgets(BufferString, BUF_STRING_LEN, (*my_arg).fin);
      
      if( res==NULL )
         break;

      (*my_arg).TCPServer->send(BufferString, strlen(BufferString));
   }
   
   fclose((*my_arg).fin);
   wait_TCP = 0;
   pthread_exit(0);
}

int main(int argc, char **argv)
{
   if(argc != 3) 
   {
      cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings" << endl;
      return 1;
   }
   
   struct thread_args *send_args;
   send_args = (struct thread_args*) malloc(sizeof(struct thread_args));
   wait_TCP = 0;
   
   // Create SLAM system. It initializes all system threads and gets ready to process frames.
   ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
   
   cout << endl << "-------" << endl;
   cout << "Start processing sequence ..." << endl;
   
   TCPServerSocket servSock(STRINGPORT);
   (*send_args).TCPServer = servSock.accept();
   HandleTCPClient((*send_args).TCPServer);   
   
   pthread_t thread1;
   
   float tframe=T_WAIT;
   cv::Mat frame;

   while (1) {
		
      while (ReadFrames(frame)!=true);

      // Pass the image to the SLAM system
      SLAM.TrackMonocular(frame,tframe);
         
      // Wait to load the next frame
      usleep((T_WAIT)*1e6);
      tframe+=T_WAIT;
      
      // If the file isn't already opened, save the file with camera points
      if(wait_TCP == 0){
         SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
         
      // Open the file
      if(((*send_args).fin = fopen(NAMEFILE, "r")) == NULL)
      {
         printf ("Error opening the file");
         exit (EXIT_FAILURE);
      }
   
      // Verify the file's lenght and then send it to the client only if it isn't empty
      fseek ((*send_args).fin, 0, SEEK_END);
      
      if (ftell((*send_args).fin) != 0){
         wait_TCP = 1;
         try {
            fseek ((*send_args).fin, 0, SEEK_SET);
            pthread_create (&thread1, NULL, SendFile , (void *) send_args);
      } catch (SocketException &e) {
            cerr << e.what() << endl;
            exit(1);
         }
      }
      else
         fclose((*send_args).fin);
         
      }
   }
   
   SLAM.Shutdown();
    
   return 0;
}