void correctPlanarPolygon( vector<Pt3dr> &aPolygon )
{
	if (aPolygon.size() < 3) ELISE_ERROR_EXIT("aPolygon.size() = " << aPolygon.size() << " < 3");
	Pt3dr u = vunit(aPolygon[1] - aPolygon[0]), minV = vunit(aPolygon[2] - aPolygon[0]);
	size_t minI = 2;
	REAL minScal = ElAbs(scal(u, minV));
	for (size_t i = 3; i < aPolygon.size(); i++)
	{
		Pt3dr v = vunit(aPolygon[i] - aPolygon[0]);
		REAL d = ElAbs(scal(u, v));
		if (d < minScal)
		{
			minScal = d;
			minI = i;
			minV = v;
		}
	}
	cout << "minI = " << minI << endl;
	cout << "minScal = " << minScal << endl;
	cout << "minV = " << minV << endl;
	Pt3dr n = u ^ minV;
	cout << "minV = " << minV << endl;
	ElMatrix<REAL> planToGlobalMatrix = MatFromCol(u, minV, n);
	if (planToGlobalMatrix.Det() < 1e-10) ELISE_ERROR_EXIT("matrix is not inversible");

	ElRotation3D planToGlobalRot(aPolygon[0], planToGlobalMatrix, true);
	ElRotation3D globalToPlanRot = planToGlobalRot.inv();

	//~ const size_t nbVertices = aPolygon.size();
	//~ ostringstream ss;
	//~ static int ii = 0;
	//~ const REAL extrudSize = 1e4;
	//~ ss << "polygon_" << (ii++) << ".ply";
	//~ ofstream f(ss.str().c_str());
	//~ f << "ply" << endl;
	//~ f << "format ascii 1.0" << endl;
	//~ f << "element vertex " << 4 * nbVertices << endl;
	//~ f << "property float x" << endl;
	//~ f << "property float y" << endl;
	//~ f << "property float z" << endl;
	//~ f << "property uchar red" << endl;
	//~ f << "property uchar green" << endl;
	//~ f << "property uchar blue" << endl;
	//~ f << "element face " << nbVertices << endl;
	//~ f << "property list uchar int vertex_indices" << endl;
	//~ f << "end_header" << endl;

	REAL zDiffSum = 0.;
	for (size_t i = 0; i < aPolygon.size(); i++)
	{
		Pt3dr p = globalToPlanRot.ImAff(aPolygon[i]);
		zDiffSum += ElAbs(p.z);
		aPolygon[i] = planToGlobalRot.ImAff(Pt3dr(p.x, p.y, 0.));

		//~ Pt3dr p0 = (i == 0) ? aPolygon[aPolygon.size() - 1] : aPolygon[i - 1], p1 = aPolygon[i], p2 = p1 + n * extrudSize, p3 = p0 + n * extrudSize;
		//~ f << p0.x << ' ' << p0.y << ' ' << p0.z << " 128 128 128" << endl;
		//~ f << p1.x << ' ' << p1.y << ' ' << p1.z << " 128 128 128" << endl;
		//~ f << p2.x << ' ' << p2.y << ' ' << p2.z << " 128 128 128" << endl;
		//~ f << p3.x << ' ' << p3.y << ' ' << p3.z << " 128 128 128" << endl;
	}

	//~ for (size_t i = 0; i < aPolygon.size(); i++)
		//~ f << 4 << ' ' << i * 4 << ' ' << i * 4 + 1 << ' ' << i * 4 + 2 << ' ' << i* 4 + 3 << endl;
	//~ f.close();
	cout << "zDiffSum = " << zDiffSum << endl;
}
cNewO_CombineCple::cNewO_CombineCple(const  cStructMergeTieP< cFixedSizeMergeTieP<2,Pt2dr> >  &  aMap,ElRotation3D * aTestSol) :
    mCurStep     (1<<NbPow2),
    mNbStepTeta  (4 * NbDecoup0PIS2 * mCurStep),
    mCurRot      (3,3),
    mW           (0)
{

    // REDONDANT AVEC FONCTION GLOBALES FAITE APRES ....  PackReduit

    /******************************************************/
    /*                                                    */
    /*   A-        Selection des sommets                  */
    /*                                                    */
    /******************************************************/

    //------------------------------------------------------------------------
    // A- 1- Preselrection purement aleatoire d'un nombre raisonnable depoints
    //------------------------------------------------------------------------

    const std::list<tMerge *> & aLM  = aMap.ListMerged();
    RMat_Inertie aMat;

    {
       cRandNParmiQ aSelec(NbMaxInit, (int)aLM.size());
       for (std::list<tMerge *>::const_iterator itM=aLM.begin() ; itM!=aLM.end() ; itM++)
       {
            if (aSelec.GetNext())
            {
               mVAllCdt.push_back(cCdtCombTiep(*itM));
               Pt2dr aP1 = (*itM)->GetVal(0);
               aMat.add_pt_en_place(aP1.x,aP1.y);
            }
       }
    }
    aMat = aMat.normalize();
    int aNbSomTot = int(mVAllCdt.size());

    double aSurfType  =  sqrt (aMat.s11()* aMat.s22() - ElSquare(aMat.s12()));
    double aDistType = sqrt(aSurfType/aNbSomTot);

    double aSzW = 800;
    if (1)
    {
         mP0W = aMap.ValInf(0);
         Pt2dr aP1 = aMap.ValSup(0);
         Pt2dr aSz = aP1-mP0W;
         mP0W = mP0W - aSz * 0.1;
         aP1 = aP1 + aSz * 0.1;
         aSz = aP1-mP0W;

         mScaleW  = aSzW /ElMax(aSz.x,aSz.y) ;
         mW = Video_Win::PtrWStd(round_ni(aSz*mScaleW));
    }

    //------------------------------------------------------------------------
    // A-2   Calcul d'une fonction de deponderation  
    //------------------------------------------------------------------------

    for (int aKS1 = 0 ; aKS1 <aNbSomTot ; aKS1++)
    {
        for (int aKS2 = aKS1 ; aKS2 <aNbSomTot ; aKS2++)
        {
           // sqrt pour attenuer la ponderation
           double aDist = sqrt(dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0);
// aDist=1;
           // double aDist = (dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0);
           double aPds = 1 / (aDistType+aDist);
           mVAllCdt[aKS1].mPdsOccup += aPds;
           mVAllCdt[aKS2].mPdsOccup += aPds;
        }
        if (mW)
            mW->draw_circle_abs(ToW( mVAllCdt[aKS1].mP1),2.0,mW->pdisc()(P8COL::blue));
    }
    for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++)
    {
       cCdtCombTiep & aCdt = mVAllCdt[aKSom];
       aCdt.mPdsOccup *= ElSquare(aCdt.mMerge->NbArc());
    }
    



    int aNbSomSel = ElMin(aNbSomTot,NbTieP);

    //------------------------------------------------------------------------
    // A-3  Calcul de aNbSomSel points biens repartis
    //------------------------------------------------------------------------

    ElTimer aChrono;
    for (int aKSel=0 ; aKSel<aNbSomSel ; aKSel++)
    {
         // Recherche du cdt le plus loin
         double aMaxDMin = 0;
         cCdtCombTiep * aBest = 0;
         for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++)
         {
             cCdtCombTiep & aCdt = mVAllCdt[aKSom];
             double aDist = aCdt.mDMin *  aCdt.mPdsOccup;
             if ((!aCdt.mTaken) &&  (aDist > aMaxDMin))
             {
                 aMaxDMin = aDist;
                 aBest = & aCdt;
             }
         }
         ELISE_ASSERT(aBest!=0,"cNewO_CombineCple");
         for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++)
         {
             cCdtCombTiep & aCdt = mVAllCdt[aKSom];
             aCdt.mDMin = ElMin(aCdt.mDMin,dist48(aCdt.mP1-aBest->mP1));
         }
         aBest->mQ1 = vunit(Pt3dr(aBest->mP1.x,aBest->mP1.y,1.0));
         Pt2dr aP2 = aBest->mMerge->GetVal(1);
         aBest->mQ2Init = vunit(Pt3dr(aP2.x,aP2.y,1.0));

         mVCdtSel.push_back(aBest);
         if (mW)
            mW->draw_circle_abs(ToW( aBest->mP1),3.0,mW->pdisc()(P8COL::red));
    }



    /******************************************************/
    /*                                                    */
    /*  B- Calcul des arcs                                */
    /*                                                    */
    /******************************************************/
 

    // B-1  Au max le nombre d'arc  possible
    int aNbA = NbCple;
    while (aNbA >  ((aNbSomSel * (aNbSomSel-1)) /2)) aNbA--;
    
    int aNbIter = (aNbA-1) / aNbSomSel + 1;
    cRandNParmiQ aSelec(aNbA- (aNbIter-1) * aNbSomSel,aNbSomSel);
    int aNbAMaj = aNbIter * aNbSomSel;


    std::vector<int> aPermut = RandPermut(aNbA);

    // B-2 Recherche des arsc
    int  aKA=0;
    for (int aCptAMaj = 0 ; aCptAMaj < aNbAMaj ; aCptAMaj++)
    { 
        // Tous les sommets sont equi repartis, sauf a la fin on choisit a hasard
        bool aSelK = true;
        if ( (aCptAMaj/aNbSomSel)== (aNbIter-1))  // Si derniere iter, test special
        {
           aSelK = aSelec.GetNext();
        }

        if (aSelK)
        {
            int aKP1 =  (aCptAMaj%aNbSomSel);
            double aTeta = (aPermut[aKA] * 2 * PI) / aNbA;
            Pt2dr aDir = Pt2dr::FromPolar(1.0,aTeta);
            // std::cout << "teta " << aTeta << "\n";
            double aBestSc=-1.0;
            int aBestK=-1;
            for (int aKP2 = 0 ; aKP2 < aNbSomSel ; aKP2++)
            {
                if (aKP2!=aKP1)
                {
                    Pt2dr aV =  (mVCdtSel[aKP2]->mP1- mVCdtSel[aKP1]->mP1) / aDir;
                    Pt2dr aU = vunit(aV);
                    
               // Favorise les llongs arc et homogeneise les directions
                    double aSc = NRrandom3() * euclid(aV) * (1/(1+ElSquare(5.0*aU.y)));
                    if ((aSc>aBestSc) && (aKP2!=aKP1))
                    {
                       aBestSc= aSc;
                       aBestK = aKP2;
                    }
                }
            }
            ELISE_ASSERT((aBestK>=0),"No Best Arc");
            mLArcs.push_back(Pt2di(aKP1,aBestK));
            if (mW)
            {
                mW->draw_seg(ToW( mVCdtSel[aKP1]->mP1),ToW( mVCdtSel[aBestK]->mP1),mW->pdisc()(P8COL::green));
            }
            aKA++;
        }
    }


    /******************************************************/
    /*                                                    */
    /*                                                    */
    /*                                                    */
    /******************************************************/

    if (mW) mW->clik_in();


    if (aTestSol)
    {
       ElRotation3D aR = * aTestSol;

       // Le sens corret a ete retabli (j'espere !!)
       // SetCurRot(aR.Mat());
       // std::cout << "Test Externe : " << CalculCostCur() <<"\n";
       // aR = aR.inv();

       SetCurRot(aR.Mat());
       std::cout << "Test Externe I : " << CalculCostCur() <<"\n";
       std::cout << "CostBase " << CostOneBase(aR.tr()) << "\n";
          // ElRotation3D *
    }

    std::cout << "cNewO_CombineCple::cNewO_CombineCple " << aNbSomTot << "\n";
    Pt3di aP;

    std::list<Pt3di> aLPMin;
    double aCostMin = 1e10;
    Pt3di aPMin(1000,1000,1000);

    for (aP.x =  -NbDecoup0PIS2 ; aP.x <= NbDecoup0PIS2 ; aP.x ++)
    {
         std::cout << "DECx " << aP.x << "\n";
         for (aP.y =  -NbDecoup0PIS2 ; aP.y <= NbDecoup0PIS2 ; aP.y ++)
         {
              for (aP.z =  - (2*NbDecoup0PIS2) ; aP.z < (2*NbDecoup0PIS2) ; aP.z ++)
              {
                    double aVC =  GetCost(aP*mCurStep);
                    bool IsMinLoc =    (aVC < GetCost((aP+Pt3di( 1,0,0)) * mCurStep))
                                    && (aVC < GetCost((aP+Pt3di(-1,0,0)) * mCurStep))
                                    && (aVC < GetCost((aP+Pt3di(0, 1,0)) * mCurStep))
                                    && (aVC < GetCost((aP+Pt3di(0,-1,0)) * mCurStep))
                                    && (aVC < GetCost((aP+Pt3di(0,0, 1)) * mCurStep))
                                    && (aVC < GetCost((aP+Pt3di(0,0,-1)) * mCurStep));

                    int aDelta = 2;
                    for (int aDx=-aDelta ; (aDx<=aDelta) && IsMinLoc ; aDx++)
                    {
                        for (int aDy=-aDelta ; (aDy<=aDelta) && IsMinLoc ; aDy++)
                        {
                            for (int aDz=-aDelta ; (aDz<=aDelta) && IsMinLoc ; aDz++)
                            {
                                 if ((aDx!=0) || (aDy!=0) || (aDz!=0))
                                 {
                                     IsMinLoc = IsMinLoc && (aVC<GetCost( (aP+Pt3di(aDx,aDy,aDz))*mCurStep));
                                 }
                            }
                        }
                    }
                    if (IsMinLoc)
                    {
                       std::cout << " IisssMinn " << aP << " " << aVC << "\n";

                       aLPMin.push_back(aP*mCurStep);
                    }
                    if (aVC<aCostMin)
                    {
                       aPMin = aP*mCurStep;
                       aCostMin = aVC;
                    }
              }
         }
    }

    std::cout << "COST " << aCostMin  << " PMIN " << PInt2Tetas(aPMin ) << " NbMinLoc " << aLPMin.size() << "\n";

    Pt3dr aTeta =  PInt2Tetas(aPMin);
    ElMatrix<double> aR  = ElMatrix<double>::Rotation(aTeta.z,aTeta.y,aTeta.x);
    for (int aY=0 ; aY<3 ; aY++)
    {
        for (int aX=0 ; aX<3 ; aX++)
        {
            std::cout  << aR(aX,aY) << " ";
        }
        std::cout << "\n";
    }
