/*Header-MicMac-eLiSe-25/06/2007

    MicMac : Multi Image Correspondances par Methodes Automatiques de Correlation
    eLiSe  : ELements of an Image Software Environnement

    www.micmac.ign.fr

   
    Copyright : Institut Geographique National
    Author : Marc Pierrot Deseilligny
    Contributors : Gregoire Maillet, Didier Boldo.

[1] M. Pierrot-Deseilligny, N. Paparoditis.
    "A multiresolution and optimization-based image matching approach:
    An application to surface reconstruction from SPOT5-HRS stereo imagery."
    In IAPRS vol XXXVI-1/W41 in ISPRS Workshop On Topographic Mapping From Space
    (With Special Emphasis on Small Satellites), Ankara, Turquie, 02-2006.

[2] M. Pierrot-Deseilligny, "MicMac, un lociel de mise en correspondance
    d'images, adapte au contexte geograhique" to appears in 
    Bulletin d'information de l'Institut Geographique National, 2007.

Francais :

   MicMac est un logiciel de mise en correspondance d'image adapte 
   au contexte de recherche en information geographique. Il s'appuie sur
   la bibliotheque de manipulation d'image eLiSe. Il est distibue sous la
   licences Cecill-B.  Voir en bas de fichier et  http://www.cecill.info.


English :

    MicMac is an open source software specialized in image matching
    for research in geographic information. MicMac is built on the
    eLiSe image library. MicMac is governed by the  "Cecill-B licence".
    See below and http://www.cecill.info.

Header-MicMac-eLiSe-25/06/2007*/

// #define _ELISE_ALGO_GEOM_QDT_H 1
// #define _ELISE_ALGO_GEOM_QDT_IMPLEM_H 1
// #define __QDT_INSERTOBJ__ 1



#include "cPose.h"

    /* ========== cStructRigidInit ============*/

cStructRigidInit::cStructRigidInit(cPoseCam * RigidMere,const ElRotation3D & aR) :
  mCMere  (RigidMere),
  mR0m1L0 (aR)
{
}


static const int NbMinCreateIm = 200;

//class cPtAVGR;
//class cAperoVisuGlobRes;


/***************** classes moved to cPose.h *******************/
     /*===========     cPtAVGR  ===========*/



cPtAVGR::cPtAVGR(const Pt3dr & aP,double aRes) :
   mPt  (Pt3df::P3ToThisT(aP)),
   mRes (aRes)
{
}

/*class cFoncPtOfPtAVGR
{
   public :
       Pt2dr operator () (cPtAVGR * aP) {return  Pt2dr(aP->mPt.x,aP->mPt.y);}
};*/

     /*===========     cAperoVisuGlobRes  ===========*/

/*typedef enum
{
    eBAVGR_X,
    eBAVGR_Y,
    eBAVGR_Z,
    eBAVGR_Res
} eBoxAVGR;

class cAperoVisuGlobRes
{
    public :
       void AddResidu(const Pt3dr & aP,double aRes);
       void DoResidu(const std::string & aDir,int aNbMes);

       cAperoVisuGlobRes();
       
    private :
       Interval  CalculBox(double & aVMil,double & aResol,eBoxAVGR aMode,double PropElim,double Rab);
       Box2dr    CalculBox_XY(double PropElim,double Rab);
       double    ToEcartStd(double anE) const;
       double    FromEcartStd(double anE) const;
       Pt3di     ColOfEcart(double anE);

       typedef ElQT<cPtAVGR *,Pt2dr,cFoncPtOfPtAVGR> tQtTiepT;

       int                  mNbPts;
       std::list<cPtAVGR *> mLpt;
       tQtTiepT *           mQt;
       double               mResol;
       double               mResolX;
       double               mResolY;
       double               mSigRes;
       double               mMoyRes;
       cPlyCloud            mPC;
       cPlyCloud            mPCLeg;  // Legende
       double               mVMilZ;
};*/


cAperoVisuGlobRes::cAperoVisuGlobRes() :
   mNbPts  (0),
   mQt     (0)
{
}



void cAperoVisuGlobRes::AddResidu(const Pt3dr & aP,double aRes)
{
     cPtAVGR * aPVG = new cPtAVGR(aP,aRes);

     mLpt.push_back(aPVG);
     mNbPts++;
}


Interval cAperoVisuGlobRes::CalculBox(double & aVMil,double & aResol,eBoxAVGR aMode,double aPropElim,double aPropRab)
{
    std::vector<float> aVVals;
    // double aSomV = 0;
    // double aSomV2 = 0;

    for (auto iT=mLpt.begin() ; iT!=mLpt.end() ; iT++)
    {
        double aVal = 0.0;
        if       (aMode==eBAVGR_Res)     aVal = (*iT)->mRes  ;
        else if  (aMode==eBAVGR_X)       aVal = (*iT)->mPt.x;
        else if  (aMode==eBAVGR_Y)       aVal = (*iT)->mPt.y;
        else if  (aMode==eBAVGR_Z)       aVal = (*iT)->mPt.z;
        else
        {
                 aVal = (*iT)->mPt.z;
                ELISE_ASSERT(false,"cAperoVisuGlobRes::CalculBox");
        }
        aVVals.push_back(aVal);
        // aSomV += aVal;
        // aSomV2 += ElSquare(aVal);
    }
    // aSomV /= mNbPts;
    // aSomV2 /= mNbPts;
    // aSomV2 -= ElSquare(aSomV);
    // Sigma = Larg / srqt(12) pour une distrib uniforme
    // double Larg =  sqrt(12*aSomV2);
    double aV25 = KthValProp(aVVals,0.25);  
    double aV75 = KthValProp(aVVals,0.75);
    aVMil = (aV75+aV25) / 2.0;
    double Larg = 2 * (aV75 - aV25);
    aResol = Larg / mNbPts;

    double aVMin = KthValProp(aVVals,aPropElim);  
    double aVMax = KthValProp(aVVals,1-aPropElim);  

    double aRab = (aVMax-aVMin) * aPropRab;

    aVMin -= aRab ;
    aVMax += aRab ;

    return Interval(aVMin,aVMax);
}

Box2dr  cAperoVisuGlobRes::CalculBox_XY(double aPropElim,double aPropRab)
{
    double aVMilX,aVMilY;
    Interval aIntX = CalculBox(aVMilX,mResolX,eBAVGR_X,aPropElim,aPropRab);
    Interval aIntY = CalculBox(aVMilY,mResolY,eBAVGR_Y,aPropElim,aPropRab);

    mResol = euclid(Pt2dr(mResolX,mResolY));

    return Box2dr(Pt2dr(aIntX._v0,aIntY._v0), Pt2dr(aIntX._v1,aIntY._v1));
}

double DirErFonc(double x);
double InvErrFonc(double x);

double  cAperoVisuGlobRes::ToEcartStd(double anE) const
{
     return DirErFonc((anE-mMoyRes)/mSigRes); 
}

double  cAperoVisuGlobRes::FromEcartStd(double anE) const
{
     return mMoyRes + mSigRes *InvErrFonc(anE);
}



Pt3di     cAperoVisuGlobRes::ColOfEcart(double anE)
{
    Elise_colour aCol = Elise_colour::its(0.5,(1-anE)/3.0,1.0);

    return Pt3di (AdjUC(aCol.r()*255),AdjUC(aCol.g()*255),AdjUC(aCol.b()*255));
}

template <class Type> bool gen_std_isnan(const Type & aP)
{
   return std_isnan(aP.x) || std_isnan(aP.y) ;
}

