#include <gp_Pnt.hxx>
#include <gp_Vec.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec2d.hxx>

#ifndef DEB
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#endif


#define SURFACE1 (*((ThePSurface *)(surface1)))
#define SURFACE2 (*((ThePSurface *)(surface2)))
#define CURVE    (*((TheCurveOnSurf *)(curve)))

IntImp_ZerCOnSSParFunc::IntImp_ZerCOnSSParFunc(const ThePSurface& S1,
					       const TheCurveOnSurf& C,
					       const ThePSurface& S2)
{
  surface1 = (Standard_Address)(&S1);
  surface2 = (Standard_Address)(&S2);
  curve    = (Standard_Address)(&C);
}

Standard_Integer IntImp_ZerCOnSSParFunc::NbVariables()const { return 3;}

Standard_Integer IntImp_ZerCOnSSParFunc::NbEquations()const { return 3;}

Standard_Boolean IntImp_ZerCOnSSParFunc::Value(const math_Vector& X,
			                    math_Vector& F){

  gp_Pnt Psurf(ThePSurfaceTool::Value(SURFACE1,X(1),X(2)));
  gp_Pnt2d p2d(TheCurveTool::Value(CURVE,X(3)));
  gp_Pnt Pcurv(ThePSurfaceTool::Value(SURFACE2,p2d.X(),p2d.Y()));

  F(1) = Psurf.X()-Pcurv.X();
  F(2) = Psurf.Y()-Pcurv.Y();
  F(3) = Psurf.Z()-Pcurv.Z();
  f = F(1)*F(1)+ F(2)*F(2)+ F(3)*F(3);
  p = gp_Pnt((Psurf.XYZ()+Pcurv.XYZ())/2.);
  return Standard_True;
}

Standard_Boolean IntImp_ZerCOnSSParFunc::Derivatives ( const math_Vector& X,
						    math_Matrix& D) {
  gp_Pnt Psurf,Pcurv;
  gp_Vec D1u,D1v,D1w;
  gp_Pnt2d p2d;
  gp_Vec2d d2d;
  gp_Vec d1u,d1v;

  ThePSurfaceTool::D1(SURFACE1,X(1),X(2),Psurf,D1u,D1v);
  TheCurveTool::D1(CURVE,X(3),p2d,d2d);
  ThePSurfaceTool::D1(SURFACE2,p2d.X(),p2d.Y(),Pcurv,d1u,d1v);
  D1w.SetLinearForm(d2d.X(),d1u,d2d.Y(),d1v);

  D(1,1) = D1u.X();
  D(1,2) = D1v.X();
  D(1,3) = -D1w.X();
  D(2,1) = D1u.Y();
  D(2,2) = D1v.Y();
  D(2,3) = -D1w.Y();
  D(3,1) = D1u.Z();
  D(3,2) = D1v.Z();
  D(3,3) = -D1w.Z();
  return Standard_True;
} 

Standard_Boolean IntImp_ZerCOnSSParFunc::Values( const math_Vector& X,
					      math_Vector& F,
					      math_Matrix& D) {
  gp_Pnt Psurf,Pcurv;
  gp_Vec D1u,D1v,D1w;

  gp_Pnt2d p2d;
  gp_Vec2d d2d;
  gp_Vec d1u,d1v;

  ThePSurfaceTool::D1(SURFACE1,X(1),X(2),Psurf,D1u,D1v);
  TheCurveTool::D1(CURVE,X(3),p2d,d2d);
  ThePSurfaceTool::D1(SURFACE2,p2d.X(),p2d.Y(),Pcurv,d1u,d1v);
  D1w.SetLinearForm(d2d.X(),d1u,d2d.Y(),d1v);

  D(1,1) = D1u.X();
  D(1,2) = D1v.X();
  D(1,3) = -D1w.X();
  D(2,1) = D1u.Y();
  D(2,2) = D1v.Y();
  D(2,3) = -D1w.Y();
  D(3,1) = D1u.Z();
  D(3,2) = D1v.Z();
  D(3,3) = -D1w.Z();
  F(1) = Psurf.X()-Pcurv.X();
  F(2) = Psurf.Y()-Pcurv.Y();
  F(3) = Psurf.Z()-Pcurv.Z();
  f = F(1)*F(1)+ F(2)*F(2)+ F(3)*F(3);
  p = gp_Pnt((Psurf.XYZ()+Pcurv.XYZ())/2.);
  return Standard_True;
}

const gp_Pnt& IntImp_ZerCOnSSParFunc::Point() const { return p;}

Standard_Real IntImp_ZerCOnSSParFunc::Root() const { return f;}

const ThePSurface& IntImp_ZerCOnSSParFunc::AuxillarSurface() const { 
  return SURFACE1;}

const TheCurveOnSurf& IntImp_ZerCOnSSParFunc::AuxillarCurve() const { 
  return CURVE;}

#undef SURFACE1
#undef SURFACE2
#undef CURVE