// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <gce_MakePln.hxx>
#include <Extrema_ExtPS.hxx>
#include <Extrema_ExtPC.hxx>

#ifdef DRAW
#include <DrawTrSurf.hxx>
#endif


static const Standard_Real CosRef3D = 0.88;

static void RecadreIfPeriodic(Standard_Real& NewU,
                              Standard_Real& NewV,
                              const Standard_Real OldU,
                              const Standard_Real OldV,
                              const Standard_Real UPeriod,
                              const Standard_Real VPeriod)
{
  if (UPeriod > 0.)
  {
    Standard_Real sign = (NewU < OldU)? 1 : -1;
    while (Abs(NewU - OldU) > UPeriod/2)
      NewU += sign * UPeriod;
  }
  if (VPeriod > 0.)
  {
    Standard_Real sign = (NewV < OldV)? 1 : -1;
    while (Abs(NewV - OldV) > VPeriod/2)
      NewV += sign * VPeriod;
  }
}

static void evalpinit(math_Vector& parinit,
		      const Blend_Point& previousP,
		      const Standard_Real parprec,
		      const Standard_Real param,
		      const math_Vector& infbound,
		      const math_Vector& supbound,
		      const Standard_Boolean classonS1, 
		      const Standard_Boolean classonS2)
{
  if(previousP.IsTangencyPoint()){
    previousP.ParametersOnS1(parinit(1),parinit(2));
    previousP.ParametersOnS2(parinit(3),parinit(4));
  }
  else {
    Standard_Real u1,v1,u2,v2;
    Standard_Real du1,dv1,du2,dv2;
    Standard_Boolean Inside=Standard_True;
    previousP.ParametersOnS1(u1,v1);
    previousP.ParametersOnS2(u2,v2);
    previousP.Tangent2dOnS1().Coord(du1,dv1);
    previousP.Tangent2dOnS2().Coord(du2,dv2);
    Standard_Real step = param - parprec;
    u1+= step*du1;
    v1+= step*dv1;
    if ( classonS1 ) {
      if ((u1<infbound(1)) || (u1>supbound(1))) Inside=Standard_False;
      if ((v1<infbound(2)) || (v1>supbound(2))) Inside=Standard_False;
    }
    u2+= step*du2;
    v2+= step*dv2;
    if ( classonS2) {
      if ((u2<infbound(3)) || (u2>supbound(3))) Inside=Standard_False;
      if ((v2<infbound(4)) || (v2>supbound(4))) Inside=Standard_False;
    }

    if (Inside) {
      parinit(1) = u1;
      parinit(2) = v1;
      parinit(3) = u2;
      parinit(4) = v2;
    }
    else { // on ne joue pas au plus malin
      previousP.ParametersOnS1(parinit(1),parinit(2));
      previousP.ParametersOnS2(parinit(3),parinit(4));
    }
    
  }
}