void cAperoVisuGlobRes::DoResidu(const std::string & aDir,int aNbMes)
{
    {
       double aRR;
       CalculBox(mVMilZ,aRR,eBAVGR_Z,0.16,0.0);
    }

    Box2dr aBoxQt = CalculBox_XY(0.02,0.3);
    cFoncPtOfPtAVGR aFoncP;
    
    mQt = new tQtTiepT(aFoncP,aBoxQt,10,mResol*5);

    for (auto iT=mLpt.begin() ; iT!=mLpt.end() ; iT++)
    {
        (*iT)->mInQt=   mQt->insert(*iT,true);
    }

    
    for (auto iTGlob=mLpt.begin() ; iTGlob!=mLpt.end() ; iTGlob++)
    {
        if ((*iTGlob)->mInQt)
        {
           // std::set<cPtAVGR *> aSet;
           Pt2dr aP = aFoncP(*iTGlob);
           // std::list<cPtAVGR *> aLVois = mQt->KPPVois(aP,aNbMes,1.5*mResol*sqrt(aNbMes));
           double aDist = mResol*sqrt(aNbMes);
           std::set<cPtAVGR *> aLVois;  
           mQt->RVoisins(aLVois,aP,mResol*sqrt(aNbMes));
           double aSomP=0.0;
           double aSomPRes=0.0;
           for (auto itV=aLVois.begin() ; itV!=aLVois.end() ; itV++)
           {
               if (euclid((*iTGlob)->mPt-(*itV)->mPt) < aDist)
               {
                   double aPds = 1.0;
                   aSomP += aPds;
                   aSomPRes += aPds * (*itV)->mRes;
               }
           }
           double aRes = aSomPRes / aSomP;
           (*iTGlob)->mResFiltr = aRes;
        }
    }
 
    double aRR; // Inutilise
    // Pour une gaussienne 68 % compris dans [-Sig,Sig]
    double aVMilRes;
    Interval  aIntRes = CalculBox(aVMilRes,aRR,eBAVGR_Res,0.16,0.0);
    mSigRes = (aIntRes._v1 - aIntRes._v0) / 2.0;
    mMoyRes = (aIntRes._v1 + aIntRes._v0) / 2.0;

    for (auto iTGlob=mLpt.begin() ; iTGlob!=mLpt.end() ; iTGlob++)
    {
        if ((*iTGlob)->mInQt)
        {
             double anEcart = ToEcartStd( (*iTGlob)->mResFiltr);
             Pt3di aCol = ColOfEcart(anEcart);
             mPC.AddPt(aCol,Pt3dr::P3ToThisT((*iTGlob)->mPt));
        }
    }

    mPC.PutFile(aDir+"CloudResidual.ply");

    Pt2dr aP0 = aBoxQt._p0 - Pt2dr(0,-40) * mResol;

    double aPasLeg = mResol *100;

    double aZLeg = mVMilZ;
    int aNbLeg = 10;
    double aLargX = 10;
    int    aLargY = 20;
    for (int aCpt=0 ; aCpt<=aNbLeg; aCpt++)
    {
        double aEcartStd =  (aCpt-aNbLeg*0.5) /(0.5+aNbLeg*0.5);
        double aRes = FromEcartStd(aEcartStd);
        if (aRes >0)
        {
           char aBuf[100];
           sprintf(aBuf,"%.2f",aRes);
           std::string aStrRes (aBuf);
           
           mPCLeg.PutString
           (
                aStrRes,
                Pt3dr(aP0.x,aP0.y,aZLeg)+Pt3dr(0,aCpt*aPasLeg*aLargY,0) , 
                Pt3dr(1,0,0),
                Pt3dr(0,1,0),
                Pt3di(255,255,255),
                aLargX* aPasLeg,  //  La largeur des caractere
                aPasLeg,  // espacement
                4  // carre de 4x4
            );


        }
    }

    int DensLeg = 20;
    for (int aCpt= -DensLeg/2 ; aCpt<=((DensLeg*aNbLeg) + DensLeg/2) ; aCpt++)
    {
        double aCptN  =  aCpt/double(DensLeg);
        double aEcartStd =  (aCptN-aNbLeg*0.5) /(0.5+aNbLeg*0.5);
        Pt3di aCol = ColOfEcart(aEcartStd);

        
        Pt3dr aPLine  = Pt3dr(aP0.x,aP0.y,aZLeg)+Pt3dr(0,aCptN*aPasLeg*aLargY,0) ; 

        for (int anX=0 ; anX<=DensLeg ; anX++)
        {
            Pt3dr aP = aPLine + Pt3dr(-(anX/double(DensLeg))*aPasLeg*aLargY,0,0);
            mPCLeg.AddPt(aCol,aP);
        }
    }
    mPCLeg.PutFile(aDir+"CloudResidual_Leg.ply");


/*
    for (int aK=-10 ; aK<10 ; aK++)
    {
        double aV= aK*0.2;
        std::cout << "EErr " << aV << " " << erfcc(aV) << " " <<DirErFonc(aV) <<  " Dif=" <<  InvErrFonc(DirErFonc(aV)) - aV << "\n";
    }
    getchar();
*/
}


static cAperoVisuGlobRes mAVGR;


//============================================

/*class cInfoAccumRes
{
     public :
       cInfoAccumRes(const Pt2dr & aPt,double aPds,double aResidu,const Pt2dr & aDir);

       Pt2dr  mPt;
       double mPds;
       double mResidu;
       Pt2dr  mDir;
};*/


cInfoAccumRes::cInfoAccumRes(const Pt2dr & aPt,double aPds,double aResidu,const Pt2dr & aDir) :
   mPt      (aPt),
   mPds     (aPds),
   mResidu  (aResidu),
   mDir     (aDir)
{
}

/*class cAccumResidu
{
    public :
       void Accum(const cInfoAccumRes &);
       cAccumResidu(Pt2di aSz,double aRed,bool OnlySign,int aDegPol);

       const Pt2di & SzRed() {return mSzRed;}

       void Export(const std::string & aDir,const std::string & aName,const cUseExportImageResidu &,FILE * );
       void ExportResXY(TIm2D<REAL4,REAL8>* aTResX,TIm2D<REAL4,REAL8>* aTResY);
       void ExportResXY(const Pt2di&,Pt2dr& aRes);
    private :
       void AccumInImage(const cInfoAccumRes &);

       std::list<cInfoAccumRes> mLIAR;
       int                      mNbInfo;
       double                   mSomPds;
       bool                     mOnlySign;
       double                   mResol;
       Pt2di                    mSz;
       Pt2di                    mSzRed;

       Im2D_REAL4               mPds;
       TIm2D<REAL4,REAL8>       mTPds;
       Im2D_REAL4               mMoySign;
       TIm2D<REAL4,REAL8>       mTMoySign;
       Im2D_REAL4               mMoyAbs;
       TIm2D<REAL4,REAL8>       mTMoyAbs;
       bool                     mInit;
       int                      mDegPol;
       L2SysSurResol *          mSys;
};*/

cAccumResidu::cAccumResidu(Pt2di aSz,double aResol,bool OnlySign,int aDegPol) :
   mNbInfo (0),
   mSomPds   (0.0),
   mOnlySign (OnlySign),
   mResol    (aResol),
   mSz       (aSz),
   mSzRed    (round_up(Pt2dr(aSz)/mResol)),
   mPds      (1,1),
   mTPds     (mPds),
   mMoySign  (1,1),
   mTMoySign (mMoySign),
   mMoyAbs   (1,1),
   mTMoyAbs  (mMoyAbs),
   mInit     (false),
   mDegPol   (aDegPol),
   mSys      (0)
{
}

