///
  /// @primary authors: S.Gorbunov; I.Kisel
  /// @        authors: H.Pabst et al. (Intel); M.Zyzak; I.Kulakov
  ///
#include <iostream>
#include <fstream>
#include <math.h>
#include "fit.h"

using namespace std;

void readInput( int nThreads, int nTracksPerThread ){

    fstream FileGeo, FileTracks, FileMCTracks;

    FileGeo.open( "data/geo.dat", std::ios::in );
    if ( ! FileGeo.is_open() )
        cout<<"Open geo.dat fails"<<endl;
    FileTracks.open( "data/tracks.dat", std::ios::in );
    if ( ! FileTracks.is_open() )
        cout<<"Open track.dat fails"<<endl;
    FileMCTracks.open( "data/mctracksin.dat", std::ios::in );
    if ( ! FileMCTracks.is_open() )
        cout<<"Open mctracksin.dat fails"<<endl;
    {
        ftype H[3][3];
        ftype Hz[3];
        for( int i=0; i<3; i++){
            ftype Bx, By, Bz, z;
            FileGeo >> z >> Bx >> By >> Bz;
            Hz[i] = z; H[i][0] = Bx;   H[i][1] = By; H[i][2] = Bz; 
 //           cout<<"Input Magnetic field:"<<z<<" "<<Bx<<" "<<By<<" "<<Bz<<endl;
        }
        magField.set( H[0], Hz[0], H[1], Hz[1], H[2], Hz[2] );
    }
    int nStations;
    FileGeo >> nStations;
//    cout<<"Input "<<nStations<<" Stations:"<<endl;
    vStations.init( nStations );
    for( int i=0; i<nStations; i++ ){
      int ist;    
      FileGeo >> ist;
      if( ist!=i ) { cout << "Corrupted geo.dat file" << endl; exit(1); }
      FileGeo >> vStations.z[i] >> vStations.thick[i] >> vStations.RL[i] >> vStations.UInfo.sigma2[i] >> vStations.VInfo.sigma2[i];

      vStations.UInfo.sigma2[i] *= vStations.UInfo.sigma2[i];
      vStations.VInfo.sigma2[i] *= vStations.VInfo.sigma2[i];
      vStations.UInfo.sigma216[i] = vStations.UInfo.sigma2[i]*16.f;
      vStations.VInfo.sigma216[i] = vStations.VInfo.sigma2[i]*16.f;
    
      if( i < 2 ){ // mvd //TODO From Geo File!!!
        vStations.UInfo.cos_phi[i] = 1.f;
        vStations.UInfo.sin_phi[i] = 0.f;
        vStations.VInfo.cos_phi[i] = 0.f;
        vStations.VInfo.sin_phi[i] = 1.f;
      }
      else{
        vStations.UInfo.cos_phi[i] = 1.f;           // 0 degree
        vStations.UInfo.sin_phi[i] = 0.f;
        vStations.VInfo.cos_phi[i] = 0.9659258244f; // 15 degree
        vStations.VInfo.sin_phi[i] = 0.2588190521f;
      }

      ftype idet = vStations.UInfo.cos_phi[i]*vStations.VInfo.sin_phi[i] - vStations.UInfo.sin_phi[i]*vStations.VInfo.cos_phi[i];
      idet = 1.f/(idet*idet);
      vStations.XYInfo.C00[i] = ( vStations.VInfo.sin_phi[i]*vStations.VInfo.sin_phi[i]*vStations.UInfo.sigma2[i] +
                                  vStations.UInfo.sin_phi[i]*vStations.UInfo.sin_phi[i]*vStations.VInfo.sigma2[i] )*idet;
      vStations.XYInfo.C10[i] =-( vStations.VInfo.sin_phi[i]*vStations.VInfo.cos_phi[i]*vStations.UInfo.sigma2[i] +
                                  vStations.UInfo.sin_phi[i]*vStations.UInfo.cos_phi[i]*vStations.VInfo.sigma2[i] )*idet;
      vStations.XYInfo.C11[i] = ( vStations.VInfo.cos_phi[i]*vStations.VInfo.cos_phi[i]*vStations.UInfo.sigma2[i] +
                                  vStations.UInfo.cos_phi[i]*vStations.UInfo.cos_phi[i]*vStations.VInfo.sigma2[i] )*idet;

    
        // vStations.zhit[i] = vStations.z[i] - vStations.thick[i]/2.f;
      vStations.zhit[i] = vStations.z[i];
      vStations.RadThick[i] = vStations.thick[i]/vStations.RL[i];
      vStations.logRadThick[i] = log(vStations.RadThick[i]);
        
      int N=0;
      FileGeo >> N;
//      cout << N << " field coeff." <<endl;
      ftype mx[MaxNFieldCoeff], my[MaxNFieldCoeff], mz[MaxNFieldCoeff];
      for( int j=0; j<N; j++ ) FileGeo >> mx[j];
      for( int j=0; j<N; j++ ) FileGeo >> my[j];
      for( int j=0; j<N; j++ ) FileGeo >> mz[j];
      vStations.initMap( i, mx, my, mz );
    }
    {
        // field intergals with respect to Last station
      ftype z0  = vStations.z[nStations-1];
      ftype sy = 0., Sy = 0.;
      for( int i=nStations-1; i>=0; i-- ){
        ftype dz = vStations.z[i]-z0;
        ftype Hy = vStations.mapY[i][0];
        Sy += dz*sy + dz*dz*Hy/2.f;
        sy += dz*Hy;
        vStations.SyL[i] = Sy;
        z0 = vStations.z[i];
      }
    }
    {
        // field intergals with respect to First station
      ftype z0  = vStations.z[0];
      ftype sy = 0., Sy = 0.;
      for( int i=0; i<nStations; i++ ){
        ftype dz = vStations.z[i]-z0;
        ftype Hy = vStations.mapY[i][0];
        Sy += dz*sy + dz*dz*Hy/2.f;
        sy += dz*Hy;
        vStations.SyF[i] = Sy;
        z0 = vStations.z[i];
      }
    }
    FileGeo.close();

//    cout << "Number of stations = " << nStations << endl;

    vTracks.init( nStations, maxNTracks );
    int nTracks = 0;
    
    int TrackIndex[maxNTracks];
    while( !FileTracks.eof() ){

        int itr;
        FileTracks>>itr;
        //if( itr!=nTracks ) break;
        //if( nTracks>=maxNTracks ) break;
        if( nTracks>=nTracksPerThread ) break;

        ftype MC_para[7];
        int lNHits = 0;
        FileTracks >> MC_para[0] >> MC_para[1] >> MC_para[2]
        >> MC_para[3] >> MC_para[4] >> MC_para[5] >> MC_para[6]
        >> lNHits;
        int *lIsta = (int*)arbb::aligned_malloc(lNHits*sizeof(int));
        ftype *lHitsX = (ftype*)arbb::aligned_malloc(lNHits*sizeof(ftype));
        ftype *lHitsY = (ftype*)arbb::aligned_malloc(lNHits*sizeof(ftype));
        for( int i=0; i<lNHits; i++ ){
            FileTracks >> lIsta[i];
            FileTracks >> lHitsX[i] >> lHitsY[i];
        }
        if( lNHits == nStations ){
            for(int iTh=0; iTh<nThreads; ++iTh)
            {
              vTracks.setMC( nTracksPerThread * iTh + nTracks, MC_para );
              vTracks.setHits( nTracksPerThread * iTh + nTracks, lIsta, lHitsX, lHitsY );
            }
            TrackIndex[nTracks] = itr;
            nTracks++;
        }
        arbb::aligned_free( lIsta );
        arbb::aligned_free( lHitsX );
        arbb::aligned_free( lHitsY );
    }
    FileTracks.close();

    int NMCTracks=0;
    int iPoint = 0;
    while( !FileMCTracks.eof() ){
    
      int itr;
      FileMCTracks>>itr;
        //if( itr!=NTracks ) break;
      // if( NMCTracks>=MaxNTracks ) break;
      if( NMCTracks>=nTracksPerThread ) break;
      MCTracks &mc = vMCTracks[NMCTracks];
      float temp;
      int NMCPoints;
      FileMCTracks >> temp   >> temp  >> temp
                   >> temp >> temp >> temp >> temp
                   >> NMCPoints;
      mc.NMCPoints = NMCPoints;
      for( int i=0; i<NMCPoints; i++ ){
        int ist;
        FileMCTracks >> ist;
        mc.vPoints[i].ista = ist;
        FileMCTracks >> mc.vPoints[i].x >> mc.vPoints[i].y>>mc.vPoints[i].z>>mc.vPoints[i].px>>mc.vPoints[i].py>>mc.vPoints[i].pz;

#ifdef DAF
          //   if(ist == 5) vTracks[NMCTracks].vHits[ist].x = mc.vPoints[i].x + sqrt(vStations[ist].UInfo.sigma2[0]) * 20.f; // shift for DAF
#endif
      }
    
      iPoint = 0; // compare paraments at the first station
         // iPoint = NMCPoints-1;
      vTracks.MC_x[NMCTracks] = mc.vPoints[iPoint].x;
      vTracks.MC_y[NMCTracks] = mc.vPoints[iPoint].y;
      vTracks.MC_z[NMCTracks] = mc.vPoints[iPoint].z;
      vTracks.MC_px[NMCTracks] = mc.vPoints[iPoint].px;
      vTracks.MC_py[NMCTracks] = mc.vPoints[iPoint].py;
      vTracks.MC_pz[NMCTracks] = mc.vPoints[iPoint].pz;

      if( itr == TrackIndex[NMCTracks]) NMCTracks++;
    }
    FileMCTracks.close();

    vTracks.nTracks = nTracks * nThreads;

    
    
    //cout << "Number of tracks = " << nTracks << endl;
}

