/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>

#include <deque>

#include "../../agent/headers/CircBufPacket.h"
#include "../../agent/headers/WaveLAN.h"

#include "WLDecoder.h"

WLDecoder::WLDecoder()
{
  packet.contentType = RobotDataPacket::EMPTY;
  packet.data=new uchar[MaxPacketSize];
  packet.length=MaxPacketSize;
  packetCallback = NULL;
  userData       = NULL;
  robotId = -1;

  reset();
}

void
WLDecoder::setCallback(void *user_data,PacketCallback callback) {
  packetCallback = callback;
  userData       = user_data;
}

void
WLDecoder::setRobotId(int robot_id) {
  robotId = robot_id;
}

void
WLDecoder::reset() {
  inPacket = false;
  readHeader = false;
  readPos = 0;
  rawPacketStart = 0;
}

bool
WLDecoder::processInput(int port) {
  int read_count=0;

  while((read_count=read(port,&readBuf[readPos],BufSize-readPos))>0) {
    //printf("fd %d read_count %d\n",port,read_count);
    //dumpBinary(&readBuf[readPos],read_count);

    readPos += read_count;

    //printf("buffer is now:\n");
    //dumpBinary(readBuf,readPos);

    bool packet_found;
    bool more_to_process;
    if(!inPacket)
      rawPacketStart=readPos-read_count;
    do {
      more_to_process=false;
      if(inPacket) {
        if(!readHeader) {
          if(readPos-rawPacketStart > WLPacketHeaderLength+1) {
            if(readBuf[rawPacketStart+1]==(uchar)'\x80') {
              printText(&readBuf[0],rawPacketStart);
              memmove(&readBuf[0],&readBuf[rawPacketStart],BufSize-rawPacketStart);
              readPos -= rawPacketStart;
              rawPacketStart = 0;
              continue;
            }
            //printf("raw_head:");
            //dumpBinary(&readBuf[rawPacketStart],WLPacketHeaderLength+1);
            rawPacketStart++;
            packet.dataType = readBuf[rawPacketStart++];

            binary = (packet.dataType!=(uchar)'\xBE'); // \xBE is text lead character
            packet.contentType = 
              binary ?
              RobotDataPacket::BINARY :
              RobotDataPacket::TEXT;

            memcpy(&packet.length,&readBuf[rawPacketStart],4);
            rawPacketStart += 4;

            if(packet.length >= (ulong)MaxPacketLength) {
              inPacket = false;
              readHeader = false;
              memmove(&readBuf[0],&readBuf[rawPacketStart],BufSize-rawPacketStart);
              readPos -= rawPacketStart;
              rawPacketStart = 0;
              more_to_process = (0 != readPos);
              continue;
            }
            
            memset(packet.data,'\xEE',packet.length); // helps with debugging
            destData = packet.data;
            sourceLengthToRead = packet.length;
            //printf("psltr %d\n",sourceLengthToRead);
            memcpy(&packet.timestamp,&readBuf[rawPacketStart],sizeof(packet.timestamp));
            rawPacketStart += 4;
            readHeader=true;
            memmove(&readBuf[0],&readBuf[rawPacketStart],BufSize-rawPacketStart);
            readPos -= rawPacketStart;
            rawPacketStart = 0;
            more_to_process = (0 != readPos);
          }
        } else { /* readHeader */
          int data_available;
          int raw_data_start;
          raw_data_start = 0;
          data_available = readPos - raw_data_start;
          while(data_available >=8 && sourceLengthToRead >= 8) {
            for(int i=0; i<8; i++)
              destData[i]=readBuf[raw_data_start++];
            destData += 8;
            data_available -= 8;
            sourceLengthToRead -= 8;
          }
          //printf("destData is now:\n");
          //dumpBinary(packet.data,packet.length);
          //fprintf(stderr,"sltr %d\n",sourceLengthToRead);
          if(data_available >= sourceLengthToRead) {
            assert(sourceLengthToRead < 8);
            for(int i=0; i<sourceLengthToRead; i++)
              destData[i]=readBuf[raw_data_start++];
            inPacket = false;
            readHeader = false;
            // process packet here
            //printf("processing wavelan packet of type %x\n",packet.dataType);
            //dumpBinary(packet.data,packet.length);
            PacketCallback cb=packetCallback;
            if(robotId!=-1 && cb)
              (*cb)(robotId,userData,&packet);
          }
          if(0 != raw_data_start) {
            memmove(&readBuf[0],&readBuf[raw_data_start],BufSize-raw_data_start);
            readPos -= raw_data_start;
          }
          rawPacketStart = 0;
          if(!inPacket)
            more_to_process = true;
        }
        //fprintf(stderr,"sltr %d\n",sourceLengthToRead);
      } else { /* !inPacket */
        packet_found=false;
        while(rawPacketStart < readPos) {
          packet_found = (readBuf[rawPacketStart]==(uchar)'\x80');
          if(packet_found)
            break;
          rawPacketStart++;
        }
        read_count = 0;
        if(packet_found) {
          inPacket=true;
          readHeader=false;
          //fprintf(stderr,"found packet\n");
        }
        
        if(0!=rawPacketStart) {
          printText(&readBuf[0],rawPacketStart);
          memmove(&readBuf[0],&readBuf[rawPacketStart],BufSize-rawPacketStart);
          readPos -= rawPacketStart;
          rawPacketStart = 0;
        }
        more_to_process = (0 != readPos);
      }
    } while(more_to_process);
  }

  if(read_count==-1 && (errno==EBADF || errno==EINVAL)) {
    return false;
  }

  // indicates disconnected socket since otherwise would get a EAGAIN
  if(read_count==0) {
    static bool sent_it=false;
    if(!sent_it) {
      printf("rc0 on port (fd=%d)",port);
      sent_it = true;
    }
    //printf("read count was %d (errno=%d), stopping reads on port (fd=%d)\n",read_count,errno,port);

    return false;
  }

  return true;
}