void cAccumResidu::Export(const std::string & aDir,const std::string & aName,const cUseExportImageResidu & aUEIR,FILE * aFP ) 
{
   if (! mInit) return;

   fprintf(aFP,"=== %s =======\n",aName.c_str());
   fprintf(aFP,"  Nb=%d  WAver=%f\n",mNbInfo,mSomPds/mNbInfo);

   Tiff_Im::CreateFromFonc
   (
        aDir+"RawWeight-"+aName+".tif",
        mSzRed,
        mPds.in() * (mNbInfo/mSomPds),
        GenIm::real4
   );

   int aNbPixel = mSzRed.x * mSzRed.y;
   double aNbMesByCase = mNbInfo / double(aNbPixel);
   double aTargetNbMesByC = aUEIR.NbMesByCase().Val();
   if (aTargetNbMesByC >= 0)
   {
        double aSigma = sqrt(aTargetNbMesByC / aNbMesByCase);
        FilterGauss(mPds,aSigma);
        FilterGauss(mMoySign,aSigma);
        FilterGauss(mMoyAbs,aSigma);
   }

   Tiff_Im::CreateFromFonc
   (
        aDir+"ResSign-"+aName+".tif",
        mSzRed,
        mMoySign.in() / Max(mPds.in(),1e-4),
        GenIm::real4
   );

   double aMoySign,aMoyAbsSign,aSomPds;
   ELISE_COPY
   (
        mPds.all_pts(),
        Virgule(mPds.in(),mMoySign.in(),Abs(mMoySign.in())),
        Virgule(sigma(aSomPds),sigma(aMoySign),sigma(aMoyAbsSign))
   );
   fprintf(aFP,"  AverSign=%f  AverAbsSign=%f\n",aMoySign/aSomPds,aMoyAbsSign/aSomPds);

   if (!mOnlySign)
   {
      Tiff_Im::CreateFromFonc
      (
           aDir+"ResAbs-"+aName+".tif",
           mSzRed,
           mMoyAbs.in() / Max(mPds.in(),1e-4),
           GenIm::real4
      );
   }

    if (mSys)
    {
        bool aOk;
        Im1D_REAL8  aSol = mSys->Solve(&aOk);
        if (aOk)
        {
            double * aDS = aSol.data();

            Im2D_REAL4               aResX(mSzRed.x,mSzRed.y);
            TIm2D<REAL4,REAL8>       aTRx(aResX);
            Im2D_REAL4               aResY(mSzRed.x,mSzRed.y);
            TIm2D<REAL4,REAL8>       aTRy(aResY);

            Pt2di aPInd;
            for (aPInd.x=0 ; aPInd.x<mSzRed.x ; aPInd.x++)
            {
                for (aPInd.y=0 ; aPInd.y<mSzRed.y ; aPInd.y++)
                {
                    Pt2dr aPFulRes = Pt2dr(aPInd) * mResol;
                    Pt2dr aSzN = mSz/2.0;
                    double  aX = (aPFulRes.x-aSzN.x) / aSzN.x;
                    double  aY = (aPFulRes.y-aSzN.y) / aSzN.y;

                    std::vector<double> aVMx; // Monome Xn
                    std::vector<double> aVMy; // Monome Yn
                    aVMx.push_back(1.0);
                    aVMy.push_back(1.0);
                    for (int aD=0 ; aD< mDegPol ; aD++)
                    {
                      aVMx.push_back(aVMx.back() * aX);
                      aVMy.push_back(aVMy.back() * aY);
                    }


                    int anIndEq = 0;
                    double aSX=0 ;
                    double aSY=0 ;
                    for (int aDx=0 ; aDx<= mDegPol ; aDx++)
                    {
                       for (int aDy=0 ; aDy<= mDegPol - aDx ; aDy++)
                       {
                            double aMonXY = aVMx[aDx] * aVMy[aDy]; // X ^ Dx * Y ^ Dy
                            aSX += aDS[anIndEq++] * aMonXY;
                            aSY += aDS[anIndEq++] * aMonXY;
                       }
                    }
                    aTRx.oset(aPInd,aSX);
                    aTRy.oset(aPInd,aSY);
                }
            }
            Tiff_Im::CreateFromFonc
            (
                 aDir+"ResX-"+aName+".tif",
                 mSzRed,
                 aResX.in(),
                 GenIm::real4
            );
            Tiff_Im::CreateFromFonc
            (
                 aDir+"ResY-"+aName+".tif",
                 mSzRed,
                 aResY.in(),
                 GenIm::real4
            );
        }
    }
}

void cAccumResidu::ExportResXY(const Pt2di& aPt,Pt2dr& aRes)
{
    if (mSys)
    {
        bool aOk;
        Im1D_REAL8  aSol = mSys->Solve(&aOk);
        if (aOk)
        {
            double * aDS = aSol.data();

            std::vector<double> aVMx;
            std::vector<double> aVMy;
            aVMx.push_back(1.0);
            aVMy.push_back(1.0);

            Pt2dr aPFulRes = Pt2dr(aPt) * mResol;
            Pt2dr aSzN = mSz/2.0;
            double  aX = (aPFulRes.x-aSzN.x) / aSzN.x;
            double  aY = (aPFulRes.y-aSzN.y) / aSzN.y;

            for (int aD=0 ; aD< mDegPol ; aD++)
            {
              aVMx.push_back(aVMx.back() * aX);
              aVMy.push_back(aVMy.back() * aY);
            }

            int anIndEq = 0;
            double aSX=0 ;
            double aSY=0 ;
            for (int aDx=0 ; aDx<= mDegPol ; aDx++)
            {
               for (int aDy=0 ; aDy<= mDegPol - aDx ; aDy++)
               {
                    double aMonXY = aVMx[aDx] * aVMy[aDy];
                    aSX += aDS[anIndEq++] * aMonXY;
                    aSY += aDS[anIndEq++] * aMonXY;
               }
            }

            aRes.x = aSX;
            aRes.y = aSY;
        }
    }
}


void cAccumResidu::ExportResXY(TIm2D<REAL4,REAL8>* aTRx,TIm2D<REAL4,REAL8>* aTRy)
{
    if (mSys)
    {
        bool aOk;
        Im1D_REAL8  aSol = mSys->Solve(&aOk);
        if (aOk)
        {
            double * aDS = aSol.data();

            Pt2di aPInd;
            for (aPInd.x=0 ; aPInd.x<mSzRed.x ; aPInd.x++)
            {
                for (aPInd.y=0 ; aPInd.y<mSzRed.y ; aPInd.y++)
                {
                    Pt2dr aPFulRes = Pt2dr(aPInd) * mResol;
                    Pt2dr aSzN = mSz/2.0;
                    double  aX = (aPFulRes.x-aSzN.x) / aSzN.x;
                    double  aY = (aPFulRes.y-aSzN.y) / aSzN.y;

                    std::vector<double> aVMx; 
                    std::vector<double> aVMy;  
                    aVMx.push_back(1.0);
                    aVMy.push_back(1.0);
                    for (int aD=0 ; aD< mDegPol ; aD++)
                    {
                      aVMx.push_back(aVMx.back() * aX);
                      aVMy.push_back(aVMy.back() * aY);
                    }


                    int anIndEq = 0;
                    double aSX=0 ;
                    double aSY=0 ;
                    for (int aDx=0 ; aDx<= mDegPol ; aDx++)
                    {
                       for (int aDy=0 ; aDy<= mDegPol - aDx ; aDy++)
                       {
                            double aMonXY = aVMx[aDx] * aVMy[aDy]; 
                            aSX += aDS[anIndEq++] * aMonXY;
                            aSY += aDS[anIndEq++] * aMonXY;
                       }
                    }
                    aTRx->oset(aPInd,aSX);
                    aTRy->oset(aPInd,aSY);
                }
            }
        }
    }
}


void cAccumResidu::AccumInImage(const cInfoAccumRes & anInfo)
{
    Pt2dr aP = anInfo.mPt / mResol;
    mTPds.incr(aP,anInfo.mPds);
    mTMoySign.incr(aP,anInfo.mPds * anInfo.mResidu);

    if (!mOnlySign)
    {
         mTMoyAbs.incr(aP,anInfo.mPds*ElAbs(anInfo.mResidu));
    }
    if (mSys)
    {
        // 
        Pt2dr aSzN = mSz/2.0;
        Pt2dr aN = anInfo.mDir * Pt2dr(0,1);
        // Pour precision matrice, mieux vaut coordonnees normalisees
        double  aX = (anInfo.mPt.x-aSzN.x) / aSzN.x;
        double  aY = (anInfo.mPt.y-aSzN.y) / aSzN.y;

        std::vector<double> aVMx; // Monome Xn
        std::vector<double> aVMy; // Monome Xn
        aVMx.push_back(1.0);
        aVMy.push_back(1.0);
        for (int aD=0 ; aD< mDegPol ; aD++)
        {
          aVMx.push_back(aVMx.back() * aX);
          aVMy.push_back(aVMy.back() * aY);
        }

        std::vector<double> anEq;

        for (int aDx=0 ; aDx<= mDegPol ; aDx++)
        {
           for (int aDy=0 ; aDy<= mDegPol - aDx ; aDy++)
           {
                double aMonXY = aVMx[aDx] * aVMy[aDy]; // X ^ Dx * Y ^ Dy
                anEq.push_back(aMonXY* aN.x);
                anEq.push_back(aMonXY* aN.y);

//    std::cout << " eq " << aDx << " " << aDx << " " << aVMx[aDx] << " " << aVMy[aDy] << " " << aN.x << " " << aN.y << " " << anInfo.mDir <<"\n";
  //  std::cout << " eq " << aMonXY* aN.x << " " << aMonXY* aN.y << "\n";
           }
        }
        mSys->AddEquation(anInfo.mPds,VData(anEq),anInfo.mResidu);
    }
}
void cAccumResidu::Accum(const cInfoAccumRes & anInfo)
{
   mSomPds += anInfo.mPds;
   mNbInfo++;
   if (mNbInfo<NbMinCreateIm)
   {
      mLIAR.push_back(anInfo);
   }
   else if (mNbInfo==NbMinCreateIm)
   {
       mInit = true;
       mLIAR.push_back(anInfo);
       mPds = Im2D_REAL4(mSzRed.x,mSzRed.y,0.0);
       mTPds =  TIm2D<REAL4,REAL8>(mPds);
       mMoySign = Im2D_REAL4(mSzRed.x,mSzRed.y,0.0);
       mTMoySign =  TIm2D<REAL4,REAL8>(mMoySign);
       if (! mOnlySign)
       {
           mMoyAbs = Im2D_REAL4(mSzRed.x,mSzRed.y,0.0);
           mTMoyAbs =  TIm2D<REAL4,REAL8>(mMoyAbs);
       }
       if (mDegPol >=0)
       {
          mSys =  new L2SysSurResol((1+mDegPol)*(mDegPol+2));
       }
       for (std::list<cInfoAccumRes>::const_iterator itI=mLIAR.begin() ;itI!=mLIAR.end() ; itI++)
       {
           AccumInImage(*itI);
       }
       mLIAR.clear();
   }
   else
   {
        AccumInImage(anInfo);
   }
}

       // cAccumResidu(Pt2di aSz,double aRed,bool OnlySign);
