#Receiver side
FPS="30"
netcat -l -p 5000 | mplayer -vf scale -zoom -xy 1280 -fps $FPS -cache-min 50 -cache 1024 - &

#RPi side
FPS="30"
mkfifo netcat_fifo
raspivid -t 0 -md 5 -fps $FPS -o - | tee --output-error=warn netcat_fifo > $video_out &
cat netcat_fifo | netcat -v 192.168.0.101 5000 &> $netcat_log &

#RPi side
FPS="30"
MULTIPIPE="ftee"
mkfifo netcat_fifo
raspivid -t 0 -md 5 -fps $FPS -o - | ./${MULTIPIPE} netcat_fifo > $video_out &
cat netcat_fifo | mbuffer --direct -t -s 2k 2> $mbuffer_log | netcat -v 192.168.0.101 5000 &> $netcat_log &

////////////////////////////////////////////////////////////////////////////////
// main.cpp
// Mark Setchell
//
// Read video stream from"raspivid" and write (independently) to both disk file
// and stdout - for onward netcatting to another host.
//
// Compiles with:
//    g++ main.cpp -o main -lpthread
//
// Run on Raspberry Pi with:
//    raspivid -t 0 -md 5 -fps 30 -o - | ./main video.h264 | netcat -v 192.168.0.8 5000
//
// Receive on other host with:
//    netcat -l -p 5000 | mplayer -vf scale -zoom -xy 1280 -fps 30 -cache-min 50 -cache 1024 -
////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>
#include <unistd.h>
#include
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#define BUFSZ    65536
#define NBUFS    64

class Buffer{
   public:
   int bytes=0;
   std::atomic<int> NeedsWriteToDisk{0};
   std::atomic<int> NeedsWriteToFifo{0};
   unsigned char data[BUFSZ];
};

std::vector<Buffer> buffers(NBUFS);

////////////////////////////////////////////////////////////////////////////////
// This is the DiskWriter thread.
// It loops through all the buffers waiting in turn for each one to become ready
// then writes it to disk and marks the buffer as written before moving to next
// buffer.
////////////////////////////////////////////////////////////////////////////////
void DiskWriter(char* filename){
   int bufIndex=0;

   // Open output file
   int fd=open(filename,O_CREAT|O_WRONLY|O_TRUNC,S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP);
   if(fd==-1)
   {
      std::cerr <<"ERROR: Unable to open output file" << std::endl;
      exit(EXIT_FAILURE);
   }

   bool Error=false;
   while(!Error){

      // Wait for buffer to be filled by main thread
      while(buffers[bufIndex].NeedsWriteToDisk!=1){
   //      std::this_thread::sleep_for(std::chrono::milliseconds(1));
      }

      // Write to disk
      int bytesToWrite=buffers[bufIndex].bytes;
      int bytesWritten=write(fd,reinterpret_cast<unsigned char*>(&buffers[bufIndex].data),bytesToWrite);
      if(bytesWritten!=bytesToWrite){
         std::cerr <<"ERROR: Unable to write to disk" << std::endl;
         exit(EXIT_FAILURE);
      }

      // Mark buffer as written
      buffers[bufIndex].NeedsWriteToDisk=0;

      // Move to next buffer
      bufIndex=(bufIndex+1)%NBUFS;
   }
}

////////////////////////////////////////////////////////////////////////////////
// This is the FifoWriter thread.
// It loops through all the buffers waiting in turn for each one to become ready
// then writes it to the Fifo, flushes it for reduced lag, and marks the buffer
// as written before moving to next one. Errors are ignored.
////////////////////////////////////////////////////////////////////////////////
void FifoWriter(){
   int bufIndex=0;

   bool Error=false;
   while(!Error){

      // Wait for buffer to be filled by main thread
      while(buffers[bufIndex].NeedsWriteToFifo!=1){
    //     std::this_thread::sleep_for(std::chrono::milliseconds(1));
      }

      // Write to fifo
      int bytesToWrite=buffers[bufIndex].bytes;
      int bytesWritten=write(STDOUT_FILENO,reinterpret_cast<unsigned char*>(&buffers[bufIndex].data),bytesToWrite);
      if(bytesWritten!=bytesToWrite){
         std::cerr <<"ERROR: Unable to write to fifo" << std::endl;
      }
      // Try to reduce lag
      fflush(stdout);

      // Mark buffer as written
      buffers[bufIndex].NeedsWriteToFifo=0;

      // Move to next buffer
      bufIndex=(bufIndex+1)%NBUFS;
   }
}

int main(int argc, char *argv[])
{  
   int bufIndex=0;

   if(argc!=2){
      std::cerr <<"ERROR: Usage" << argv[0] <<" filename" << std::endl;
      exit(EXIT_FAILURE);
   }
   char * filename = argv[1];

   // Start disk and fifo writing threads in parallel
   std::thread tDiskWriter(DiskWriter,filename);
   std::thread tFifoWriter(FifoWriter);

   bool Error=false;
   // Continuously fill buffers from"raspivid" on stdin. Mark as full and
   // needing output to disk and fifo before moving to next buffer.
   while(!Error)
   {
      // Check disk writer is not behind before re-using buffer
      if(buffers[bufIndex].NeedsWriteToDisk==1){
         std::cerr <<"ERROR: Disk writer is behind by" << NBUFS <<" buffers" << std::endl;
      }

      // Check fifo writer is not behind before re-using buffer
      if(buffers[bufIndex].NeedsWriteToFifo==1){
         std::cerr <<"ERROR: Fifo writer is behind by" << NBUFS <<" buffers" << std::endl;
      }

      // Read from STDIN till buffer is pretty full
      int bytes;
      int totalBytes=0;
      int bytesToRead=BUFSZ;
      unsigned char* ptr=reinterpret_cast<unsigned char*>(&buffers[bufIndex].data);
      while(totalBytes<(BUFSZ*.75)){
         bytes = read(STDIN_FILENO,ptr,bytesToRead);
         if(bytes<=0){
            Error=true;
            break;
         }
         ptr+=bytes;
         totalBytes+=bytes;
         bytesToRead-=bytes;
      }

      // Signal buffer ready for writing
      buffers[bufIndex].bytes=totalBytes;
      buffers[bufIndex].NeedsWriteToDisk=1;
      buffers[bufIndex].NeedsWriteToFifo=1;

      // Move to next buffer
      bufIndex=(bufIndex+1)%NBUFS;
   }
}