//
// $Id: CylindricalSurface.cpp,v 1.22 2004/02/27 02:57:35 mikeh Exp $
//
// File: CylindricalSurface.cc
// Purpose: 
// Created: 13-SEP-1997   John Hobbs
// Modified 14-JUL-1999  A.Mayorov add onSurface method.
// Modified 25-AUG-1999  A.Mayorov fix resize
//
// $Revision: 1.22 $
//
//

// Include files

#include <sstream>
#include "geometry_system/components/CylindricalSurface.hpp"
#include "geometry_system/components/CylindricalCoordinate.hpp"
#include "geometry_system/components/CartesianCoordinate.hpp"
#include "geometry_system/components/CylindricalMatrix.hpp"
#include "geometry_system/components/XGS.hpp"
#include "PhysicsVectors/Rotation.h"
#include "CLHEP/Units/PhysicalConstants.h"
#include "geometry_system/components/absGeometryDistortion.hpp"
using namespace dgs;
using namespace std;

//#ifdef DEFECT_CMATH_NOT_STD
//// This is primarily MSVC++ 5/6.  Our version is defined in GeometryXform.cpp
//static double abs(double x) {
//  if( x<0.0 ) return -x;
//  return x;
//}
//#endif

CylindricalSurface::CylindricalSurface():  _radius(0), _zhalf(0), 
    _cvers("Revision: $") 
{
  _tangang[0]=halfpi;
  _tangang[1]=halfpi;
  _tangang[2]=0;
}

CylindricalSurface::CylindricalSurface(const double radius, const double zhalf):
    _radius(radius), _zhalf(zhalf), _cvers("Revision: $") 
{
  _tangang[0]=halfpi;
  _tangang[1]=halfpi;
  _tangang[2]=0;
}

SpacePoint 
CylindricalSurface::local_to_global(const CylindricalCoordinate& lpt) const
//
// Purpose: Convert a local point to its equivalent in D0 space. 
//
{
  return GeometryElement::local_to_global(lpt);
}

MatrixD
CylindricalSurface::local_to_global(const CylindricalMatrix& errmat) const
//
// Purpose: Convert a local point to its equivalent in D0 space. 
//
{
  return GeometryElement::local_to_global(errmat);
}

CylindricalCoordinate CylindricalSurface::global_to_local(const SpacePoint& gpt) const
//
// Purpose: Convert a global D0 point to its local equivalent
//
{
  //  CylindricalCoordinate undone = get_position().invert(gpt);
  CylindricalCoordinate undone = GeometryElement::global_to_local(gpt);
  double radius = undone.rxy();
  double absz = abs(undone.z());
  if(onSurface(undone)) return undone;
  else return  CylindricalCoordinate(0.0,0.0,0.0);
}

bool CylindricalSurface::onSurface(const SpacePoint &lpt) const
{
  double radius = lpt.rxy();
  double absz = abs(lpt.z());

  // Is the point in +-z range?
  if( (_zhalf!=0) && (absz > _zhalf + 0.0001) ) {   // Try 1 micron safety
    std::cerr << "CylindricalSurface::global_to_local, Z-coordinate = " << lpt.z()
	 << " outside maximum of " << _zhalf << endl;
    return false;
  }

  // Z-coordinate OK.  Check radius
  if( (_zhalf!=0) && absz==_zhalf ) {  // On endplate
    if( abs(radius - _radius) > 0.00001 ) {   // Try 0.1 micron safety
      std::cerr << "CylindricalSurface::global_to_local, radius = " << radius
	   << " outside maximum of " << _radius << endl;
      return false;
    }
    return true;
  }
  else {                            // On body
    if( abs(radius - _radius) > 0.00001 ) {   //  Try 0.1 micron difference??
      std::cerr << "CylindricalSurface::global_to_local, radius = " << radius
	   << " not expected radius of " << _radius << endl;
      return false;
    }
    return true;
  }
}

CylindricalCoordinate CylindricalSurface::xform_to_local(const GeometryXform& xf) const
//
// Purpose: Support positioning children on my surface.
//
{
  CartesianCoordinate offset(xf[0],xf[1],xf[2]);
  return global_to_local(offset);
}

GeometryXform CylindricalSurface::local_to_xform(const CylindricalCoordinate& lpt) const
//
// Purpose: Convert a point on myself to a geometry transform.  The rotataion
//          portion gives a plane tangent to the surface.
//
{
  Rotation _tRotation(_tangang[0],_tangang[1],_tangang[2]);
  return GeometryXform(lpt.x(),lpt.y(),lpt.z(),
		       _tRotation.getPhi()+lpt.phi(),
		       _tRotation.getTheta(),
		       _tRotation.getPsi());
}