void cAppliApero::AddOneInfoImageResidu
     (
         const cInfoAccumRes & anInfo,
         const std::string &   aName,
         Pt2di                 aSz,
         double                aSzRed,
         bool                  OnlySign,
         int                   aDegPol
     )
{

  if (aSzRed <=0) return;

  double aFactRed = dist8(aSz) / aSzRed;

  cAccumResidu * & aRef = mMapAR[aName];
  if (aRef==0)
  {
     aRef = new cAccumResidu(aSz,aFactRed,OnlySign,aDegPol);
  }

  aRef->Accum(anInfo);
}

void cAppliApero::AddInfoImageResidu
     (
         const Pt3dr &                 aPt,
         const  cNupletPtsHomologues & aNupl,
         const std::vector<cGenPoseCam *> aVP,
         const std::vector<double> &  aVpds
     )
{
  if ((!Param().UseExportImageResidu().IsInit()) || (! IsLastEtapeOfLastIter()))
     return;

  const cUseExportImageResidu & aUEIR = Param().UseExportImageResidu().Val();


  double aSomPds     = 0.0;
  double aSomPdsRes  = 0.0;

  for (int aK1=0 ; aK1< aNupl.NbPts() ; aK1++)
  {
      double aPds1 = aVpds[aK1];
      if (aPds1>0)
      {
         for (int aK2=0 ; aK2< aNupl.NbPts() ; aK2++)
         {
             double aPds2 = aVpds[aK2];
             if ((aK1!=aK2) && (aPds2>0))
             {
                const cBasicGeomCap3D * aCam1 = aVP[aK1]->GenCurCam();
                const cBasicGeomCap3D * aCam2 = aVP[aK2]->GenCurCam();
                Pt2di aSzCam1 = aCam1->SzBasicCapt3D();
                Pt2dr aP1 = aNupl.PK(aK1);
                Pt2dr aP2 = aNupl.PK(aK2);
                   
                Pt2dr aDir;
                double aRes = aCam1->EpipolarEcart(aP1,*aCam2,aP2,&aDir);
                cInfoAccumRes anInfo(aP1,ElMin(aPds1,aPds2),aRes,aDir);

                double aPds = aPds1 * aPds2;
                aSomPds    += aPds;
                aSomPdsRes += aPds * ElAbs(aRes);

                if (aK1<aK2)
                {
                    std::string aNamePair = "Pair-"+aVP[aK1]->Name() + "-" + aVP[aK2]->Name();
                    AddOneInfoImageResidu(anInfo,aNamePair,aSzCam1,aUEIR.SzByPair().Val(),true,-1);
                }
                AddOneInfoImageResidu(anInfo,"Pose-"+aVP[aK1]->Name(),aSzCam1,aUEIR.SzByPose().Val(),false,5);

                cCalibCam *  aCC1 = aVP[aK1]->CalibCam();
                if (aCC1)
                {
                   AddOneInfoImageResidu(anInfo,"Cam-"+aCC1->KeyId(),aSzCam1,aUEIR.SzByCam().Val(),false,10);
                }
             }
         }
      }
  }

  if (aSomPds)
  {
       mAVGR.AddResidu(aPt,aSomPdsRes/aSomPds);
  }
}

void cAppliApero::ExportImageResidu(const std::string & aName,const cAccumResidu & anAccum) 
{
    const cUseExportImageResidu & aUEIR = Param().UseExportImageResidu().Val();
    // ELISE_fp::MkDirRec(mDirExportImRes);

    const_cast<cAccumResidu &>(anAccum).Export(mDirExportImRes,aName,aUEIR,mFileExpImRes);
}

void cAppliApero::ExportImageResidu() 
{
  if ((!Param().UseExportImageResidu().IsInit()) )
     return;

  const cUseExportImageResidu & aUEIR = Param().UseExportImageResidu().Val();
  mDirExportImRes =  DC() + "Ori" + aUEIR.AeroExport() + "/ImResidu/";
  ELISE_fp::MkDirRec(mDirExportImRes);

  mFileExpImRes = FopenNN(mDirExportImRes+"StatRes.txt","w","cAppliApero::ExportImageResidu");

  for (auto it=mMapAR.begin() ; it!=mMapAR.end() ; it++)
  {
       ExportImageResidu(it->first,*(it->second));
  }
  fclose(mFileExpImRes);

   mAVGR.DoResidu(mDirExportImRes,aUEIR.NbMesByCase().Val());

}

//============================================


int PROF_UNDEF() { return -1; }


int cPoseCam::theCpt = 0;

int  TheDefProf2Init = 1000000;

void cPoseCam::SetNameCalib(const std::string & aNameC)
{
   mNameCalib = aNameC;
}

static int theNumCreate =0;


cStructRigidInit * cPoseCam::GetSRI(bool SVP) const 
{
   if (!SVP && (mSRI==0))
   {
       ELISE_ASSERT(false,"NO SRI");
   }
   return mSRI;
}
void  cPoseCam::SetSRI(cStructRigidInit * aSRI) 
{
    ELISE_ASSERT(mSRI==0,"Muliple SRI set");
    mSRI = aSRI;
}

cPreCompBloc * cPoseCam::GetPreCompBloc(bool SVP) const 
{
   if (!SVP && (mBlocCam==0))
   {
       ELISE_ASSERT(false,"NO Boc Cam");
   }
   return mBlocCam;
}
void  cPoseCam::SetPreCompBloc(cPreCompBloc * aBloc) 
{
    ELISE_ASSERT(mBlocCam==0,"Muliple Bloc set");
    mBlocCam = aBloc;
}

cPreCB1Pose * cPoseCam::GetPreCB1Pose(bool SVP) const 
{
   if (!SVP && (mPoseInBlocCam==0))
   {
       ELISE_ASSERT(false,"NO Pose in Boc Cam");
   }
   return mPoseInBlocCam;
}
void  cPoseCam::SetPreCB1Pose(cPreCB1Pose * aPoseInBloc) 
{
    ELISE_ASSERT(mPoseInBlocCam==0,"Muliple Bloc set");
    mPoseInBlocCam = aPoseInBloc;
}


void cPoseCam::SetOrInt(const cTplValGesInit<cSetOrientationInterne> & aTplSI)
{
  if (! aTplSI.IsInit()) return;

  const cSetOrientationInterne & aSOI = aTplSI.Val();

   cSetName *  aSelector = mAppli.ICNM()->KeyOrPatSelector(aSOI.PatternSel());

   if (! aSelector->IsSetIn(mName))
      return;

  std::string aNameFile =  mAppli.DC() + mAppli.ICNM()->Assoc1To1(aSOI.KeyFile(),mName,true);

   cAffinitePlane aXmlAff = StdGetObjFromFile<cAffinitePlane>
                            (
                                 aNameFile,
                                 StdGetFileXMLSpec("ParamChantierPhotogram.xml"),
                                 aSOI.Tag().Val(),
                                 "AffinitePlane"
                            );

    ElAffin2D  anAffM2C = Xml2EL(aXmlAff);

   if (! aSOI.M2C())
      anAffM2C = anAffM2C.inv();

   if (aSOI.AddToCur())
      mOrIntM2C= anAffM2C * mOrIntM2C ; 
   else
      mOrIntM2C= anAffM2C;

   //  Si on le fait avec les marques fiduciaires ca ecrase le reste

   mOrIntC2M = mOrIntM2C.inv();
}