void
WLDecoder::printText(uchar *start, int count) {
  uchar *end;

  end = start + count;
  while(start!=end) {
    uchar ch_in=*start;
    start++;
    if(isPrintable(ch_in)) {
      putchar(ch_in);
    }
    else {
      putchar('x');
      unsigned int in=(unsigned char)ch_in;
      unsigned int high=in>>4;
      unsigned int low=in & 0xF;
      putchar((high > 9) ? (high - 10 + 'a') : (high + '0'));
      putchar((low  > 9) ? (low  - 10 + 'a') : (low  + '0'));
    }
  }
}

void
WLDecoder::dumpBinary(uchar *data, int length) {
  int char_cnt = 0;
  bool end_row;
  uchar *asc_data;

  end_row=false;
  asc_data = data;
  while(length > 0) {
    putchar('x');
    unsigned int in=*data;
    unsigned int high=in>>4;
    unsigned int low=in & 0xF;
    putchar((high > 9) ? (high - 10 + 'a') : (high + '0'));
    putchar((low  > 9) ? (low  - 10 + 'a') : (low  + '0'));

    data++;
    length--;

    char_cnt++;
    end_row = end_row || (char_cnt == 30) || (length==0);
    if(end_row) {
      printf(" | ");

      while(asc_data!=data) {
        uchar ch_in=*asc_data;
        asc_data++;
        if(isprint(ch_in))
          putchar(ch_in);
        else
          putchar('.');
      }

      printf("\n");

      end_row = false;
      char_cnt  = 0;
    }
  }
}

bool
WLDecoder::isPrintable(char c) {
  return (isprint(c) || c=='\r' || c=='\n' || c=='\x1B' || c=='\x09');
}

WLDemuxer::WLDemuxer() {
  for(int robot_idx=0; robot_idx<MaxRobotId+1; robot_idx++) {
    activeDecoders[robot_idx] = false;
    decoders      [robot_idx] = NULL;
    readFDs       [robot_idx] = -1;
    writeFDs      [robot_idx] = -1;
  }
  packetCallback = NULL;
  userData = NULL;

  bufIdx = 0;
  readingHeader = true;
  dataToRead = 0;
  curRobotId = -1;
}

