// ----------------------------------------------------------------------------
//
// Copyright (C) Columbia University, 1994. All Rights Reserved.
// Sameer A. Nene, Shree K. Nayar, Hiroshi Murase
//
// See file LICENSE for details of software license agreement.
//
// ----------------------------------------------------------------------------
//
// vector.C
//
// Author:               Sameer Nene
// Date:                 05/23/94
// Version:              1.0
// Modification History:
//   07/02/94: Added getText() and putText() to load/store vector in text form
//
// Bugs:
//   VectAryIter allows advances beyond last element and coredumps. There is no
//   last element check for Vector::element also and consequently suffers from
//   the same problem.
//
//   There are no out of memory checks.
//
// Classes:
//   Vector
//   VectAry
//   VectAryIter
//
// Notes:
//   This module contains implementation of classes declared in vector.h
//   
// ----------------------------------------------------------------------------

#include <ctype.h>
#include <iostream.h>
#include <math.h>
#include <memory.h>
#include <stdio.h>
#include "errorscope.h"
#include "persistent.h"
#include "registry.h"
#include "vector.h"

#ifndef SEEK_SET
#define SEEK_SET 0
#endif

void* Vector::d_protocol = (void*)(&Vector::d_protocol);
char* Vector::d_name_p = "Vector";
Registrar Vector::d_registrar(Vector::d_name_p, &(Vector::createFunc));

void* VectAry::d_protocol = (void*)(&VectAry::d_protocol);
char* VectAry::d_name_p = "VectAry";
Registrar VectAry::d_registrar(VectAry::d_name_p, &(VectAry::createFunc));

Persistent* Vector::createFunc()
{
  return new Vector;
}

int Vector::hasProtocol(void* protocol) const
{
  return protocol == d_protocol;
}

Vector::Vector() : d_data_p(0), d_size(0)
{
}

Vector::Vector(int size) : d_data_p(new double[size]), d_size(size)
{
}

Vector::Vector(const Vector &v) : d_data_p(new double[v.d_size]),
d_size(v.d_size)
{
  memcpy(d_data_p, v.d_data_p, d_size * sizeof(double));
}

Vector::~Vector()
{
  delete[] d_data_p;
}

Vector& Vector::operator=(const Vector &v)
{
  if(&v != this) {
    if(d_size != v.d_size) {
      delete[] d_data_p;
      d_data_p = new double[d_size = v.d_size];
    }
    memcpy(d_data_p, v.d_data_p, d_size * sizeof(double));
  }
  return *this;
}

void Vector::element(int i, double v) {
  d_data_p[i] = v;
}

Vector& Vector::operator+=(const Vector &v)
{
  for(int i = 0; i < d_size; ++i)
    d_data_p[i] += v.d_data_p[i];
  return *this;
}

Vector& Vector::operator-=(const Vector &v)
{
  for(int i = 0; i < d_size; ++i)
    d_data_p[i] -= v.d_data_p[i];
  return *this;
}

Vector& Vector::operator*=(double v)
{
  for(int i = 0; i < d_size; d_data_p[i++] *= v);
  return *this;
}

Vector& Vector::operator/=(double v)
{
  int i;
  double r;
  for(i = 0, r = 1. / v; i < d_size; d_data_p[i++] *= r);
  return *this;
}

void Vector::length(int i)
{
  if(d_size != i) {
    delete[] d_data_p;
    d_data_p = new double[d_size = i];
  }
}

Vector* Vector::safeCast(Persistent *p)
{
  return p -> hasProtocol(d_protocol) ? (Vector*)p : 0;
}
  
double Vector::element(int i) const
{
  return d_data_p[i];
}

int Vector::length() const
{
  return d_size;
}

double Vector::operator()() const
{
  double t = *d_data_p, v = t * t;
  for(int i = 1; i < d_size; ++i) {
    t = d_data_p[i];
    v += t * t;
  }
  return sqrt(v);
}

