#include <geometry_system/components/SphericalSurface.hpp>
#include <geometry_system/components/SphericalCoordinate.hpp>
#include <geometry_system/components/CartesianCoordinate.hpp>

using namespace std;
using namespace dgs;

// local to global:
SpacePoint
SphericalSurface::local_to_global( const SphericalCoordinate& point ) const {
  return GeometryElement::local_to_global( point );
}

// global to local:
SphericalCoordinate
SphericalSurface::global_to_local( const SpacePoint& point ) const {

  SphericalCoordinate coord = get_position().invert( point );

  // radius check:
  double r = coord.rxyz();
  if( r != _radius ) {
    std::cerr << "SphericalSurface::global_to_local error: radius = " << r;
    std::cerr << ", surface radius = " << _radius << endl;
  }

  // polar angle check:
  double theta = coord.theta();
  if( theta < _theta_min || theta > _theta_max ) {
    std::cerr << "SphericalSurface::global_to_local error: polar angle = ";
    std::cerr << theta << ", surface angle range is " << _theta_min << " to "; 
    std::cerr << _theta_max << endl;
  }

  // azimuthal angle check:
  double phi = coord.phi();
  if( phi < 0 || phi > _phi ) {
    std::cerr << "SphericalSurface::global_to_local error: azimuthal angle = ";
    std::cerr << phi << ", surface maximum angle is " << _phi << endl;
  }

  // everything is ok:
  return coord;

}

// resize:
bool SphericalSurface::resize(const double dr,
			      const double dtheta_min, const double dtheta_max,
			      const double dphi ) {

  _radius += dr;
  _theta_min += dtheta_min;
  _theta_max += dtheta_max;
  _phi += dphi;

  if( dr != 0 ) {

    list<GeometryElement*> vol_list = get_children();

    for( list<GeometryElement*>::iterator i = vol_list.begin();
	 i != vol_list.end(); i++ ) {

      GeometryXform kid_local = (*i)->get_relative_position(get_position());
      SphericalCoordinate pt = xform_to_local( kid_local );
      SphericalCoordinate new_origin( _radius, pt.theta(), pt.phi() );
      (*i)->set_relative_position(get_position(),local_to_xform(new_origin) );

    }

  }

  return true;

}

// position child:
bool SphericalSurface::position_child_at( const SphericalCoordinate& point,
					  GeometryElement& vol ) {

  GeometryXform pos = get_position().apply( local_to_xform( point ) );
  vol.set_position( pos );

  return true;

}

// output:
std::ostream& dgs::operator << (std::ostream& os, const SphericalSurface& surf ) {
  os << "SphericalSurface: radius = " << surf._radius <<
    ", lower theta = " << surf._theta_min <<
    ", upper theta = " << surf._theta_max <<
    ", upper phi = " << surf._phi <<
    "; positioned at " << surf.get_position() << endl;
  return os;
}

// GeometryXform to SphericalCoordinate:
SphericalCoordinate
SphericalSurface::xform_to_local( const GeometryXform& transf ) const {
  CartesianCoordinate offset( transf[0], transf[1], transf[2] );
  return global_to_local( offset );
}

// SphericalCoordinate to GeometryXform:
GeometryXform
SphericalSurface::local_to_xform( const SphericalCoordinate& point ) const {

  double x = point.x();
  double y = point.y();
  double z = point.z();

  // rotation - new xy plane is tangent to the surface:

  double phi = point.phi();
  double theta = point.theta();

  return GeometryXform( x, y, z, -phi, -theta, 0. );

}