// ==================================================================
// Filename:    space.cpp
// Description: Implements the space class
//              Assumes angles are in radians
// Author:      L.R. McFarland
// Created:     4 Feb 1999
// Language:    C++
//
//  This file is part of lrm's Orbits software library.
//
//  Orbits is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  Orbits is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with Orbits.  If not, see <http://www.gnu.org/licenses/>.
// ==================================================================


#include <space.h>

using namespace std;
using namespace Cartesian;

// -----------------------
// ----- class space -----
// -----------------------

// ----- static data members -----

const space space::Uo(0,0,0);
const space space::Ux(1,0,0);
const space space::Uy(0,1,0);
const space space::Uz(0,0,1);

// ----- functions -----

const space Cartesian::operator+(const space& lhs, const space& rhs) {
  return(space(lhs.x() + rhs.x(), lhs.y() + rhs.y(), lhs.z() + rhs.z()));
}

const space Cartesian::operator-(const space& lhs, const space& rhs) {
  return(space(lhs.x() - rhs.x(), lhs.y() - rhs.y(), lhs.z() - rhs.z()));
}

// scale
const space Cartesian::operator*(const space& lhs, const double& rhs) {
  return(space(lhs.x() * rhs, lhs.y() * rhs, lhs.z() * rhs));
}

const space Cartesian::operator*(const double& lhs, const space& rhs) {
  return(Cartesian::operator*(rhs, lhs));
}

const space Cartesian::operator/(const space& lhs, const double& rhs)
  throw (DivideZeroError) {
  if (rhs == 0)
    throw DivideZeroError("space::operator/() attempted divide by zero.");
  return(space(lhs.x() / rhs, lhs.y() / rhs, lhs.z() / rhs));
}

const space Cartesian::operator/(const double& lhs, const space& rhs)
  throw (DivideZeroError) {
  if (rhs.x() == 0 || rhs.y() == 0 || rhs.z() == 0)
    throw DivideZeroError("space::operator/() attempted divide by zero.");
  return(space(lhs / rhs.x(), lhs / rhs.y(), lhs / rhs.z()));
}

// ----- vector products -----

const double Cartesian::operator*(const space& lhs, const space& rhs) {
  return(lhs.x()*rhs.x() + lhs.y()*rhs.y() + lhs.z()*rhs.z());
}

space Cartesian::cross(const space& a, const space& b) {
  space tmp;
  tmp.x(a.y()*b.z() - a.z()*b.y());
  tmp.y(a.z()*b.x() - a.x()*b.z());
  tmp.z(a.x()*b.y() - a.y()*b.x());
  return(tmp);
}

// ----- set using polar coordinates -----

void space::setUsingPolarCoords(double radius, double theta, double phi) {
  /// ASSUMES: angles in radians
  // theta: angle in the x-y plane
  // phi: polar angle (from the z axis)
  x(radius * sin(phi) * cos(theta));
  y(radius * sin(phi) * sin(theta));
  z(radius * cos(phi));
}

// -------------------------
// ----- class rotator -----
// -------------------------

rotator::rotator(const space& a_axis) :
  m_axis(a_axis),
  m_rotation_matrix(3, vector<double>(3, 0)),
  m_is_new_axis(true),
  m_old_radians(0.0)
{}

rotator::~rotator() {}

rotator::rotator(const rotator& rhs) :
  m_axis(rhs.axis()),
  m_rotation_matrix(rhs.m_rotation_matrix),
  m_is_new_axis(rhs.m_is_new_axis)
{} // TBD test this

rotator::rotator& rotator::operator=(const rotator::rotator& rhs) {
  if (this == &rhs) return(*this);
  axis(rhs.axis());
  m_rotation_matrix = rhs.m_rotation_matrix;
  m_is_new_axis = rhs.m_is_new_axis;
  return(*this);
} // TBD test this

void rotator::axis(const space& a_axis) {
  if (a_axis != m_axis) {
    m_axis = a_axis;
    m_is_new_axis = true;
  }
}

space rotator::rotate(const space& a_heading, const double& a_radians) {

  if (m_is_new_axis || m_old_radians != a_radians) {

    double c(cos(a_radians));
    double s(sin(a_radians));

    space normal(axis().normalized());

    double t(1-c);

    m_rotation_matrix[0][0] = c + normal.x()*normal.x()*t;
    m_rotation_matrix[1][1] = c + normal.y()*normal.y()*t;
    m_rotation_matrix[2][2] = c + normal.z()*normal.z()*t;

    double t1(normal.x()*normal.y()*t);
    double t2(normal.z()*s);

    m_rotation_matrix[1][0] = t1 + t2;
    m_rotation_matrix[0][1] = t1 - t2;

    t1 = normal.x()*normal.z()*t;
    t2 = normal.y()*s;

    m_rotation_matrix[2][0] = t1 - t2;
    m_rotation_matrix[0][2] = t1 + t2;

    t1 = normal.y()*normal.z()*t;
    t2 = normal.x()*s;

    m_rotation_matrix[2][1] = t1 + t2;
    m_rotation_matrix[1][2] = t1 - t2;

    m_is_new_axis = false;
    m_old_radians = a_radians;

  }

  space tmp(m_rotation_matrix[0][0]*a_heading.x() +
	    m_rotation_matrix[0][1]*a_heading.y() +
	    m_rotation_matrix[0][2]*a_heading.z(),
	    m_rotation_matrix[1][0]*a_heading.x() +
	    m_rotation_matrix[1][1]*a_heading.y() +
	    m_rotation_matrix[1][2]*a_heading.z(),
	    m_rotation_matrix[2][0]*a_heading.x() +
	    m_rotation_matrix[2][1]*a_heading.y() +
	    m_rotation_matrix[2][2]*a_heading.z());

  return(tmp);

}


// =========================
// ===== SpaceRecorder =====
// =========================

const unsigned int SpaceRecorder::default_size(1024);

SpaceRecorder::SpaceRecorder(const unsigned int& a_size_limit) :
  m_size_limit(a_size_limit),
  m_data(deque<space>(m_size_limit))
{}

SpaceRecorder::SpaceRecorder(const SpaceRecorder& a):
  m_size_limit(a.sizeLimit()),
  m_data(a.m_data)
{}

SpaceRecorder& 
SpaceRecorder::operator=(const SpaceRecorder::SpaceRecorder& rhs) {
  if (this == &rhs) return(*this);
  sizeLimit(rhs.sizeLimit());
  m_data = rhs.m_data;
  return(*this);
}

void SpaceRecorder::push(space a) {
  // even up the sizes to just under the limit.
  while (m_data.size() > sizeLimit() - 1)
    m_data.pop_front();
  m_data.push_back(a);
}

// output compatible for R frames <- read.table(flnm)
void SpaceRecorder::write2R(const string& flnm, bool skip_Uo) {

  ofstream ssfile(flnm.c_str());

  if (!ssfile.is_open()) {
    stringstream err;
    err << "Error: unable to open file \"" << flnm << "\"";
    throw SpaceRecorderIOError(err.str());
  }

  ssfile << "# Formated for R frames <- read.table(" << flnm << ")" << endl;
  ssfile << "x y z" << endl;

  for (unsigned int k = 0; k < m_data.size(); ++k) {

    // skip zero points from partially filled buffer.
    if (skip_Uo and m_data[k] == space::Uo)
      continue;

    ssfile << k << " "
	   << m_data[k].x() << " "
	   << m_data[k].y() << " "
	   << m_data[k].z() << endl;
  }

  ssfile.close();

}



// EOF