/*
    std::cout << "Sssz " <<  aLPMin.size() << "\n";
    if ( aLPMin.size()>0) std::cout << "PP00 " << *(aLPMin.begin()) << "\n";
*/
}
void cMEPCoCentrik::Test(const ElPackHomologue & aPack,const  ElMatrix<REAL> & aMat,const ElRotation3D * aRef,double anEcart)
{
     if (mShow)
     {
          std::cout << " ============== ROTATION PURE =============\n";
     }


     ComputePlanBase(aMat);
     Pt3dr aN1 = ComputeBase();
     Pt3dr aN2 = ComputeNormBase();

     Pt3dr aN3 = vunit(aN1^aN2);
     ElRotation3D aRInit(aN3,aMat,true);
     mSolVraiR = OneTestMatr(aMat,aN3,anEcart);
     mCostVraiR = ProjCostMEP(aPack,mSolVraiR,0.1);
     
     mPMed = MedianNuage(aPack,mSolVraiR);
     if (mPMed.z < 0)
     {
          if (mShow) std::cout <<  "  ------------- Z Neg in Pur rot : swap -------------\n";
          mSolVraiR = ElRotation3D(-mSolVraiR.tr(),mSolVraiR.Mat(),true);
          mPMed = MedianNuage(aPack,mSolVraiR);
          aN3 = -aN3;
     }
     
     if ( mShow)
     {
          if (aRef) 
          {
               std::cout << "Ref " << vunit(aRef->tr())   << " Norm " << aN3 << "\n";
               std::cout << "COST REF " << ProjCostMEP(aPack,*aRef,0.1)* mFoc << "\n";
               ElRotation3D aSol2 = OneTestMatr(aRef->Mat(),aRef->tr(),anEcart);
               std::cout << "C REFOPT " << ProjCostMEP(aPack,aSol2,0.1)* mFoc << "\n";
               //   ShowMatr("REF/Mat",aRef->Mat()*aMat.transpose());
          }
          std::cout << "    ######  Co-Centrik COST-Init " <<  ProjCostMEP(aPack,aRInit,0.1)* mFoc << "\n";
          std::cout << "Sol  , Cost " <<   mCostVraiR * mFoc  << " Tr " << mSolVraiR.tr() << " Med " << mPMed << "\n";
     }



     // ShowMatr("REF/Sol",aRef->Mat()*aSol.Mat().transpose());



     if (mDoPly)
     {
         std::list<std::string> aVCom;
         std::vector<const cElNuage3DMaille *> aVNuage;
         cElNuage3DMaille::PlyPutFile
         (
               "Base.ply",
               aVCom,
               aVNuage,
               &mPtsPly,
               &mColPly,
               true
         );
    }


}