void reset()
{
    int nt = vTracks.nTracks;
    int ns = vTracks.nStations;

    int i, j;
    for(i = 0; i < nt; i ++){
        for(j = 0; j < ns; j ++){
            vTracks.hitsX2[j * nt + i] = vTracks.hitsX[i][j];
            vTracks.hitsY2[j * nt + i] = vTracks.hitsY[i][j];
        }
    }
}

void writeOutput(ftype (*Tt)[6], ftype (*Cc)[15]){
  
    // if (vTracks.nTracks > static_cast<unsigned int>(nTracksPerThread)) return; // don't write output in the multicore mode
  
    fstream Out;

    Out.open( "data/fit.dat", ios::out );
    
    Out << "Fitter" << endl;
    Out << nTracksPerThread << endl;
    
    for( int it=0, itt=0; itt<nTracksPerThread; itt++ ){
    //    Track &t = vTracks[itt];
    //    MCTrack &mc = vMCTracks[itt];

        // convert matrix 
        ftype C[15];    
        {
//            ftype *tC = (ftype *) vTracks.C[itt];
            for( int i=0, n=0; i<5; i++)
                for( int j=0; j<=i; j++, n++ ){
                    C[n]=0;
                    //for(int k=0; k<5;k++) C[n]+=tC[i*5+k] * tC[j*5+k];
                    C[n] = Cc[itt][n];
                }
        }

        //bool ok = 1;
        //for( int i=0; i<6; i++ ){
        //    ok = ok && finite(t.T[i]);
        //}
        //for( int i=0; i<15; i++ ) ok = ok && finite(C[i]);

        //if(!ok){ cout<<" infinite "<<endl; continue; }

        Out <<it<<endl<<"   "
            << " " << vTracks.MC_x[itt]  << " " << vTracks.MC_y[itt]  << " " << vTracks.MC_z[itt] 
            << " " << vTracks.MC_px[itt] << " " << vTracks.MC_py[itt] << " " << vTracks.MC_pz[itt] 
            << " " << vTracks.MC_q[itt]<<endl;
        Out<<"   ";
        for( int i=0; i<6; i++ ) Out<< " " <<Tt[itt][i];//vTracks.T[itt][i];
        Out<<endl<<"   ";
        for( int i=0; i<15; i++ ) Out<< " " <<C[i];
        Out<<endl;

        it++;
    }
    Out.close();
}