double Vector::distance(const Vector &v) const
{
  double t = *d_data_p - *(v.d_data_p), sum = t * t;
  for(int i = 1; i < d_size; ++i) {
    t = d_data_p[i] - v.d_data_p[i];
    sum += t * t;
  }
  return sum;
}

Vector Vector::operator+(const Vector &v) const
{
  Vector t(d_size);
  for(int i = 0; i < d_size; ++i)
    t.d_data_p[i] = d_data_p[i] + v.d_data_p[i];
  return t;
}

Vector Vector::operator-(const Vector &v) const
{
  Vector t(d_size);
  for(int i = 0; i < d_size; ++i)
    t.d_data_p[i] = d_data_p[i] - v.d_data_p[i];
  return t;
}

Vector Vector::operator*(double v) const
{
  Vector t(d_size);
  for(int i = 0; i < d_size; ++i)
    t.d_data_p[i] = d_data_p[i] * v;
  return t;
}

double Vector::operator*(const Vector &v) const
{
  double t = *d_data_p * *(v.d_data_p);
  for(int i = 1; i < d_size; ++i)
    t += d_data_p[i] * v.d_data_p[i];
  return t;
}

Vector Vector::operator/(double v) const
{
  int i;
  double r;
  Vector t(d_size);
  for(i = 0, r = 1. / v; i < d_size; ++i)
    t.d_data_p[i] = d_data_p[i] * r;
  return t;
}

Vector operator*(double d, const Vector &v)
{
  Vector t(v.d_size);
  for(int i = 0; i < v.d_size; ++i)
    t.d_data_p[i] = v.d_data_p[i] * d;
  return t;
}

ostream& operator<<(ostream &o, const Vector &v)
{
  o << '[';
  for(int i = 0; i < v.d_size; ++i)
    o << v.d_data_p[i] << ' ';
  o << ']' << ' ';
  return o;
}

ErrorScope::Error Vector::get(FILE *handle)
{
  int size;

  if(fread((char *)&size, sizeof(int), 1, handle) != 1)
    return FILE_READ_ERROR;

  if(d_size != size) {
    delete[] d_data_p;
    d_data_p = new double[d_size = size];
  }

  if(fread((char *)d_data_p, sizeof(double), size, handle) != size)
    return FILE_READ_ERROR;

  return OK;
}

ErrorScope::Error Vector::getText(const char *fileName)
{
  char *p, buf[256];
  int c, l, state = 0;
  long dataPos;
  double d;
  FILE *file;

  if((file = fopen(fileName, "r")) == 0)
    return FILE_OPEN_ERROR;

  do {
    if((c = fgetc(file)) == EOF) {
      fclose(file);
      return FILE_READ_ERROR;
    }

    switch(state) {
    case 0:
      if(isspace(c))
	break;
      if(c == '#')
	state = 1;
      else if(isdigit(c) || c == '-' || c == '.') {
	ungetc(c, file);
	state = 2;
      }
      else
	state = 3;
      break;
    case 1:
      if(c == '\n')
	state = 0;
    }
  }
  while(state < 2);

  if(state == 3) {
    fclose(file);
    return INCORRECT_FORMAT;
  }

  dataPos = ftell(file);
  l = 0;
  while(1) {
    if((p = fgets(buf, 256, file)) == 0)
      break;
    for(state = 0; *p != '\0'; ++p)
      if(!isspace(*p)) {
	state = 1;
	break;
      }
    if(state == 0)
      break;
    if(sscanf(buf, "%le", &d) != 1) {
      fclose(file);
      return INCORRECT_FORMAT;
    }
    ++l;
  }

  if(l == 0) {
    fclose(file);
    return INCORRECT_FORMAT;
  }

  if(fseek(file, dataPos, SEEK_SET) == -1) {
    fclose(file);
    return FILE_READ_ERROR;
  }
  
  if(l != d_size) {
    delete[] d_data_p;
    d_data_p = new double[d_size = l];
  }
  
  for(c = 0; c < l; ++c)
    if(fscanf(file, "%le\n", &(d_data_p[c])) != 1) {
      fclose(file);
      return FILE_READ_ERROR;
    }

  fclose(file);
  return OK;
}

