/* 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 <stdio.h>
#include <deque>
#include <unistd.h>

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

#include "SPOutDecoder.h"

SPOutDecoder::SPOutDecoder()
  : inPacket(false),
    readHeader(false),
    packetCallback(NULL)
{
  packet.contentType = RobotDataPacket::EMPTY;
  packet.data=new uchar[MaxPacketSize];
  packet.length=MaxPacketSize;
  rawPacketStart = 0;
  readPos = 0;
  userData = NULL;
  robotId=0;
}

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

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

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

#ifndef MAX
#define MAX(x,y) ((x > y) ? x : y)
#endif

void
SPOutDecoder::processInput(serial *port) {
  int read_count=0;

  static const int BufSize=MAX(SPOutPacketHeaderLength,16);
  static uchar read_buf[BufSize];
  //  static uchar data_buf[BufSize];

  while((read_count=port->read(&read_buf[readPos],BufSize-readPos))>0) {
    for(int char_idx=0; char_idx<read_count; char_idx++)
      if(read_buf[char_idx+readPos]==(uchar)'\x81')
        read_buf[char_idx+readPos]='\x0A';
    readPos += read_count;

    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 > SPOutPacketHeaderLength+1) {
            //printf("raw_head:");
            //printRawBinaryScreen(packetStart,packetStart+SPOutPacketHeaderLength+1);
            rawPacketStart++;
            packet.dataType = read_buf[rawPacketStart++];

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

            memset(buf_7bit,0,sizeof(buf_7bit));
            buf_7bit[0] = read_buf[rawPacketStart++];
            buf_7bit[1] = read_buf[rawPacketStart++];
            decode7bits(buf_8bit,buf_7bit);
            memcpy(&packet.length,buf_8bit,sizeof(packet.length));
            memset(packet.data,'\xEE',packet.length); // helps with debugging
            destData = packet.data;
            sourceLengthToRead = 
              (binary ?
              (8*packet.length+6)/7 :
              packet.length);
            //fprintf(stderr,"psltr %d\n",sourceLengthToRead);
            memset(buf_7bit,0,sizeof(buf_7bit));
            buf_7bit[0] = read_buf[rawPacketStart++];
            buf_7bit[1] = read_buf[rawPacketStart++];
            buf_7bit[2] = read_buf[rawPacketStart++];
            buf_7bit[3] = read_buf[rawPacketStart++];
            buf_7bit[4] = read_buf[rawPacketStart++];
            decode7bits(buf_8bit,buf_7bit);
            memcpy(&packet.timestamp,buf_8bit,sizeof(packet.timestamp));
            //fprintf(stderr,"ts %lx b7=[%x,%x,%x,%x,%x,%x,%x,%x] b8=[%x,%x,%x,%x,%x,%x,%x]\n",
            //        packet.timestamp,
            //        buf_7bit[0],buf_7bit[1],buf_7bit[2],buf_7bit[3],buf_7bit[4],buf_7bit[5],buf_7bit[6],buf_7bit[7],
            //        buf_8bit[0],buf_8bit[1],buf_8bit[2],buf_8bit[3],buf_8bit[4],buf_8bit[5],buf_8bit[6]);
            readHeader=true;
            memmove(&read_buf[0],&read_buf[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) {
            if(binary) {
              for(int i=0; i<8; i++)
                buf_7bit[i]=read_buf[raw_data_start++];
              decode7bits(buf_8bit,buf_7bit);
              memcpy(destData,buf_8bit,7);
              destData += 7;
              data_available -= 8;
              sourceLengthToRead -= 8;
              //printf("conv b7=[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]->b8=[%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n",
              //       buf_7bit[0],buf_7bit[1],buf_7bit[2],buf_7bit[3],buf_7bit[4],buf_7bit[5],buf_7bit[6],buf_7bit[7],
              //       buf_8bit[0],buf_8bit[1],buf_8bit[2],buf_8bit[3],buf_8bit[4],buf_8bit[5],buf_8bit[6]);
            } else {
              for(int i=0; i<8; i++)
                destData[i]=read_buf[raw_data_start++];
              destData += 8;
              data_available -= 8;
              sourceLengthToRead -= 8;
            }
          }
          //fprintf(stderr,"sltr %d\n",sourceLengthToRead);
          if(data_available >= sourceLengthToRead) {
            assert(sourceLengthToRead < 8);
            if(binary) {
              memset(buf_7bit,0,sizeof(buf_7bit));
              for(int i=0; i<sourceLengthToRead; i++)
                buf_7bit[i]=read_buf[raw_data_start++];
              decode7bits(buf_8bit,buf_7bit);
              memcpy(destData,buf_8bit,packet.length-(destData-packet.data));
            } else {
              for(int i=0; i<sourceLengthToRead; i++)
                destData[i]=read_buf[raw_data_start++];
            }
            inPacket = false;
            readHeader = false;
            // process packet here
            //printf("processing packet of type %x\n",packet.dataType);
            if(packetCallback)
              (*packetCallback)(robotId,userData,&packet);
          }
          if(0 != raw_data_start) {
            memmove(&read_buf[0],&read_buf[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 = (read_buf[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(&read_buf[0],rawPacketStart);
          memmove(&read_buf[0],&read_buf[rawPacketStart],BufSize-rawPacketStart);
          readPos -= rawPacketStart;
          rawPacketStart = 0;
        }
        more_to_process = (0 != readPos);
      }
    } while(more_to_process);
  }
}

void
SPOutDecoder::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
SPOutDecoder::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
SPOutDecoder::isPrintable(char c) {
  return (isprint(c) || c=='\r' || c=='\n' || c=='\x1B' || c=='\x09');
}