#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "StateVector.hh"

#ifdef DEBUG
# include "Syslog.h" 
# include <assert.h>
# define dbgCheck assert
#else
# define dbgCheck(cond) ((void)0)
#endif // DEBUG

/*
 * class StateVector
 */

// statics :

StateVector::scalar_type *StateVector::create_tab(size_t dim) {
  if( dim<1 )
    return NULL;
  else {
    scalar_type *result = (scalar_type *)calloc(dim, sizeof(scalar_type));

    dbgCheck(NULL!=result);
    return result;
  }
}

// structors :

StateVector::StateVector() 
  :m_dimension(0), m_data(NULL) {}

StateVector::StateVector(size_t dim)
  :m_dimension(dim), m_data(create_tab(dim)) {
  dbgCheck(0<dim);
}

StateVector::StateVector(Argument const &arg)
  :m_dimension(arg.length()), 
   m_data(create_tab(arg.length())) {
// #ifdef DEBUG
//   Symbol input;
// #endif 
  for(size_t i=0; i<m_dimension; ++i ) {
// #ifdef DEBUG
//     input += arg[i];
// #endif 
    m_data[i] = strtod(arg[i].c_str(), (char **)NULL);
  }
// #ifdef DEBUG
//   {
//     Symbol output = toString();
//     Syslog::write("\t StateVector::parse(\"%s\") = %s", 
// 		  input.c_str(), output.c_str());
//   }
// #endif 
}

StateVector::StateVector(StateVector const &other)
  :m_dimension(other.m_dimension), m_data(create_tab(other.m_dimension)) {
  if( m_dimension>0 )
    memcpy(m_data, other.m_data, m_dimension*sizeof(scalar_type));
}

StateVector::~StateVector() {
  if( NULL!=m_data ) 
    free(m_data);
}

// manipulators :

StateVector &StateVector::setDimension(size_t dim) {
  dbgCheck(0<dim);
  if( dim!=m_dimension ) {
    scalar_type *to_del = m_data;
    
    m_data = create_tab(dim);
    m_dimension = dim;
    if( NULL!=to_del )
      free(to_del);
  }
  return *this;
}

StateVector &StateVector::operator= (StateVector const &other) {
  if( m_dimension==0 ) {
    m_dimension = other.m_dimension;
    m_data = create_tab(m_dimension);
  } 
  dbgCheck(m_dimension==other.m_dimension);
  memcpy(m_data, other.m_data, m_dimension*sizeof(scalar_type));
  return *this;
}

StateVector::scalar_type &StateVector::get(size_t idx) {
  dbgCheck(idx<m_dimension);
  return m_data[idx];
}

StateVector &StateVector::operator+=(StateVector const &other) {
  size_t idx = 0;
  scalar_type *i = m_data;
  scalar_type const *j = other.m_data;

  dbgCheck(m_dimension==other.m_dimension);
  for( ; idx<m_dimension; ++idx, ++i, ++j)
    *i += *j;
  return *this;
}

StateVector &StateVector::operator-=(StateVector const &other) {
  size_t idx = 0;
  scalar_type *i = m_data;
  scalar_type const *j = other.m_data;

  dbgCheck(m_dimension==other.m_dimension);
  for( ; idx<m_dimension; ++idx, ++i, ++j)
    *i -= *j;
  return *this;
}

StateVector &StateVector::operator*=(StateVector::scalar_type x) {
  size_t idx = 0;
  scalar_type *i = m_data;

  for( ; idx<m_dimension; ++idx, ++i)
    *i *= x;
  return *this;
}

StateVector &StateVector::operator/=(StateVector::scalar_type x) {
  size_t idx = 0;
  scalar_type *i = m_data;

  dbgCheck(0!=x);
  for( ; idx<m_dimension; ++idx, ++i)
    *i /= x;
  return *this;
}

StateVector &StateVector::normalize(StateVector const &avg, 
				    StateVector const &sd) {
  size_t idx = 0;
  scalar_type *i = m_data;
  scalar_type const *a = avg.m_data;
  scalar_type const *s = sd.m_data;

  dbgCheck(m_dimension==avg.m_dimension);
  dbgCheck(m_dimension==sd.m_dimension);
  for( ; idx<m_dimension; ++idx, ++i, ++a, ++s) {
    if( 0==*s )
      *i = 0; // This dimension is not significant here
    else
      *i = (*i-*a) / *s;
  }
  return *this;
}

// observers :
    
StateVector::scalar_type const &StateVector::get(size_t idx) const {
  dbgCheck(idx<m_dimension);
  return m_data[idx];
}

bool StateVector::operator==(StateVector const &other) const {
  if( m_dimension!=other.m_dimension )
    return false;
  else {
    size_t idx = 0;
    scalar_type const *i = m_data;
    scalar_type const *j = other.m_data;

    for( ; idx<m_dimension && *i==*j; ++idx, ++i, ++j);
    return m_dimension==idx;
  }
}

StateVector::scalar_type StateVector::operator* (StateVector const &other) const {
  scalar_type res = 0;
  size_t idx = 0;
  scalar_type const *i = m_data;
  scalar_type const *j = other.m_data;

  dbgCheck(m_dimension==other.m_dimension);
  for( ; idx<m_dimension; ++idx, ++i, ++j)
    res += *i * *j;
  return res;
}

StateVector::scalar_type StateVector::norm() const {
  scalar_type n2 = operator* (*this);
  n2 = sqrt(n2);
  return n2;
}

Symbol StateVector::toString() const {
  size_t idx = 0;
  scalar_type const *i = m_data;
  static char buff[256];
  Symbol result = "( ";

  for( ; idx<m_dimension; ++idx, ++i ) {
    sprintf(buff, "%g ", *i);
    result += buff;
  }
  return result+=')';
}