/* 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 <string.h>

#include "../headers/system_config.h"

#if defined(PLATFORM_APERIOS) && !defined(PLATFORM_APERIOS_SDK)
#include <OFS/OFS.h>
#else
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#endif

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

#define SSIZE_MAX (65535)


#if defined(PLATFORM_LINUX) || defined(PLATFORM_APERIOS_SDK)
namespace BFS {
  int open(const char *filename, int mode)
  {
    return(::open(filename,mode));
  }

  size_t read(int fd, void *ptr, size_t size)
  {
    return(::read(fd,ptr,size));
  }

  size_t write(int fd, void *ptr, size_t size)
  {
    return(::write(fd,ptr,size));
  }

  off_t lseek(int fd,off_t offset, int whence)
  {
    return(::lseek(fd,offset,whence));
  }

  int close(int fd)
  {
    return(::close(fd));
  }

  const char *LastError()
  {
    return("");
  }
}
#else
namespace BFS {
  int open(const char *filename, int mode)
  {
    return(OFS::open(filename,mode));
  }

  size_t read(int fd, void *ptr, size_t size)
  {
    return(OFS::read(fd,ptr,size));
  }

  size_t write(int fd, void *ptr, size_t size)
  {
    return(OFS::write(fd,ptr,size));
  }

  off_t lseek(int fd,off_t offset, int whence)
  {
    return(OFS::lseek(fd,offset,whence));
  }

  int close(int fd)
  {
    return(OFS::close(fd));
  }

  const char *LastError()
  {
    return(OFS::LastError());
  }
}
#endif

namespace HFS {
  FILE *fopen(const char *filename, const char *mode) {
    FILE *file;
    bool seek_to_end=false;
    int flags;

    switch(*mode) {
    case 'r':
      if(*(mode+1) == '+')
        flags = BFS_O_RDWR;
      else
        flags = BFS_O_RDONLY;
      break;
    case 'w':
      if(*(mode+1) == '+')
        flags = BFS_O_RDWR | BFS_O_TRUNC | BFS_O_CREAT;
      else
        flags = BFS_O_WRONLY | BFS_O_TRUNC | BFS_O_CREAT;
      break;
    case 'a':
      seek_to_end=true;
      if(*(mode+1) == '+')
        flags = BFS_O_RDWR | BFS_O_CREAT | BFS_O_APPEND;
      else
        flags = BFS_O_WRONLY | BFS_O_CREAT | BFS_O_APPEND;
      break;
    default:
      return (FILE *)0UL;
    }

    int fd;
    fd = BFS::open(filename,flags);

    if(fd != -1) {
      file=new FILE;
      file->fd=fd;
      
      if(seek_to_end)
        BFS::lseek(file->fd,0,BFS_SEEK_END);
      
      file->valid_size=0;
      file->eof=false;

      return file;
    }
    else {
      // cout << "error opening file.";
      // pprintf(TextOutputStream,"error opening file '%s' with mode '%s', error was:'%s'\n",filename,mode,BFS::LastError());
      
      return (FILE *)0UL;
    }
  }

  size_t fwrite(const void *ptr, size_t size, size_t num_elem, FILE *file) {
    int copy_size=size*num_elem;
    uchar *in_ptr;

    in_ptr = (uchar *)ptr;

    while(copy_size > 0) {
      int write_count;

      write_count=BFS::write(file->fd,in_ptr,copy_size);
      if(write_count == -1) {
	// cout << "error during fwrite";
        // pprintf(TextOutputStream,"error during fwrite, error is '%s'\n",BFS::LastError());
        return num_elem - copy_size/size;
      }
      if(write_count==0) {
        // pprintf(TextOutputStream,"couldn't write any data during fwrite\n");
        return num_elem - copy_size/size;
      }
      copy_size -= write_count;
      in_ptr += write_count;
    }

    return num_elem;
  }

  size_t fread(void *ptr, size_t size, size_t num_elem, FILE *file) {
    int copy_size=size*num_elem;
    uchar *out_ptr;

    out_ptr = (uchar *)ptr;

    if(file->valid_size > 0) {
      int size_to_copy;

      size_to_copy = (file->valid_size < copy_size) ? file->valid_size : copy_size;

      memcpy(out_ptr, file->buf, size_to_copy);
      if(size_to_copy != file->valid_size) {
        memmove(file->buf,file->buf+size_to_copy,file->valid_size-size_to_copy);
        file->valid_size -= size_to_copy;
      }

      copy_size -= size_to_copy;
      out_ptr += size_to_copy;
    }

    while(copy_size > 0) {
      int read_count;

      read_count=BFS::read(file->fd,out_ptr,copy_size);
      if(read_count == -1) {
	// cout << "error during fread.";
        // pprintf(TextOutputStream,"error during fread, error is '%s'\n",BFS::LastError());
        return num_elem - copy_size/size;
      }
      if(read_count==0) {
	// cout << "premature eof.";
        // pprintf(TextOutputStream,"premature eof during fread\n");
        file->eof=true;
        return num_elem - copy_size/size;
      }
      copy_size -= read_count;
      out_ptr += read_count;
    }

    return num_elem;
  }

  char *fgets(char *buf, int buf_size, FILE *file) {
    char *buf_ptr=buf;
    bool again;
    bool copied_data;

    copied_data=false;

    do {
      again=false;

      // read in from file until our buffer is full, our buffer is bigger than in buffer, or eof
      while(file->valid_size<buf_size && !file->eof && file->valid_size<HFS_FILE_BUF_SIZE-1) {
        int read_bytes;
        int read_req_size;
        
        read_req_size=HFS_FILE_BUF_SIZE-file->valid_size;
        if(read_req_size > SSIZE_MAX)
          read_req_size = SSIZE_MAX;
        read_bytes=BFS::read(file->fd,&file->buf[file->valid_size],read_req_size);
        if(read_bytes == -1) {
          // cout << "error reading file";
	  // pprintf(TextOutputStream,"error reading file, error is '%s'\n",BFS::LastError());
          *buf_ptr=0;
          return buf;
        }
        file->valid_size+=read_bytes;
        if(read_bytes==0)
          file->eof=true;
      }
      
      char *newline_pos;
      file->buf[file->valid_size]=0; // NULL terminate string so we can use strchr
      newline_pos=strchr(file->buf,'\n');
      int chars_to_copy;
      if(newline_pos) { 
        // got newline
        chars_to_copy = newline_pos+1 - file->buf;
        if(chars_to_copy > buf_size-1)
          chars_to_copy = buf_size-1;
      }
      else {
        // ran out of data
        chars_to_copy = file->valid_size;
        if(chars_to_copy > buf_size-1)
          chars_to_copy = buf_size-1;
      }
      if(chars_to_copy > 0) {
        strncpy(buf_ptr, file->buf, chars_to_copy);
        memmove(file->buf, file->buf+chars_to_copy, file->valid_size-chars_to_copy);
        file->valid_size -= chars_to_copy;
        copied_data = true;
      }
      buf_ptr[chars_to_copy]=0;

      if(file->valid_size==HFS_FILE_BUF_SIZE) {
        buf_ptr += chars_to_copy;
        again=true;
      }
    } while(again);

    if(copied_data)
      return buf;
    else
      return NULL;
  }

  int fclose(FILE *file) {
    int ret;

    ret=BFS::close(file->fd);

    delete file;

    return ret;
  }

#if 0
  FILE *fopen(const char *filename, const char *mode) {
    return ::fopen(filename,mode);
  }

  size_t fread(void *ptr, size_t size, size_t num_elem, FILE *file) {
    return ::fread(ptr,size,num_elem,file);
  }

  char *fgets(char *buf, int buf_size, FILE *file) {
    return ::fgets(buf,buf_size,file);
  }

  int fclose(FILE *file) {
    return ::fclose(file);
  }
#endif
}