cPoseCam * cPoseCam::DownCastPoseCamSVP() {return this;}
const cPoseCam * cPoseCam::DownCastPoseCamSVP() const {return this;}

cCalibCam *  cPoseCam::CalibCam() const {return mCalib;}
cGenPDVFormelle *   cPoseCam::PDVF()  {return  mCF;}
const cGenPDVFormelle *   cPoseCam::PDVF() const  {return  mCF;}


cPoseCam::cPoseCam
(
     cAppliApero & anAppli,
     const cPoseCameraInc & aPCI,
     const std::string & aNamePose,
     const std::string & aNameCalib,
     cPoseCam *             aPRat,
     cCompileAOI  *         aCompAOI
)   :
    cGenPoseCam (anAppli,aNamePose),
    // mAppli   (anAppli),
    mNameCalib (aNameCalib),
    // mName    (aNamePose),
    mCpt     (-1),
    mProf2Init (TheDefProf2Init),
    mPdsTmpMST (0.0),
    mPCI     (&aPCI),
	mCalib(NULL),
    mPoseRat (aPRat),
    mPoseInitMST1 (0),
    mPoseInitMST2 (0),
    mCamRF   (mPoseRat ? mPoseRat->mCF : 0),
	mCF(NULL),
	mRF(NULL),
    mAltiSol     (ALTISOL_UNDEF()),
    mProfondeur  (PROF_UNDEF()),
    mTime        (TIME_UNDEF()),
    mPrioSetAlPr (-1),
    mLastCP      (0),
    mCompAOI     (aCompAOI),
    mFirstBoxImSet (false),
    mImageLoaded (false),
    mBoxIm       (Pt2di(0,0),Pt2di(0,0)),
    mIm          (1,1),
    mTIm         (mIm),
    // mMasqH       (mAppli.MasqHom(aNamePose)),
    // mTMasqH      (mMasqH ? new  TIm2DBits<1>(*mMasqH) : 0),
    // mObsCentre   (0,0,0),
    mHasObsOnCentre (false),
    mHasObsOnVitesse (false),
    mLastItereHasUsedObsOnCentre (false),
    mNumBande     (0),
    mPrec         (NULL),
    mNext         (NULL),
    mNumCreate    (theNumCreate++),
    mNbPosOfInit  (-1),
    mFidExist     (false),
    mCamNonOrtho         (0),
    mEqOffsetGPS         (0),
    mSRI                 (nullptr),
    mBlocCam             (nullptr),
    mNumTimeBloc         (-1),
    mPoseInBlocCam       (nullptr),
    mUseRappelPose       (false),
    mRotURP              (ElRotation3D::Id)
{
    mPrec = this;
    mNext = this;

    std::string anIdGlob =  mAppli.GetNewIdIma(aNamePose);
    mCalib = mAppli.CalibFromName(aNameCalib,this);
    // mCF	=  mCalib->PIF().NewCam(cNameSpaceEqF::eRotLibre,ElRotation3D::Id,mCamRF,aNamePose,true,false,mAppli.HasEqDr());
    mCF	=  mCalib->PIF().NewCam(cNameSpaceEqF::eRotLibre,ElRotation3D::Id,mCamRF,anIdGlob,true,false,mAppli.HasEqDr());
    mCF->SetNameIm(aNamePose);
    mRF = &mCF->RF();

   SetOrInt(mAppli.Param().GlobOrInterne());
   SetOrInt(aPCI.OrInterne());

   std::pair<std::string,std::string> aPair = mAppli.ICNM()->Assoc2To1("Key-Assoc-STD-Orientation-Interne",mName,true);
   std::string aNamePtsCam = aPair.first;
   std::string aNamePtsIm  = aPair.second;

   if ((aPair.first!="NONE") && (ELISE_fp::exist_file(mAppli.OutputDirectory()+ aNamePtsIm)))
   {
       cMesureAppuiFlottant1Im aMesCam = mAppli.StdGetOneMAF(aNamePtsCam);

       // Correction du bug apparu lors du stage de Tibaut Sauter, lorsque les marques ont une origine tres
       // loin de zero, la box images est tres differente de par ex [0,0] [23x23], du coup le distorsion n'est pas coupe
       // au bon endroit
       Pt2dr aPMin(1e9,1e9);
       for 
       (
           std::list<cOneMesureAF1I>::iterator itAp = aMesCam.OneMesureAF1I().begin(); 
           itAp != aMesCam.OneMesureAF1I().end();
           itAp++
       )
       {
           aPMin = Inf(aPMin,itAp->PtIm());
       }

       for 
       (
           std::list<cOneMesureAF1I>::iterator itAp = aMesCam.OneMesureAF1I().begin(); 
           itAp != aMesCam.OneMesureAF1I().end();
           itAp++
       )
       {
           itAp->PtIm() = itAp->PtIm()-aPMin;
       }



       cMesureAppuiFlottant1Im aMesIm  = mAppli.StdGetOneMAF(aNamePtsIm);

       ElPackHomologue  aPack = PackFromCplAPF(aMesIm,aMesCam);
       ElAffin2D anAf = ElAffin2D::L2Fit(aPack);

       mOrIntM2C = anAf.inv();
       mFidExist = true;
   }

   mOrIntC2M = mOrIntM2C.inv();
   InitAvantCompens();

   if (aPCI.IdOffsetGPS().IsInit())
   {
       cAperoOffsetGPS * anOfs = mAppli.OffsetNNOfName(aPCI.IdOffsetGPS().Val());
       mEqOffsetGPS =  mAppli.SetEq().NewEqOffsetGPS(*mCF,*(anOfs->BaseUnk()));
   }

}

cEqOffsetGPS *   cPoseCam::EqOffsetGPS()
{
   return mEqOffsetGPS;
}

int cPoseCam::NumCreate() const
{
   return mNumCreate;
}


bool cPoseCam::AcceptPoint(const Pt2dr & aP) const
{


    if (mCalib-> CamInit().IsScanned())
    {
        Pt2dr aSz = Pt2dr(mCalib->SzIm());
        if ((aP.x<=0) || (aP.y <=0) || (aP.x>=aSz.x) || (aP.y>=aSz.y))
           return false;
    }

    return true;
}

bool cPoseCam::FidExist() const
{
   return mFidExist;
}


void cPoseCam::SetNumTimeBloc(int aNum)
{
   mNumTimeBloc = aNum;
}

int cPoseCam::DifBlocInf1(const cPoseCam & aPC) const
{
   if ((mNumTimeBloc==-1) || (aPC.mNumTimeBloc==-1)) return 1000;
   return ElAbs(mNumTimeBloc-aPC.mNumTimeBloc);
}



bool cPoseCam::IsInZoneU(const Pt2dr & aP) const
{
   return mCalib->IsInZoneU(aP);
}


Pt2di  cPoseCam::SzCalib() const
{
   return Calib()->SzIm();
}

void cPoseCam::SetLink(cPoseCam * aPrec,bool OK)
{
   mNumBande =  aPrec->mNumBande;
   if (OK) 
   {
      mPrec     = aPrec;
      aPrec->mNext = this;
   }
   else
   {
      mNumBande++;
   }
/*
    mNumBande =  aNumBande;
    mPrec     = aPrec;
    if (aPrec) aPrec->mNext = this;
*/
}