ErrorScope::Error Vector::putText(const char *fileName) const
{
  int i;
  FILE *file;

  if((file = fopen(fileName, "w")) == NULL)
    return FILE_OPEN_ERROR;

  if(fprintf(file, "# Vector\n# Creator: SLAM V1.0 Rev: 07/02/94\n\n") == EOF) {
    fclose(file);
    return FILE_WRITE_ERROR;
  }
  
  for(i = 0; i < d_size; ++i)
    if(fprintf(file, "%1.16le\n", d_data_p[i]) == EOF) {
      fclose(file);
      return FILE_WRITE_ERROR;
    }

  return fclose(file) ? FILE_CLOSE_ERROR : OK;
}
    
ErrorScope::Error Vector::put(FILE *handle) const
{
  if(fwrite((char *)&d_size, sizeof(int), 1, handle) != 1)
    return FILE_WRITE_ERROR;
  
  if(fwrite((char *)d_data_p, sizeof(double), d_size, handle) != d_size)
    return FILE_WRITE_ERROR;

  return OK;
}

const char* Vector::name() const
{
  return d_name_p;
}

Persistent* VectAry::createFunc()
{
  return new VectAry;
}

int VectAry::hasProtocol(void* protocol) const
{
  return protocol == d_protocol;
}

VectAry::VectAry() : d_size(0), d_data_p(0)
{
}

VectAry::VectAry(int size) : d_data_p(new Vector* [size]), d_size(size)
{
  for(int i = 0; i < size; d_data_p[i++] = new Vector);
}

VectAry::~VectAry()
{
  for(int i = 0; i < d_size; delete d_data_p[i++]);
  delete[] d_data_p;
}

int VectAry::getSize() const
{
  return d_size;
}

Vector& VectAry::operator[](int i) const
{
  return *(d_data_p[i]);
}

ErrorScope::Error VectAry::get(FILE *handle)
{
  int i, size;
  Error error;
  
  if(fread((char *)&size, sizeof(int), 1, handle) != 1)
    return FILE_READ_ERROR;

  if(d_size != size) {
    for(i = 0; i < d_size; delete d_data_p[i++]);
    delete[] d_data_p;
    d_data_p = new Vector*[d_size = size];
    for(int i = 0; i < size; d_data_p[i++] = new Vector);
  }
  
  for(i = 0; i < size; ++i)
    if((error = d_data_p[i] -> get(handle)) != OK)
      return error;
  
  return OK;
}