void Blend_Walking::InternalPerform(Blend_Function& Func,
				    Blend_FuncInv& FuncInv,
				    const Standard_Real Bound)
{
  Standard_Real Cosi = 0., Cosi2 = 0.;

  Standard_Real stepw = pasmax;
  Standard_Integer nbp = line->NbPoints();
  if(nbp >= 2){ //On reprend le dernier step s'il n est pas trop petit.
    if(sens < 0.){
      stepw = (line->Point(2).Parameter() - line->Point(1).Parameter());
    }
    else{
      stepw = (line->Point(nbp).Parameter() - line->Point(nbp - 1).Parameter());
    }
    stepw = Max(stepw,100.*tolgui);
  }
  Standard_Real parprec = param;
  gp_Vec TgOnGuide, PrevTgOnGuide;
  gp_Pnt PtOnGuide;
  hguide->D1(parprec, PtOnGuide, TgOnGuide);
  PrevTgOnGuide = TgOnGuide;

  if (sens*(parprec - Bound) >= -tolgui) {
    return;
  }
  Blend_Status State = Blend_OnRst12;
  TopAbs_State situ1 =TopAbs_IN,situ2=TopAbs_IN;
  Standard_Real w1,w2;
  Standard_Integer Index1 = 0, Index2 = 0, nbarc;
  Standard_Boolean Arrive,recad1,recad2, control;
  Standard_Boolean Isvtx1 = Standard_False, Isvtx2 = Standard_False, echecrecad;
  gp_Pnt2d p2d;
  math_Vector tolerance(1,4),infbound(1,4),supbound(1,4),parinit(1,4);
  math_Vector solrst1(1,4),solrst2(1,4);
  TheVertex Vtx1,Vtx2;
  TheExtremity Ext1,Ext2;

  //IntSurf_Transition Tline,Tarc;

  Func.GetTolerance(tolerance,tolpoint3d);
  Func.GetBounds(infbound,supbound);

  math_FunctionSetRoot rsnld(Func,tolerance,30);
  parinit = sol;

  Arrive = Standard_False;
  param = parprec + sens*stepw;
  if(sens *(param - Bound) > 0.) {
    stepw = sens*(Bound - parprec)*0.5;
    param = parprec + sens*stepw;
  }

  evalpinit(parinit,previousP,parprec,param,
	    infbound,supbound, clasonS1, clasonS2);

  while (!Arrive) {

#ifdef OCCT_DEBUG
    sectioncalculee = 0;
    nbcomputedsection++;
#endif

    hguide->D1(param, PtOnGuide, TgOnGuide);
    //Check deflection on guide
    Cosi = PrevTgOnGuide * TgOnGuide;
    if (Cosi < gp::Resolution()) //angle>=pi/2 or null magnitude
      Cosi2 = 0.;
    else
      Cosi2 = Cosi * Cosi / PrevTgOnGuide.SquareMagnitude() / TgOnGuide.SquareMagnitude();
    if (Cosi2 < CosRef3D) //angle 3d too great
    {
      State = Blend_StepTooLarge;
      stepw = stepw/2.;
      param = parprec + sens*stepw;  // on ne risque pas de depasser Bound.
      if (Abs(stepw) < tolgui) {
        Ext1.SetValue(previousP.PointOnS1(),
                      sol(1),sol(2),
                      previousP.Parameter(),tolpoint3d);
        Ext2.SetValue(previousP.PointOnS2(),
                      sol(3),sol(4),
                      previousP.Parameter(),tolpoint3d);
        if (!previousP.IsTangencyPoint()) {
          Ext1.SetTangent(previousP.TangentOnS1());
          Ext2.SetTangent(previousP.TangentOnS2());
        }
        Arrive = Standard_True;
      }
      continue;
    }
    PrevTgOnGuide = TgOnGuide;
    //////////////////////////
    
    Standard_Boolean bonpoint = 1;
    Func.Set(param);
    rsnld.Perform(Func,parinit,infbound,supbound);

    if (!rsnld.IsDone()) {
      State = Blend_StepTooLarge;
      bonpoint = 0;
    }
    else {
      rsnld.Root(sol);

      if(clasonS1) situ1 = domain1->Classify(gp_Pnt2d(sol(1),sol(2)),
					     Min(tolerance(1),tolerance(2)),0);
      else situ1 = TopAbs_IN;
      if(clasonS2) situ2 = domain2->Classify(gp_Pnt2d(sol(3),sol(4)),
					     Min(tolerance(3),tolerance(4)),0);
      else situ2 = TopAbs_IN;
    }
    if(bonpoint && line->NbPoints() == 1 && (situ1 != TopAbs_IN || situ2 != TopAbs_IN)){
      State = Blend_StepTooLarge;
      bonpoint = 0;
    }
    if(bonpoint){
      w1 = w2 = Bound;
      recad1 = Standard_False;
      recad2 = Standard_False;
      echecrecad = Standard_False;
      control = Standard_False;

      if (situ1 == TopAbs_OUT || situ1 == TopAbs_ON) {
	// pb inverse sur surf1
	//Si le recadrage s'effectue dans le sens de la progression a une tolerance pres,
	//on a pris la mauvaise solution.
	recad1 = Recadre(FuncInv,Standard_True,
			 sol,solrst1,Index1,Isvtx1,Vtx1);

	if (recad1) {
	  Standard_Real wtemp;
	  wtemp  = solrst1(2);
	  if ((param - wtemp)/sens>= -10*tolgui){
	    w1 = solrst1(2);
	    control = Standard_True;
	  }
	  else {
	    echecrecad = Standard_True;
	    recad1 = Standard_False;
	    State = Blend_StepTooLarge;
	    bonpoint = 0;
	    stepw = stepw/2.;
	  }
	}
	else {
	  echecrecad = Standard_True;
	}
      }
      if (situ2 == TopAbs_OUT || situ2 == TopAbs_ON) {
	// pb inverse sur surf2
	//Si le recadrage s'effectue dans le sens de la progression a une tolerance pres,
	//on a pris la mauvaise solution.
	recad2 = Recadre(FuncInv,Standard_False,
			 sol,solrst2,Index2,Isvtx2,Vtx2);
	
	if (recad2) {
	  Standard_Real wtemp;
	  wtemp = solrst2(2);
	  if ((param - wtemp)/sens>= -10*tolgui){
	    w2 = solrst2(2);
	    control = Standard_True;
	  }
	  else {
	    echecrecad = Standard_True;
	    recad2 = Standard_False;
	    State = Blend_StepTooLarge;
	    bonpoint = 0;
	    stepw = stepw/2.;
	  }
	}
	else {
	  echecrecad = Standard_True;
	}
      }
      
      // Que faut il controler
      if (recad1 && recad2) {
	  if (Abs(w1-w2) <= 10*tolgui) {
	  // pas besoin de controler les recadrage
	  // Le control pouvant se planter (cf model blend10)
          // La tolerance est choisie grossse afin, de permetre au 
          // cheminement suivant, de poser quelques sections ...
	  control = Standard_False; 
	  }
	  else if (sens*(w1-w2) < 0.) {
	    //sol sur 1 ?
	    recad2 = Standard_False;
	  }
	  else {
	    //sol sur 2 ?
	    recad1 = Standard_False;
	  }
	}

      // Controle effectif des recadrage
      if (control) {
	TopAbs_State situ;
	if (recad1 && clasonS2) {
	  situ = recdomain2->Classify(gp_Pnt2d(solrst1(3),solrst1(4)),
			              Min(tolerance(3),tolerance(4)));
	  if (situ == TopAbs_OUT) {
	    recad1 = Standard_False;
	    echecrecad = Standard_True;
	  }
	}
	else if (recad2 && clasonS1) {
	  situ = recdomain1->Classify(gp_Pnt2d(solrst2(3),solrst2(4)),
			              Min(tolerance(1),tolerance(1)));
	  if (situ == TopAbs_OUT) {
	    recad2 = Standard_False;
	    echecrecad = Standard_True;
	  }
	}
      }

      if(recad1 || recad2) echecrecad = Standard_False;

      if (!echecrecad) {
	if (recad1 && recad2) {
	  //sol sur 1 et 2 a la fois
	  // On passe par les arcs , pour ne pas avoir de probleme
	  // avec les surfaces periodiques.
	  State = Blend_OnRst12;
	  param =  (w1+w2)/2;
          gp_Pnt Pnt1, Pnt2;
	  p2d = TheArcTool::Value(recdomain1->Value(),solrst1(1));
	  sol(1) = p2d.X();
	  sol(2) = p2d.Y();
          Pnt1 = TheSurfaceTool::Value(surf1,sol(1),sol(2));
	  p2d = TheArcTool::Value(recdomain2->Value(),solrst2(1));
	  sol(3) = p2d.X();
	  sol(4) = p2d.Y();
          Pnt2 = TheSurfaceTool::Value(surf2,sol(3),sol(4));
          const Standard_Real TolProd = 1.e-5;
          Standard_Real SavedParams [2];
          Standard_Boolean SameDirs [2] = {Standard_False, Standard_False};
          ChFiDS_ElSpine& theElSpine = *hguide;
          SavedParams[0] = theElSpine.GetSavedFirstParameter();
          SavedParams[1] = theElSpine.GetSavedLastParameter();
          for (Standard_Integer ind = 0; ind < 2; ind++)
          {
            if (!Precision::IsInfinite(SavedParams[ind]))
            {
              //Check the original first and last parameters of guide curve
              //for equality to found parameter <param>:
              //check equality of tangent to guide curve and
              //normal to plane built on 3 points:
              //point on guide curve and points on restrictions of adjacent
              //surfaces.
              gp_Pnt Pnt0;
              gp_Vec Dir0;
              hguide->D1(SavedParams[ind], Pnt0, Dir0);
              Standard_Real Length = Dir0.Magnitude();
              if (Length <= gp::Resolution())
                continue;
              Dir0 /= Length;
              gce_MakePln PlaneBuilder(Pnt0, Pnt1, Pnt2);
              if (!PlaneBuilder.IsDone())
                continue;
              gp_Pln thePlane = PlaneBuilder.Value();
              gp_Dir DirPlane = thePlane.Axis().Direction();
              gp_Vec theProd = Dir0 ^ DirPlane;
              Standard_Real ProdMod = theProd.Magnitude();
              if (ProdMod <= TolProd)
                SameDirs[ind] = Standard_True;
            }
          }
          Standard_Real theParam = Precision::Infinite();
          //Choose the closest parameter
          if (SameDirs[0] && SameDirs[1])
            theParam = (Abs(param - SavedParams[0]) < Abs(param - SavedParams[1]))?
              SavedParams[0] : SavedParams[1];
          else if (SameDirs[0])
            theParam = SavedParams[0];
          else if (SameDirs[1])
            theParam = SavedParams[1];

          Standard_Real NewU, NewV, NewParam;
          gp_Pnt NewPnt;
          Standard_Boolean Corrected = CorrectExtremityOnOneRst(1, sol(3), sol(4), param, Pnt1,
                                                                NewU, NewV, NewPnt, NewParam);
          if (Corrected)
          {
            if (Abs(param - NewParam) < Abs(param - theParam))
              theParam = NewParam;
          }
          
          if (!Precision::IsInfinite(theParam))
            param = theParam;
	}
	else if (recad1) {
	  // sol sur 1
	  State = Blend_OnRst1;
	  param = w1;
	  recdomain1->Init();
	  nbarc = 1;
	  while (nbarc < Index1) {
	    nbarc++;
	    recdomain1->Next();
	  }
	  p2d = TheArcTool::Value(recdomain1->Value(),solrst1(1));
	  sol(1) = p2d.X();
	  sol(2) = p2d.Y();
	  sol(3) = solrst1(3);
	  sol(4) = solrst1(4);
          gp_Pnt thePntOnRst = TheSurfaceTool::Value(surf1,sol(1),sol(2));
          Standard_Real NewU, NewV, NewParam;
          gp_Pnt NewPnt;
          Standard_Boolean Corrected = CorrectExtremityOnOneRst(1, sol(3), sol(4), param, thePntOnRst,
                                                                NewU, NewV, NewPnt, NewParam);
          if (Corrected)
          {
            param = NewParam;
            sol(3) = NewU;
            sol(4) = NewV;
          }
	}
	else if (recad2) {
	  //sol sur 2
	  State = Blend_OnRst2;
	  param = w2;
	  
	  recdomain2->Init();
	  nbarc = 1;
	  while (nbarc < Index2) {
	    nbarc++;
	    recdomain2->Next();
	  }
	  p2d = TheArcTool::Value(recdomain2->Value(),solrst2(1));
	  sol(1) = solrst2(3);
	  sol(2) = solrst2(4);
	  sol(3) = p2d.X();
	  sol(4) = p2d.Y();
          gp_Pnt thePntOnRst = TheSurfaceTool::Value(surf2,sol(3),sol(4));
          Standard_Real NewU, NewV, NewParam;
          gp_Pnt NewPnt;
          Standard_Boolean Corrected = CorrectExtremityOnOneRst(2, sol(1), sol(2), param, thePntOnRst,
                                                                NewU, NewV, NewPnt, NewParam);
          if (Corrected)
          {
            param = NewParam;
            sol(1) = NewU;
            sol(2) = NewV;
          }
	}
	else {
	  State = Blend_OK;
	}

	Standard_Boolean testdefl = 1;
#ifdef OCCT_DEBUG
	testdefl = !Blend_GetcontextNOTESTDEFL();
#endif	
	if (recad1 || recad2) {
	  Func.Set(param);
          // Il vaut mieux un pas non orthodoxe que pas de recadrage!! PMN
	  State = TestArret(Func, State, 
			    (testdefl && (Abs(stepw) > 3*tolgui)),
			    Standard_False, Standard_True);
	}
	else {
	  State = TestArret(Func, State, testdefl);
	}
      }
      else { 
	// Ou bien le pas max est mal regle. On divise.
//	if(line->NbPoints() == 1) State = Blend_StepTooLarge;
	if (stepw > 2*tolgui) State = Blend_StepTooLarge;
	// Sinon echec recadrage. On sort avec PointsConfondus
	else {
#ifdef OCCT_DEBUG
	  std::cout << "Echec recadrage" << std::endl;
#endif	  
	  State = Blend_SamePoints;
	}
      }
    }

#ifdef OCCT_DEBUG
    if (Blend_GettraceDRAWSECT()){
      Drawsect(surf1,surf2,sol,param,Func, State);
    }
#endif
    switch (State) {
    case Blend_OK :
      {  
	// Mettre a jour la ligne.
	if (sens>0.) {
	  line->Append(previousP);
	}
	else {
	  line->Prepend(previousP);
	}
#ifdef DRAW
        Standard_Integer nbpts = line->NbPoints();
        char name [100];
        sprintf(name, "pg%d", nbpts);
        DrawTrSurf::Set(name, PtOnGuide);
        sprintf(name, "p1_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS1());
        sprintf(name, "p2_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS2());
#endif

	parprec = param;

	if (param == Bound) {
	  Arrive = Standard_True;
	  Ext1.SetValue(previousP.PointOnS1(),
			sol(1),sol(2),
			previousP.Parameter(), tolpoint3d);
	  Ext2.SetValue(previousP.PointOnS2(),
			sol(3),sol(4),
			previousP.Parameter(), tolpoint3d);
	  if (!previousP.IsTangencyPoint()) {
	    Ext1.SetTangent(previousP.TangentOnS1());
	    Ext2.SetTangent(previousP.TangentOnS2());
	  }

	  // Indiquer que fin sur Bound.
	}
	else {
	  param = param + sens*stepw;
	  if (sens*(param - Bound) > - tolgui) {
	    param = Bound;
	  }
	}
	evalpinit(parinit,previousP,parprec,param,
		  infbound,supbound, clasonS1, clasonS2);
      }
      break;
      
    case Blend_StepTooLarge :
      {
	stepw = stepw/2.;
	if (Abs(stepw) < tolgui) {
	  Ext1.SetValue(previousP.PointOnS1(),
			sol(1),sol(2),
			previousP.Parameter(),tolpoint3d);
	  Ext2.SetValue(previousP.PointOnS2(),
			sol(3),sol(4),
			previousP.Parameter(),tolpoint3d);
	  if (!previousP.IsTangencyPoint()) {
	    Ext1.SetTangent(previousP.TangentOnS1());
	    Ext2.SetTangent(previousP.TangentOnS2());
	  }
	  Arrive = Standard_True;
	  if (line->NbPoints()>=2) {
	    // Indiquer qu on s arrete en cours de cheminement
	  }
//	  else {
//	    line->Clear();
//	  }
	}
	else {
	  param = parprec + sens*stepw;  // on ne risque pas de depasser Bound.
	  evalpinit(parinit,previousP,parprec,param,
		    infbound,supbound, clasonS1, clasonS2);
	}
      }
      break;
      
    case Blend_StepTooSmall :
      {
	// Mettre a jour la ligne.
	if (sens>0.) {
	  line->Append(previousP);
	}
	else {
	  line->Prepend(previousP);
	}
#ifdef DRAW
        Standard_Integer nbpts = line->NbPoints();
        char name [100];
        sprintf(name, "pg%d", nbpts);
        DrawTrSurf::Set(name, PtOnGuide);
        sprintf(name, "p1_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS1());
        sprintf(name, "p2_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS2());
#endif

	parprec = param;

	stepw = Min(1.5*stepw,pasmax);
	if (param == Bound) {
	  Arrive = Standard_True;
	  Ext1.SetValue(previousP.PointOnS1(),
			sol(1),sol(2),
			previousP.Parameter(),tolpoint3d);
	  Ext2.SetValue(previousP.PointOnS2(),
			sol(3),sol(4), 
			previousP.Parameter(),tolpoint3d);
	  if (!previousP.IsTangencyPoint()) {
	    Ext1.SetTangent(previousP.TangentOnS1());
	    Ext2.SetTangent(previousP.TangentOnS2());
	  }
	  // Indiquer que fin sur Bound.
	}
	else {
	  param = param + sens*stepw;
	  if (sens*(param - Bound) > - tolgui) {
	    param = Bound;
	  }
	}
	evalpinit(parinit,previousP,parprec,param,
		  infbound,supbound, clasonS1, clasonS2);
      }
      break;
      
    case Blend_OnRst1  :
      {
	if (sens>0.) {
	  line->Append(previousP);
	}
	else {
	  line->Prepend(previousP);
	}
#ifdef DRAW
        Standard_Integer nbpts = line->NbPoints();
        char name [100];
        sprintf(name, "pg%d", nbpts);
        DrawTrSurf::Set(name, PtOnGuide);
        sprintf(name, "p1_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS1());
        sprintf(name, "p2_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS2());
#endif

	MakeExtremity(Ext1,Standard_True,Index1,
		      solrst1(1),Isvtx1,Vtx1);
        // On blinde le cas singulier ou un des recadrage a planter
        if (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2.0 * tolpoint3d)) {
	  Ext2.SetValue(previousP.PointOnS1(),
			sol(3),sol(4),tolpoint3d);
	  if (Isvtx1) MakeSingularExtremity(Ext2, Standard_False, Vtx1);
	}
	else {
	  Ext2.SetValue(previousP.PointOnS2(),
			sol(3),sol(4),
			previousP.Parameter(),tolpoint3d);
	}
	Arrive = Standard_True;
      }
      break;

    case Blend_OnRst2  :
      {
	if (sens>0.) {
	  line->Append(previousP);
	}
	else {
	  line->Prepend(previousP);
	}
#ifdef DRAW
        Standard_Integer nbpts = line->NbPoints();
        char name [100];
        sprintf(name, "pg%d", nbpts);
        DrawTrSurf::Set(name, PtOnGuide);
        sprintf(name, "p1_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS1());
        sprintf(name, "p2_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS2());
#endif

        // On blinde le cas singulier ou un des recadrage a plante
        if (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2.0 * tolpoint3d)) {
	  Ext1.SetValue(previousP.PointOnS2(),
			sol(1),sol(2),tolpoint3d);
	  if (Isvtx2) MakeSingularExtremity(Ext1, Standard_True, Vtx2);
	}
	else {
	  Ext1.SetValue(previousP.PointOnS1(),
			sol(1),sol(2),
			previousP.Parameter(),tolpoint3d);
	}
	MakeExtremity(Ext2,Standard_False,Index2,
		      solrst2(1),Isvtx2,Vtx2);
	Arrive = Standard_True;
      }
      break;


    case Blend_OnRst12 :
      {
	if (sens>0.) {
	  line->Append(previousP);
	}
	else {
	  line->Prepend(previousP);
	}
#ifdef DRAW
        Standard_Integer nbpts = line->NbPoints();
        char name [100];
        sprintf(name, "pg%d", nbpts);
        DrawTrSurf::Set(name, PtOnGuide);
        sprintf(name, "p1_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS1());
        sprintf(name, "p2_%d", nbpts);
        DrawTrSurf::Set(name, previousP.PointOnS2());
#endif

	if ( (Isvtx1 != Isvtx2) &&
	    (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2.0 * tolpoint3d)) ) {
	  // On blinde le cas singulier ou un seul recadrage
	  // est reconnu comme vertex.
	  if (Isvtx1) {
	    Isvtx2 = Standard_True;
	    Vtx2 = Vtx1;
	  }
	  else {
	    Isvtx1 = Standard_True;
	    Vtx1 = Vtx2;
	  }
	}

	MakeExtremity(Ext1,Standard_True,Index1,
		      solrst1(1),Isvtx1,Vtx1);
	MakeExtremity(Ext2,Standard_False,Index2,
		      solrst2(1),Isvtx2,Vtx2);
	Arrive = Standard_True;
      }
      break;

    case Blend_SamePoints :
      {
	// On arrete
#ifdef OCCT_DEBUG
	std::cout << " Points confondus dans le cheminement" << std::endl;
#endif
	Ext1.SetValue(previousP.PointOnS1(),
		      sol(1),sol(2),
		      previousP.Parameter(),tolpoint3d);
	Ext2.SetValue(previousP.PointOnS2(),
		      sol(3),sol(4),
		      previousP.Parameter(),tolpoint3d);
	if (!previousP.IsTangencyPoint()) {
	  Ext1.SetTangent(previousP.TangentOnS1());
	  Ext2.SetTangent(previousP.TangentOnS2());
	}
	Arrive = Standard_True;
      }
      break;
    default:
      break;
    }
    if (Arrive) {
      if (sens > 0.) {
	line->SetEndPoints(Ext1,Ext2);
      }
      else {
	line->SetStartPoints(Ext1,Ext2);

      }
    }

  }

}

Standard_Boolean Blend_Walking::CorrectExtremityOnOneRst(const Standard_Integer IndexOfRst,
                                                         const Standard_Real theU,
                                                         const Standard_Real theV,
                                                         const Standard_Real theParam,
                                                         const gp_Pnt& thePntOnRst,
                                                         Standard_Real& NewU,
                                                         Standard_Real& NewV,
                                                         gp_Pnt& NewPoint,
                                                         Standard_Real& NewParam) const
{
  const Standard_Real TolAng = 0.001; //bug OCC25701
  
  ChFiDS_ElSpine& theElSpine = *hguide;
  if (theElSpine.NbVertices() == 0)
    return Standard_False;
  
  Handle(TheTopolTool) DomainOfRst = (IndexOfRst == 1)? recdomain1 : recdomain2;
  TheSurface SurfOfRst = (IndexOfRst == 1)? surf1 : surf2;
  TheSurface AnotherSurf = (IndexOfRst == 1)? surf2 : surf1;
  
  //Correct point on surface 2
  //First we find right <param>
  Standard_Real Ends [2];
  Ends[0] = TheArcTool::FirstParameter(DomainOfRst->Value());
  Ends[1] = TheArcTool::LastParameter(DomainOfRst->Value());
  Standard_Real GlobalMinSqDist = Precision::Infinite();
  Standard_Real ParamOnGuide = 0;
  gp_Pnt PointOnGuide;
  for (Standard_Integer k = 0; k < 2; k++)
  {
    gp_Pnt2d P2dOnEnd = TheArcTool::Value(DomainOfRst->Value(), Ends[k]);
    gp_Pnt PntOnEnd = TheSurfaceTool::Value(SurfOfRst, P2dOnEnd.X(), P2dOnEnd.Y());
    Extrema_ExtPC projoncurv(PntOnEnd, theElSpine);
    if (!projoncurv.IsDone())
      continue;
    Standard_Real MinSqDist = Precision::Infinite();
    Standard_Integer imin = 0;
    for (Standard_Integer ind = 1; ind <= projoncurv.NbExt(); ind++)
    {
      Standard_Real aSqDist = projoncurv.SquareDistance(ind);
      if (aSqDist < MinSqDist)
      {
        MinSqDist = aSqDist;
        imin = ind;
      }
    }
    if (MinSqDist < GlobalMinSqDist)
    {
      GlobalMinSqDist = MinSqDist;
      ParamOnGuide = projoncurv.Point(imin).Parameter();
      PointOnGuide = projoncurv.Point(imin).Value();
    }
  }
  NewParam = ParamOnGuide;
  if (hguide->IsPeriodic())
  {
    Standard_Real Period = hguide->Period();
    Standard_Real sign = (NewParam < theParam)? 1 : -1;
    while (Abs(NewParam - theParam) > Period/2)
      NewParam += sign *Period;
  }

  //Second we find right point and tangent on guide
  GlobalMinSqDist = Precision::Infinite();
  gp_Ax1 theAx1;
  for (Standard_Integer ind = 1; ind <= theElSpine.NbVertices(); ind++)
  {
    const gp_Ax1& anAx1 = theElSpine.VertexWithTangent(ind);
    gp_Pnt aPnt = anAx1.Location();
    Standard_Real aSqDist = PointOnGuide.SquareDistance(aPnt);
    if (aSqDist < GlobalMinSqDist)
    {
      GlobalMinSqDist = aSqDist;
      theAx1 = anAx1;
    }
  }
  const gp_Pnt& Pnt0 = theAx1.Location();
  const gp_Dir& Dir0 = theAx1.Direction();
  //Check new point: is it real solution?
  gp_Pnt OldPonGuide = hguide->Value(theParam);
  gp_Pnt PntOnSurf2 = TheSurfaceTool::Value(AnotherSurf,theU,theV); //old point
  gce_MakePln PlaneBuilder(thePntOnRst, OldPonGuide, PntOnSurf2);
  if (!PlaneBuilder.IsDone())
    return Standard_False;
  gp_Pln OldPlane = PlaneBuilder.Value();
  gp_Dir OldDir = OldPlane.Axis().Direction();
  Standard_Real Angle = OldDir.Angle(Dir0);
  if (Angle > M_PI/2)
    Angle = M_PI - Angle;
  if (Angle > TolAng)
    return Standard_False;
  ///////////////////////////////////////
  //Project the point(theU,theV) on the plane(Pnt0,Dir0)
  gp_Vec aVec(Pnt0, PntOnSurf2);
  gp_Vec aTranslation( (aVec.XYZ() * Dir0.XYZ()) * Dir0.XYZ() );
  gp_Pnt PntOnPlane = PntOnSurf2.Translated(-aTranslation);

  //Check new point again: does point on restriction belong to the plane?
  PlaneBuilder = gce_MakePln(thePntOnRst, Pnt0, PntOnPlane);
  if (!PlaneBuilder.IsDone())
    return Standard_False;
  gp_Pln NewPlane = PlaneBuilder.Value();
  const gp_Dir& DirOfNewPlane = NewPlane.Axis().Direction();
  Angle = Dir0.Angle(DirOfNewPlane);
  if (Angle > M_PI/2)
    Angle = M_PI - Angle;
  if (Angle > TolAng)
    return Standard_False;
  ////////////////////////////////////////////////////////////////////////
  
  //Project the point <PntOnPlane> on the surface 2
  Extrema_ExtPS projonsurf(PntOnPlane, *AnotherSurf,
                           Precision::PConfusion(), Precision::PConfusion(),
                           Extrema_ExtFlag_MIN);
  if (projonsurf.IsDone())
  {
    Standard_Real MinSqDist = Precision::Infinite();
    Standard_Integer imin = 0;
    for (Standard_Integer ind = 1; ind <= projonsurf.NbExt(); ind++)
    {
      Standard_Real aSqDist = projonsurf.SquareDistance(ind);
      if (aSqDist < MinSqDist)
      {
        MinSqDist = aSqDist;
        imin = ind;
      }
    }
    if (imin)
    {
      Extrema_POnSurf NewPOnSurf2 = projonsurf.Point(imin);
      NewPoint = NewPOnSurf2.Value();
      NewPOnSurf2.Parameter(NewU, NewV);
      Standard_Real uperiod = (AnotherSurf->IsUPeriodic())? AnotherSurf->UPeriod() : 0.;
      Standard_Real vperiod = (AnotherSurf->IsVPeriodic())? AnotherSurf->VPeriod() : 0.;
      RecadreIfPeriodic(NewU, NewV, theU, theV,
                        uperiod, vperiod);
      return Standard_True;
    }
  }

  return Standard_False;
}