double  GuimbalAnalyse(const ElRotation3D & aR,bool show)
{
    // aR = aR.inv();
// aR  = ElRotation3D(Pt3dr(0,0,0),0,1.57,0);
    double aTeta01 = aR.teta01();
    double aTeta02 = aR.teta02();
    double aTeta12 = aR.teta12();

    double aEps = 1e-2;
    ElMatrix<double> aM1 = (ElRotation3D(aR.tr(),aTeta01+aEps,aTeta02,aTeta12).Mat()-aR.Mat())*(1/aEps);
    ElMatrix<double> aM2 = (ElRotation3D(aR.tr(),aTeta01,aTeta02+aEps,aTeta12).Mat()-aR.Mat())*(1/aEps);
    ElMatrix<double> aM3 = (ElRotation3D(aR.tr(),aTeta01,aTeta02,aTeta12+aEps).Mat()-aR.Mat())*(1/aEps);

    aM1 = aM1 * (1/sqrt(aM1.L2()));
    aM2 = aM2 * (1/sqrt(aM2.L2()));
    aM3 = aM3 * (1/sqrt(aM3.L2()));

    ElMatrix<double> aU1 = aM1;
    ElMatrix<double> aU2 = aM2 -aU1*aU1.scal(aM2);
    double aD2 = sqrt(aU2.L2());
    aU2 = aU2 *(1/aD2);


    ElMatrix<double> aU3 = aM3 -aU1*aU1.scal(aM3) -aU2*aU2.scal(aM3);

    double aD3 = sqrt(aU3.L2());

    if (show)
    {
        std::cout << aU2.scal(aU1) << " " << aU3.scal(aU1) << " " << aD2*aD3 << "\n";
    

        ShowMatr("M1",aM1);
        ShowMatr("M2",aM2);
        ShowMatr("M3",aM3);
        getchar();
    }
    return aD2 * aD3;
}

void cPoseCam::Trace() const
{
    if (! mAppli.TracePose(*this))
       return;

    std::cout   << mName ;

    if (RotIsInit())
    {
        ElRotation3D  aR = CurRot() ;
        std::cout <<  " C=" << aR.tr() ;
        std::cout <<  " Teta=" << aR.teta01() << " " << aR.teta02() <<  " " << aR.teta12()  ;
       
        if (mAppli.Param().TraceGimbalLock().Val())
        {
            std::cout << " GL-Score=" <<  GuimbalAnalyse(aR,false);
        }
    }
    std::cout << "\n";
}


double & cPoseCam::PdsTmpMST()
{
   return mPdsTmpMST;
}

void cPoseCam::Set0Prof2Init()
{
    mProf2Init = 0;
}

bool cPoseCam::ProfIsInit() const {return mProf2Init != TheDefProf2Init ;}


double cPoseCam::Time() const
{
  return mTime;
}

int cPoseCam::Prof2Init() const
{
    return mProf2Init;
}

void cPoseCam::UpdateHeriteProf2Init(const  cPoseCam & aC2)
{
    if (mProf2Init==TheDefProf2Init)
         mProf2Init = 0;

    ElSetMax(mProf2Init,1+aC2.mProf2Init);
}



cPoseCam * cPoseCam::PoseInitMST1()
{
   return mPoseInitMST1;
}
void cPoseCam::SetPoseInitMST1(cPoseCam * aPoseInitMST1)
{
   mPoseInitMST1 = aPoseInitMST1;
}


cPoseCam * cPoseCam::PoseInitMST2()
{
   return mPoseInitMST2;
}
void cPoseCam::SetPoseInitMST2(cPoseCam * aPoseInitMST2)
{
   mPoseInitMST2 = aPoseInitMST2;
}



std::string  cPoseCam::CalNameFromL(const cLiaisonsInit & aLI)
{
     std::string aName2 = aLI.NameCam();
     if (aLI.NameCamIsKeyCalc().Val())
     {
         aName2 =  mAppli.ICNM()->Assoc1To1(aName2,mName,aLI.KeyCalcIsIDir().Val());
     }

     return aName2;
}


cRotationFormelle & cPoseCam::RF()
{
   return *mRF;
}

ElRotation3D cPoseCam::CurRot() const
{
   return mRF->CurRot();
}



const std::string &  cPoseCam::NameCalib() const
{
   return mCalib->CCI().Name();
}



void cPoseCam::SetRattach(const std::string & aNameRat)
{
   if ((mPoseRat==0) || (mPoseRat != mAppli.PoseFromName(aNameRat)))
   {
       std::cout << mPoseRat << "\n";
       std::cout << "Pour rattacher " << mName << " a " << aNameRat << "\n";
       std::cout << "(Momentanement) : le ratachement doit etre prevu a l'initialisation";
       ELISE_ASSERT(false,"Rattachement impossible");
   }
}

void    cPoseCam::VirtualInitAvantCompens()
{
    mLastItereHasUsedObsOnCentre = false;
    AssertHasNotCamNonOrtho();
}

const CamStenope *  cPoseCam::CurCam() const
{
   if (mCamNonOrtho) return mCamNonOrtho;
   return  mCF->CameraCourante() ;
}
CamStenope *  cPoseCam::NC_CurCam() 
{
   if (mCamNonOrtho) return mCamNonOrtho;
   return  mCF->NC_CameraCourante() ;
}

const cBasicGeomCap3D * cPoseCam::GenCurCam () const { return CurCam(); }
cBasicGeomCap3D * cPoseCam::GenCurCam () { return NC_CurCam(); }





CamStenope *  cPoseCam::DupCurCam() const 
{
   if (mCamNonOrtho) return mCamNonOrtho;
   return  mCF->DuplicataCameraCourante() ;
}



double cPoseCam::GetProfDyn(int & Ok) const
{
    Ok = true;

    if (PMoyIsInit())
    {
        Ok =1 ;
        return ProfMoyHarmonik();
    }
    if (mLastEstimProfIsInit)
    {
       Ok =2 ;
       return mLasEstimtProf;
    }

    if (mProfondeur != PROF_UNDEF())
    {
       Ok = 3 ;
       return mProfondeur;
    }


    Ok = 0;
    return 0;
}






void cPoseCam::ActiveContrainte(bool Stricte)
{
    mAppli.SetEq().AddContrainte(mRF->StdContraintes(),Stricte);
}

void ShowResMepRelCoplan(cResMepRelCoplan aRMRC)
{
    std::vector<cElemMepRelCoplan> aV =  aRMRC.VElOk();
    std::cout << "  NB SOL = " << aV.size() << "\n";
    for (int aKS=0; aKS<int(aV.size()) ; aKS++)
    {
        cElemMepRelCoplan aS = aV[aKS];
        std::cout << "ANGLE " << aKS << " " << aS.AngTot() << "\n";
        ElRotation3D aR = aS.Rot();
        std::cout << aR.ImRecAff(Pt3dr(0,0,0)) << " "
                  << aR.teta01() << " "
                  << aR.teta02() << " "
                  << aR.teta12() << " "
                  << "\n";
    }
}


/*
void ShowPrecisionLiaison(ElPackHomologue  aPack,ElRotation3D aR1,ElRotation3D aR2)
{
   CamStenopeIdeale aC1(1.0,Pt2dr(0.0,0.0));
   aC1.SetOrientation(aR1.inv());
   CamStenopeIdeale aC2(1.0,Pt2dr(0.0,0.0));
   aC2.SetOrientation(aR2.inv());

   double aSD=0;
   double aSP=0;
   for (ElPackHomologue::iterator itP= aPack.begin(); itP!=aPack.end() ; itP++)
   {
        ElSeg3D aS1 = aC1.F2toRayonR3(itP->P1());
        ElSeg3D aS2 = aC2.F2toRayonR3(itP->P2());

	Pt3dr aPTer = aS1.PseudoInter(aS2);

	Pt2dr aQ1 = aC1.R3toF2(aPTer);
	Pt2dr aQ2 = aC2.R3toF2(aPTer);

	std::cout << aQ1 << aQ2 << euclid(aQ1,itP->P1()) << " " <<  euclid(aQ2,itP->P2())  << "\n";
	aSP += 2;
	aSD += euclid(aQ1,itP->P1()) + euclid(aQ2,itP->P2());

   }

   std::cout << "DMOYENNE " << aSD/aSP * 1e4 << "/10000\n";
}
*/
 

ElMatrix<double> RFrom3P(const Pt3dr & aP1, const Pt3dr & aP2, const Pt3dr & aP3)
{
    Pt3dr aU = aP2-aP1;
    aU = aU / euclid(aU);
    Pt3dr aW = aU ^(aP3-aP1);
    aW = aW / euclid(aW);
    Pt3dr aV =  aW ^aU;
    return ElMatrix<double>::Rotation(aU,aV,aW);
}