WLDemuxer::~WLDemuxer() {
  for(int robot_idx=0; robot_idx<MaxRobotId+1; robot_idx++) {
    if(decoders[robot_idx]!=NULL)
      delete decoders[robot_idx];
    if(activeDecoders[robot_idx]) {
      close(readFDs [robot_idx]);
      close(writeFDs[robot_idx]);
    }
  }
}

void
WLDemuxer::setCallback(void *user_data, PacketCallback callback) {
  packetCallback = callback;
  userData = user_data;
}

void
WLDemuxer::createDecoder(int robot_idx) {
  int fds[2];

  decoders[robot_idx] = new WLDecoder();
  decoders[robot_idx]->setCallback(userData,packetCallback);
  decoders[robot_idx]->setRobotId(robot_idx);

  if(pipe(fds)!=0)
    return;

  readFDs [robot_idx] = fds[0];
  writeFDs[robot_idx] = fds[1];

  if(fcntl(readFDs[robot_idx],F_SETFL,O_NONBLOCK)==-1) {
    perror("problem setting read socket to nonblocking");
    close(readFDs [robot_idx]); readFDs [robot_idx]=-1;
    close(writeFDs[robot_idx]); writeFDs[robot_idx]=-1;
    return;
  }

  if(fcntl(writeFDs[robot_idx],F_SETFL,O_NONBLOCK)==-1) {
    perror("problem setting write socket to nonblocking");
    close(readFDs [robot_idx]); readFDs [robot_idx]=-1;
    close(writeFDs[robot_idx]); writeFDs[robot_idx]=-1;
    return;
  }

  activeDecoders[robot_idx] = true;
}

bool
WLDemuxer::processInput(int port) {
  int read_count;

  while((read_count=read(port,&buf[bufIdx],sizeof(buf)-bufIdx))>0) {
    bool more_to_process;

    bufIdx += read_count;
    do {
      more_to_process = false;

      if(readingHeader) {
        if(bufIdx >= (int)sizeof(NetMsgHeader)) {
          NetMsgHeader msg_header;
          memcpy(&msg_header,buf,sizeof(NetMsgHeader));

          dataToRead = msg_header.totalLength - sizeof(NetMsgHeader);
          curRobotId = msg_header.senderID;

          if(0<=curRobotId && curRobotId<=MaxRobotId) {
            if(!activeDecoders[curRobotId])
              createDecoder(curRobotId);
          }

          memmove(&buf[0],&buf[sizeof(NetMsgHeader)],sizeof(buf)-sizeof(NetMsgHeader));
          bufIdx -= sizeof(NetMsgHeader);

          readingHeader = false;

          more_to_process = true;
        }
      } else {
        if(dataToRead == 0) {
          readingHeader = true;
        } else {
          while(bufIdx>0 && dataToRead!=0) {
            int write_count = min(bufIdx,dataToRead);
            if(0<=curRobotId && curRobotId<=MaxRobotId && activeDecoders[curRobotId])
              write_count = write(writeFDs[curRobotId],buf,write_count);
            if(write_count!=-1) {              
              //printf("dataToRead was %d wrote %d to fd %d\n",dataToRead,write_count,writeFDs[curRobotId]);
              //WLDecoder::dumpBinary(buf,write_count);
              memmove(&buf[0],&buf[write_count],sizeof(buf)-write_count);
              bufIdx -= write_count;
              dataToRead -= write_count;
            }
            if(0<=curRobotId && curRobotId<=MaxRobotId && activeDecoders[curRobotId])
              decoders[curRobotId]->processInput(readFDs[curRobotId]);
          }

          more_to_process = (bufIdx > 0);
        }
      }
    } while(more_to_process);
  }

  if(read_count==-1 && (errno==EBADF || errno==EINVAL)) {
    return false;
  }

  // indicates disconnected socket since otherwise would get a EAGAIN
  if(read_count==0) {
    printf("read count was %d (errno=%d), stopping reads on socket (fd=%d)\n",read_count,errno,port);

    return false;
  }

  return true;
}