bool CylindricalSurface::resize(const double dr, const double dzhalf)
//
// Purpose: Change the size of the cylinder.  If there are dependents,
//          move them if the radius changes.
// States:
//
{
  // Sanity checks
  if( _radius+dr < 0.0 ) {
    ostringstream smsg;
    smsg << "CylindricalSurface::resize(), New radius = " << _radius 
	 << " + " << dr << " < 0";
    throw XGSIllegalSize(smsg.str().c_str());
  }

  if( _zhalf+dzhalf < 0.0 ) {
    ostringstream smsg;
    smsg << "CylindricalSurface::resize(), New half length = " << _zhalf
	 << " + " << dzhalf << " < 0";
    throw XGSIllegalSize(smsg.str().c_str());
  }

  double radius =_radius+dr;
  _zhalf += dzhalf;

  // First adjust kids (because of "correct radius" test in global_to_local)
  if( dr != 0.0 ) {
    list<GeometryElement*> mykids = get_children();
    list<GeometryElement*>::iterator j=mykids.begin();
    while( j != mykids.end() ) {
      GeometryXform kid_relative = (*j)->get_relative_position(get_position());
      Rotation rot=kid_relative.get_rotation();
      double phi=atan2(kid_relative[1],kid_relative[0]);
      double x = kid_relative[0] + dr*cos(phi);
      double y = kid_relative[1] + dr*sin(phi);
      double z = kid_relative[2];
      kid_relative = GeometryXform(SpaceVector(x,y,z),rot);
      (*j)->set_relative_position(get_position(),kid_relative);
      ++j;
    }
    _radius = radius;
  }

  return true;
}

bool CylindricalSurface::position_child_at(const CylindricalCoordinate& lpt,
					   GeometryElement& kid)
//
// Purpose: Set a child's reference position
//
{
  GeometryXform gxf = local_to_xform(lpt);
  gxf = get_positionptr().apply(gxf);
  kid.set_position(gxf);
  return true;
}

////////////////////////////////////////////////////////////////////////
//
//  Return global r coordinate, given global phi,z.
//
//  Written with assumption that cylinder is nearly aligned along z axis,
//  and thus only one solution possible.  If zero or two solutions found,
//  r = -1.0 returned.
//
//  HLM, 04APR98
//
double CylindricalSurface::get_r(double phi, double z) const
{
  //
  //  Get origin
  //
  double x0 = get_positionptr()[0];
  double y0 = get_positionptr()[1];
  double z0 = get_positionptr()[2];
  //
  //  Get inverse transormation
  //
  Rotation inv = get_positionptr().get_inverse_rotation();
  //
  //  Calculate intermediate expressions
  //
  double a1 = inv.xx() * cos(phi) + inv.xy() * sin(phi);
  double a2 = inv.yx() * cos(phi) + inv.yy() * sin(phi);
  double a3 = inv.zx() * cos(phi) + inv.zy() * sin(phi);

  double b1 = inv.xz() * z - inv.xx() * x0 - inv.xy() * y0 - inv.xz() * z0;
  double b2 = inv.yz() * z - inv.yx() * x0 - inv.yy() * y0 - inv.yz() * z0;
  double b3 = inv.zz() * z - inv.zx() * x0 - inv.zy() * y0 - inv.zz() * z0;

  double sq = ( (a1 * a1 + a2 * a2) * _radius * _radius) - 
    (a2 * b1 - a1 * b2) * (a2 * b1 - a1 * b2);

  if (sq <= 0)
    return -1.0;      //  No solution, return illegal radius as flag
  else
    sq = sqrt(sq);
  //
  //  Calculate quadratic solution.
  //
  double r1 = (- (a1 * b1 + a2 * b2) + sq) / (a1*a1 + a2*a2);
  double r2 = (- (a1 * b1 + a2 * b2) - sq) / (a1*a1 + a2*a2);
  //
  //  Check if two (or zero) solutions found -- if so, return illegal flag
  //  Otherwise, return solution.
  //
  double r=-1.0;
  if (r1 * r2 <= 0) {
    // calculated solution assumes that cylinder is perfect
    // assuming that phi and r distortions are independ
    // recalculate r
    r=(r1>0) ? r1:r2;
    absGeometryDistortion * distortion=get_distortion();
    if(distortion!=0) {
      SpacePoint *  gpt=distortion->distort(CylindricalPoint(r,phi,z));
      r=gpt->rxy();
    }
  }
  return r;
}

 void CylindricalSurface::set_tangent_Xform(const Rotation & tForm) 
{
  _tangang[0] = tForm.phi();
  _tangang[1] = tForm.theta();
  _tangang[2] = tForm.psi();

  // _tRotation=tForm;
}

////////////////////////////////////////////////////////////////////////
//
std::ostream& dgs::operator <<(std::ostream& os, const CylindricalSurface& me)
//
// Purpose: Text dump this instance.
// States:
//
{
  os << "CylindricalSurface, (radius,Zlen/2)=(" << me._radius << "," << 
    me._zhalf << ") at" << me.get_position() << endl;
  return os;
}