void  TTTT (ElPackHomologue & aPack)
{
//
   int aCpt=0;
   Pt2dr aS1(0,0);
   Pt2dr aS2(0,0);
   double aSP=0;
   std::cout << " ccccccccccc " << aPack.size() << "\n";
   for 
   (
       ElPackHomologue::iterator itP=aPack.begin() ;
       itP!=aPack.end();
       itP++
   )
   {
       aS1 = aS1 + itP->P1() * itP->Pds();
       aS2 = aS2 + itP->P2() * itP->Pds();
       aSP += itP->Pds();
       aCpt++;
       if (euclid(itP->P1()) > 1e5)
          std::cout << itP->P1() << itP->P2() << itP->Pds()  << "\n";
   }
   std::cout << "SOMES ::  "<< (aS1/aSP) << " " << (aS2/aSP) << "\n";
}

void  TTTT (cElImPackHom & anIP)
{
    ElPackHomologue aPack = anIP.ToPackH(0);
    TTTT(aPack);
}

void  cPoseCam::TenteInitAltiProf
      (
           int    aPrio,
           double anAlti,
	   double aProf
      )
{
   if ((aProf != PROF_UNDEF()) && (aPrio > mPrioSetAlPr))
   {
         mProfondeur = aProf;
         mAltiSol = anAlti;
	 mPrioSetAlPr = aPrio;
   }
}
extern bool AllowUnsortedVarIn_SetMappingCur;

cPoseCam * cPoseCam::Alloc
           (
               cAppliApero & anAppli,
               const cPoseCameraInc & aPCI,
               const std::string & aNamePose,
               const std::string & aNameCalib,
               cCompileAOI * aCompAOI
           )
{

    cPoseCam * aPRat=0;

    if (aPCI.PosesDeRattachement().IsInit())
    {
        // En fait le tri sur les variables n'avait pas d'effet puisque la fonction est symetrique !!
        AllowUnsortedVarIn_SetMappingCur = true;
        aPRat = anAppli.PoseCSFromNameGen
                (
                    aPCI.PosesDeRattachement().Val(),
                    aPCI.NoErroOnRat().Val()
                );

    }

    cPoseCam * aRes = new cPoseCam(anAppli,aPCI,aNamePose,aNameCalib,aPRat,aCompAOI);
    return aRes;
}

int   cPoseCam::NbPosOfInit(int aDef)
{
   return (mNbPosOfInit>=0) ? mNbPosOfInit : aDef;
}

void  cPoseCam::SetNbPosOfInit(int aNbPosOfInit)
{
   mNbPosOfInit = aNbPosOfInit;
}


void cPoseCam::DoInitIfNow()
{

    mPreInit = true;


    mAppli.AddRotPreInit();
    if (mPCI->InitNow().Val())
    {
       InitRot();
    }
}

/*
void  cPoseCam::AddLink(cPoseCam * aPC)
{
   if (! BoolFind(mVPLinked,aPC))
      mVPLinked.push_back(aPC);
}
*/



void cPoseCam::PCSetCurRot(const ElRotation3D & aRot)
{
    ELISE_ASSERT(!mUseRappelPose,"Internam Error, probaly bascule + UseRappelPose");


    AssertHasNotCamNonOrtho();
    mCF->SetCurRot(aRot,aRot);
}


void  cPoseCam::SetBascRig(const cSolBasculeRig & aSBR)
{
    PCSetCurRot(aSBR.TransformOriC2M(CurRot()));

    Pt3dr aP;
    if (mSomPM)
    {
       aP = mPMoy / mSomPM;
       mSomPM = 0;
    }
    else
    {
        const CamStenope *  aCS = CurCam() ;
        if (mProfondeur == PROF_UNDEF())
        {
            std::cout << "Warn : NoProfInBasc For camera =" << mName << "\n";
            // ELISE_ASSERT( false,"No Profondeur in cPoseCam::SetBascRig");
            return ;
        }

        aP =  aCS->ImEtProf2Terrain(aCS->Sz()/2.0,mProfondeur);
        aP =  aSBR(aP);
    }



    const CamStenope *  aCS = CurCam() ;
    mAltiSol = aP.z;
    mProfondeur = aCS->ProfondeurDeChamps(aP);
    mLasEstimtProf = mProfondeur;
}


void cPoseCam::VirtualAddPMoy
     (
           const Pt2dr & aPIm,
           const Pt3dr & aP,
           int aKPoseThis,
           const std::vector<double> * aVPds,
           const std::vector<cGenPoseCam*>*
     )
{
  if (mAppli.UsePdsCalib())
      mCalib->AddPds(aPIm,(*aVPds)[aKPoseThis]);

}


void cPoseCam::BeforeCompens()
{
   mRF->ReactuFcteurRapCoU();
}

void cPoseCam::InitCpt()
{
    if (mCpt<0)
    {
       mCpt  = theCpt++;
       mAppli.AddPoseInit(mCpt,this);
    }
}

bool cPoseCam::HasObsOnCentre() const
{
   return mHasObsOnCentre;
}

bool  cPoseCam::LastItereHasUsedObsOnCentre() const
{
    return mLastItereHasUsedObsOnCentre;
}

void cPoseCam::AssertHasObsCentre() const
{
   if (!mHasObsOnCentre)
   {
       std::cout << "Name Pose = " << mName << "\n";
       ELISE_ASSERT
       (
             false,
             "Observation on centre (GPS) has no been associated to camera"
       );
   }
}

bool cPoseCam::HasObsOnVitesse() const
{
   return mHasObsOnVitesse;
}

void cPoseCam::AssertHasObsVitesse() const
{
    AssertHasObsCentre();
    if (! mObsCentre.mVitesse.IsInit())
    {
       std::cout << "Name Pose = " << mName << "\n";
       ELISE_ASSERT
       (
             false,
             "No speed has  been associated to camera"
       );
    }
}

const Pt3dr  & cPoseCam::ObsCentre() const
{
   AssertHasObsCentre();
 
   return mObsCentre.mCentre;
}

Pt3dr   cPoseCam::Vitesse() const
{
    AssertHasObsVitesse();
    return mObsCentre.mVitesse.Val();
}


bool cPoseCam::IsId(const ElAffin2D & anAff) const
{
    Pt2dr aSz =  Pt2dr(mCalib->SzIm());
    Box2dr aBox(Pt2dr(0,0),aSz);
    Pt2dr aCoins[4];
    aBox.Corners(aCoins);
    double aDiag = euclid(aSz);
    // double aDiag2 = 0;

    double aDMax = 0.0;

    for (int aK=0 ; aK<4 ; aK++)
    {
         Pt2dr aP1 = aCoins[aK];
         Pt2dr aP2 = anAff(aP1);
         double aD = euclid(aP1,aP2) / aDiag;
         ElSetMax(aDMax,aD);
    }

    return aDMax < 1e-3;
}
/*
*/

double DistanceMatr(const ElRotation3D & aR1,const ElRotation3D & aR2)
{
   ElMatrix<double> aMatr = aR1.inv().Mat() * aR2.Mat(); 
   ElMatrix<double> aId(3,true);

   return aId.L2(aMatr);
}

/*class cTransfo3DIdent : public cTransfo3D
{
     public :
          std::vector<Pt3dr> Src2Cibl(const std::vector<Pt3dr> & aSrc) const {return aSrc;}

};*/


extern bool DebugOFPA;
extern int aCPTOkOFA ;
extern int aCPTNotOkOFA ;