ErrorScope::Error VectAry::getText(const char *fileName)
{
  char *p, buf[256];
  int c, l, m, n, blankLines, prevState, state = 0;
  long dataPos;
  double d;
  FILE *file;

  if((file = fopen(fileName, "r")) == 0)
    return FILE_OPEN_ERROR;

  do {
    if((c = fgetc(file)) == EOF) {
      fclose(file);
      return FILE_READ_ERROR;
    }

    switch(state) {
    case 0:
      if(isspace(c))
	break;
      if(c == '#')
	state = 1;
      else if(isdigit(c) || c == '-' || c == '.') {
	ungetc(c, file);
	state = 2;
      }
      else
	state = 3;
      break;
    case 1:
      if(c == '\n')
	state = 0;
    }
  }
  while(state < 2);

  if(state == 3) {
    fclose(file);
    return INCORRECT_FORMAT;
  }

  dataPos = ftell(file);
  l = prevState = 0;
  while(1) {
    if((p = fgets(buf, 256, file)) == 0)
      break;
    for(state = 0; *p != '\0'; ++p)
      if(!isspace(*p)) {
	state = 1;
	break;
      }
    if(state == 1)
      if(sscanf(buf, "%le", &d) != 1) {
	fclose(file);
	return INCORRECT_FORMAT;
      }
      else
	if(prevState == 0)
	  ++l;
    prevState = state;
  }

  if(l == 0) {
    fclose(file);
    return INCORRECT_FORMAT;
  }

  if(fseek(file, dataPos, SEEK_SET) == -1) {
    fclose(file);
    return FILE_READ_ERROR;
  }
  
  if(l != d_size) {
    delete[] d_data_p;
    d_data_p = new Vector*[d_size = l];
  }
  for(c = 0; c < l; d_data_p[c++] = 0);
  
  for(c = 0; c < l; ++c) {
    dataPos = ftell(file);
    m = blankLines = 0;
    prevState = 1;
    while(1) {
      if((p = fgets(buf, 256, file)) != 0) {
	for(state = 0; *p != '\0'; ++p)
	  if(!isspace(*p)) {
	    state = 1;
	    break;
	  }
      }
      else {
	state = 1;
	prevState = blankLines = 0;
      }
      if(state == 1) {
	if(prevState == 0) {
	  d_data_p[c] = new Vector(m);
	  if(fseek(file, dataPos, SEEK_SET) == -1) {
	    fclose(file);
	    return FILE_READ_ERROR;
	  }
	  for(n = 0; n < m; ++n) {
	    if(fscanf(file, "%le", &d) != 1) {
	      fclose(file);
	      return FILE_READ_ERROR;
	    }
	    d_data_p[c] -> element(n, d);
	  }
	  
	  // Take care of last \n
	  if(fgets(buf, 256, file) == 0) {
	    fclose(file);
	    return FILE_READ_ERROR;
	  }
	  
	  for(n = 0; n < blankLines; ++n)
	    if(fgets(buf, 256, file) == 0) {
	      fclose(file);
	      return FILE_READ_ERROR;
	    }
	  break;
	}
	++m;
      }
      else
	++blankLines;
      prevState = state;
    }
  }

  fclose(file);
  return OK;
}

VectAry* VectAry::safeCast(Persistent *p)
{
  return p -> hasProtocol(d_protocol) ? (VectAry*)p : 0;
}

ErrorScope::Error VectAry::put(FILE *handle) const
{
  Error error;

  if(fwrite((char *)&d_size, sizeof(int), 1, handle) != 1)
    return FILE_WRITE_ERROR;
  
  for(int i = 0; i < d_size; ++i)
    if((error = d_data_p[i] -> put(handle)) != OK)
      return error;

  return OK;
}

ErrorScope::Error VectAry::putText(const char *fileName) const
{
  int i, j, k;
  FILE *file;

  if((file = fopen(fileName, "w")) == 0)
    return FILE_OPEN_ERROR;

  if(fprintf(file, "# VectAry\n# Creator: SLAM V1.0 Rev: 07/02/94\n\n") == EOF) {
    fclose(file);
    return FILE_WRITE_ERROR;
  }

  for(i = 0; i < d_size; ++i) {
    Vector &v = *(d_data_p[i]);
    for(j = 0, k = v.length(); j < k; ++j)
      if(fprintf(file, "%1.16le\n", v.element(j)) == EOF) {
	fclose(file);
	return FILE_WRITE_ERROR;
      }
    if(fprintf(file, "\n") == EOF) {
      fclose(file);
      return FILE_WRITE_ERROR;
    }
  }

  return fclose(file) ? FILE_CLOSE_ERROR : OK;
}
  
const char* VectAry::name() const
{
  return d_name_p;
}

VectAryIter::VectAryIter(const VectAry &va) : d_va(va), d_pos_p(va.d_data_p)
{
}

Vector& VectAryIter::operator()() const
{
  return **d_pos_p;
}

VectAryIter::operator const void*() const
{
  return d_pos_p < d_va.d_data_p + d_va.d_size ? this : 0;
}

void VectAryIter::operator++()
{
  ++d_pos_p;
}

void VectAryIter::set(int i)
{
  d_pos_p = &d_va.d_data_p[i];
}