void   cPoseCam::InitRot()
{
   const cLiaisonsInit * theLiasInit = 0;
   mNumInit =  mAppli.NbRotInit();
   if (mAppli.ShowMes())
      std::cout << "NUM " << mNumInit << " FOR " << mName<< "\n";
/*
{

if (mNumInit==90)
{
   BugFE=true;
}
else
{
   std::cout << "END BUG FE \n";
   BugFE=false;
}
 BugFE=true;
}
*/


   if (mPCI->IdBDCentre().IsInit())
   {

//  std::cout << "CCCcccC : " << mName <<  " " << mAppli.HasObsCentre(mPCI->IdBDCentre().Val(),mName) << "\n";
      if (mAppli.HasObsCentre(mPCI->IdBDCentre().Val(),mName))
      {
          mObsCentre = *( mAppli.ObsCentre(mPCI->IdBDCentre().Val(),mName).mVals );
          mHasObsOnCentre = mObsCentre.mHasObsC;
          mHasObsOnVitesse = mHasObsOnCentre && mObsCentre.mVitFiable && mObsCentre.mVitesse.IsInit();

//   std::cout << "NameBDDCCC " << mName << " HasC " << mHasObsOnCentre << "\n";
      }
   }

  
    // std::cout << mName << "::Prof=" << mProf2Init << "\n";
// std::cout <<  "Init Pose " << aNamePose << "\n";

   InitCpt();


    double aProfI = mPCI->ProfSceneImage().ValWithDef(mAppli.Param().ProfSceneChantier().Val());
    ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
    std::string aNZPl ="";
    double aDZPl = -1;
    double aDZPl2 = -1;
    cPoseCam * aCam2PL = 0;
    double aLambdaRot=1;

    CamStenope* aCS1 =    (mCalib->PIF().CurPIF());

    double  aProfPose = -1;
    double  anAltiSol = ALTISOL_UNDEF();


   bool isMST = mPCI->MEP_SPEC_MST().IsInit();
   if (isMST)
   {
       ELISE_ASSERT
       (
           mPCI->PoseFromLiaisons().IsInit(),
           "MST requires PoseFromLiaisons"
       );
   }

    bool isAPC =  mAppli.Param().IsAperiCloud().Val();
    bool isForISec =  mAppli.Param().IsChoixImSec().Val();
    bool initFromBD = false;


    if  (mSRI)
    {
        ElRotation3D aR1 = mSRI->mCMere->CurRot()  ;  // R1 to M
        ElRotation3D aL1Bis = aR1 * mSRI->mR0m1L0;
        aRot = aL1Bis;
    }
    else if (mPCI->PosId().IsInit())
    {
         aRot =  ElRotation3D(Pt3dr(0,0,0),0,0,-PI);
    }
    else if (mPCI->PosFromBlockRigid().IsInit())
    {
         mAppli.PreInitBloc(mPCI->PosFromBlockRigid().Val());
         aRot = GetPreCB1Pose(false)->mRot;
    }
    else if(mPCI->PosFromBDOrient().IsInit())
    {
        initFromBD = true;
	const std::string & anId = mPCI->PosFromBDOrient().Val();
        aRot =mAppli.Orient(anId,mName);
	cObserv1Im<cTypeEnglob_Orient>  &  anObs = mAppli.ObsOrient(anId,mName);

         bool Ok1 = IsId(anObs.mOrIntC2M);
         bool Ok2 = IsId(anObs.mOrIntC2M * mOrIntC2M.inv());
         ELISE_ASSERT
         (
             Ok1 || Ok2,
             "Specicied Internal Orientation is incompatible with Fiducial marks"
         );
         

	aProfPose = anObs.mProfondeur;
	anAltiSol =  anObs.mAltiSol;
        mTime     =  anObs.mTime;
    }
    else if (mPCI->PoseInitFromReperePlan().IsInit())
    {
       cPoseInitFromReperePlan aPP = mPCI->PoseInitFromReperePlan().Val();
       ElPackHomologue aPack;
       std::string  aNamePose2 = aPP.NameCam();
       std::string  aNameCal2 = mAppli.NameCalOfPose(aNamePose2);

       std::string aTestNameFile = mAppli.DC()+aPP.IdBD();
       if (ELISE_fp::exist_file(aTestNameFile))
       {
            ELISE_ASSERT(false,"Obsolet Init Form repere plan");
            // Onsolete, pas cohrent avec orient interen
            // aPack = ElPackHomologue::- FromFile(aTestNameFile);
       }
       else 
           mAppli.InitPack(aPP.IdBD(),aPack,mName,aPP.NameCam());
       CamStenope & aCS2 = mAppli.CalibFromName(aNameCal2,0)->CamInit();

// TTTT(aPack);



       aPack = aCS1->F2toPtDirRayonL3(aPack,&aCS2);
       cResMepRelCoplan aRMRC = aPack.MepRelCoplan(1.0,aPP.L2EstimPlan().Val());
       cElemMepRelCoplan & aSP = aRMRC.RefBestSol();

       // aM1 aM2 aM3 -> coordonnees monde, specifiees par l'utilisateur
       // aC1 aC2 aC3 -> coordonnees monde

       Pt3dr aM1,aM2,aM3;
       Pt2dr aIm1,aIm2,aIm3;
       Pt3dr aDirPl;
       bool aModeDir = false;
       if (aPP.MesurePIFRP().IsInit())
       {
           aM1 = aPP.Ap1().Ter();
           aM2 = aPP.Ap2().Ter();
           aM3 = aPP.Ap3().Ter();
           aIm1 = aCS1->F2toPtDirRayonL3(aPP.Ap1().Im());
           aIm2 = aCS1->F2toPtDirRayonL3(aPP.Ap2().Im());
           aIm3 = aCS1->F2toPtDirRayonL3(aPP.Ap3().Im());
       }
       else if (aPP.DirPlan().IsInit())
       {
           aModeDir = true;
           aDirPl = aPP.DirPlan().Val();
           aM1 = Pt3dr(0,0,0);

           Pt3dr aW = vunit(aPP.DirPlan().Val());
           aM2 = OneDirOrtho(aW);
           aM3 = aW ^ aM2;
           
           // aIm1 = aCS1.Sz() /2.0;
           // aIm2 = aIm1 + Pt2dr(1.0,0.0);
           // aIm3 = aIm1 + Pt2dr(0.0,1.0);
           aIm1 = Pt2dr(0,0);
           aIm2 = aIm1 + Pt2dr(0.1,0.0);
           aIm3 = aIm1 + Pt2dr(0.0,0.1);
       }



       Pt3dr aC1 = aSP.ImCam1(aIm1);
       Pt3dr aC2 = aSP.ImCam1(aIm2);
       Pt3dr aC3 = aSP.ImCam1(aIm3);

       double aFMult=0;


       if (aPP.DEuclidPlan().IsInit())
           aFMult = aPP.DEuclidPlan().Val() / aSP.DistanceEuclid();
       else
           aFMult = euclid(aM2-aM1)/euclid(aC2-aC1);
        aDZPl = aSP.DPlan() * aFMult;
        aNZPl = aPP.OnZonePlane();

        aC1 = aC1 * aFMult;
        aC2 = aC2 * aFMult;
        aC3 = aC3 * aFMult;

	ElMatrix<double> aMatrM = RFrom3P(aM1,aM2,aM3);
	ElMatrix<double> aMatrC = RFrom3P(aC1,aC2,aC3);
        ElMatrix<double>  aMatrC2M = aMatrM * gaussj(aMatrC);

	Pt3dr aTr = aM1 - aMatrC2M * aC1;

        if (aModeDir)
        {
            Pt3dr aC1 = aMatrC2M*Pt3dr(1,0,0);
            Pt3dr aC2 = aMatrC2M*Pt3dr(0,1,0);
            Pt3dr aC3 = aMatrC2M*Pt3dr(0,0,1);
            double aScal = scal(aC3,aDirPl);

            // std::cout << "CSAL = " << aScal << "\n";
            // std::cout << aMatrC2M*Pt3dr(1,0,0) << aMatrC2M*Pt3dr(0,1,0) <<  aMatrC2M*Pt3dr(0,0,1) << "\n";

            if (aScal <0)
            {
                 aMatrC2M = ElMatrix<double>::Rotation(aC1,aC2*-1,aC3*-1);
            }
            // std::cout << aMatrC2M*Pt3dr(1,0,0) << aMatrC2M*Pt3dr(0,1,0) <<  aMatrC2M*Pt3dr(0,0,1) << "\n";
        }

	aRot = ElRotation3D(aTr,aMatrC2M,true);

        anAltiSol = aM1.z;
        aProfPose = euclid(aC1);
	mAppli.AddPlan(aNZPl,aM1,aM2,aM3,false);
	//Pt3dr  uM = 
    }
    else if(mPCI->PosFromBDAppuis().IsInit())
    {
         // DebugOFPA = (mName=="Im00523.png");
         if (mAppli.ShowMes() || DebugOFPA)
            std::cout << "InitByAppuis " << mName  << "\n\n";
         const cPosFromBDAppuis & aP…