#include <CellAtlasPrimordium.hpp>

#include <CellAtlas.hpp>
#include <Progress.hpp>
#include <Geometry.hpp>



#include <CellAtlasUtils.hpp>

using namespace std;

namespace mgx 
{

  // this process is similar to analyze cells 3d, however a 2D bezier is used
bool AnalyzeLeafPrimSurf::run(const Stack *s1, Mesh *m1, primDataStructure& pr)
  {
    // create beziermaps

  CellAtlasAttr *cellAtlasAttr;
  cellAtlasAttr = &m1->attributes().attrMap<int, CellAtlasData>("Cell Atlas 3D Data");

  CellAtlasConfigAttr *cellAtlasConfigAttr;
  cellAtlasConfigAttr = &m1->attributes().attrMap<int, CellAtlasConfig>("Cell Atlas 3D Config");


    // meshes
    const vvGraph& surfaceMesh = m1->graph();

    VVGraphVec cellVertex;
    VtxIntMap vertexCell;

    triVector triVec;
    RootCellAnalyzing rca(cellAtlasAttr, cellAtlasConfigAttr);

    // create label - vertex map
    progressStart("Analyze Cells 3D - Create vertex vector", 0);
    rca.generateMeshTriangleVector(surfaceMesh, triVec);

    progressStart("Analyze Cells 3D - Interpolate Bezier", 0);
    // Bezier
    CuttingSurface* cutSurf = cuttingSurface();

    Matrix4d rotMatrixS1, rotMatrixCS;
    s1->getFrame().getMatrix(rotMatrixS1.data());
    cutSurf->frame().getMatrix(rotMatrixCS.data());

    // fix the rotations
    Matrix4d mGLTot = transpose(inverse(rotMatrixS1)) * transpose(rotMatrixCS);

    int dataPointsBezier = 200; // number of intermediate points
    //int midP = dataPointsBezier/2-1;

    // create bezier vector and its derivative
    std::map<int, Point3d> bMap,diffbMap;
    std::map<int, Point3d> bMapRadRef;

    discretizeBezierLineRef(cutSurf, 0.5, 0.0, mGLTot, dataPointsBezier, bMap, diffbMap, bMapRadRef);

    bool flipBez = false;
    Point3d selPos;

    forall(const vertex& v, m1->graph()){
      if(v->selected){
        selPos = v->pos;
        break;
      }
    }

    int bezIdx = 0;
    double bezWeight = 0.;
    calcNearestPointOnBezierLine(selPos, bMap, bezIdx, bezWeight);

    if(bezIdx > dataPointsBezier/2){
      flipBez = true;
      Information::out << "Bezier flipped!" << endl;
    }

    if(flipBez){
      typedef std::pair<int, Point3d> IntP3dP;
      forall(IntP3dP p, bMap){
        pr.bez[dataPointsBezier-1-p.first] = p.second;
        pr.diffBez[dataPointsBezier-1-p.first] = diffbMap[p.first];
      }

      forall(IntP3dP p, bMapRadRef){
        pr.bMapRadRef[bMapRadRef.size()-1-p.first] = p.second;
      }

    } else {
      pr.bez = bMap;
      pr.diffBez = diffbMap;
      pr.bMapRadRef = bMapRadRef;
      pr.meshTris = triVec;
    }



    return true;
  }
  REGISTER_PROCESS(AnalyzeLeafPrimSurf);

double calcWeight(double w, int idx)
{
  return ((1-w) + idx*(2*w-1));
}

int interpolateVoxelValue(Stack* s1, const HVecUS& data, Point3d p)
{

  int result = 0;

  Point3i intP(p);

  Point3i imgSize(s1->size());

  double wx = p.x() - intP.x();
  double wy = p.y() - intP.y();
  double wz = p.z() - intP.z();

  for(int ix = 0; ix <= 1; ix++){
    for(int iy = 0; iy <= 1; iy++){
      for(int iz = 0; iz <= 1; iz++){
        Point3i pi = Point3i(ix,iy,iz)+intP;
        if(pi.x() >= 0 and pi.y() >= 0 and pi.z() >= 0 and pi.x() < imgSize.x() and pi.y() < imgSize.y() and pi.z() < imgSize.z())
          result += data[s1->offset(pi)] * calcWeight(wx,ix) * calcWeight(wy,iy) * calcWeight(wz,iz);//((1-wx) + ix*(2*wx-1)) * ((1-wy) + iy*(2*wy-1)) * ((1-wz) + iz*(2*wz-1));
      }
    }
  }
  //return data[s1->offset(intP)];// = value;
  return result;
}


bool ProjectLeafPrimStack::run(Stack* s1, Stack* s2, Store* input, Store* output, primDataStructure& p1, primDataStructure &p2, int threshold)
  {
    // requires RootCellProcessing data

  Mesh* m1 = mesh(0);

  CellAtlasAttr *cellAtlasAttr;
  cellAtlasAttr = &m1->attributes().attrMap<int, CellAtlasData>("Cell Atlas 3D Data");

  CellAtlasConfigAttr *cellAtlasConfigAttr;
  cellAtlasConfigAttr = &m1->attributes().attrMap<int, CellAtlasConfig>("Cell Atlas 3D Config");

  //int numCells = (*cellAtlasAttr).size();

    // convert world coord stack1 into prim coords celldata1
    const HVecUS& dataI = input->data();
    HVecUS& dataO = output->data();
    Point3i imgSize(s2->size());

    std::map<Point3i, Point3i> pointMap;

    int xDim = imgSize.x();
    int yDim = imgSize.y();
    int zDim = imgSize.z();

    //double dif = 0;
    Point3d dif3 (0,0,0);
    //int difCount = 0;

    //double std = 0;
    //int difCount = 0;

    RootCellAnalyzing rca1(cellAtlasAttr, cellAtlasConfigAttr);
    RootCellAnalyzing rca2(cellAtlasAttr, cellAtlasConfigAttr);
    rca1.generateSurfaceMap(p1);//p1.bez, p1.meshTris, p1.diffBez, p1.bMapRadRef);
    rca2.generateSurfaceMap(p2);//.bez, p2.meshTris, p2.diffBez, p2.bMapRadRef);

    cout << "surf map generated" << endl;

    //#pragma omp parallel for schedule(guided)
    for(int x=0; x<=xDim; x++){
      for(int y=0; y<yDim; y++){
        for(int z=0; z<zDim; z++){
          Point3i pOut (x,y,z); // point of O

          Point3d pOutWorld(s2->imageToWorld(pOut));

          // convert I_world into organ
          Point3d organC = rca2.cartesianToOrganCoords(pOutWorld, p2); //worldToPrim(pOutWorld, p2, rca2);

          if(organC.y() == -1 or organC.y() > 1.1) continue; // coordinate not valid (too far from surface or did not hit surface)

          // convert organ into O_world
          Point3d outC = rca1.organToCartesianCoordsWithMap(organC, p1); //primToWorld2(organC, p1, rca1);

          // set Out value
          
          if(outC != Point3d(0,0,0)){
            Point3f pIn = s1->worldToImagef(Point3f(outC));
            //  cout << " vvv World " << pInWorld << "/" << organC << "/" << outC << endl;
            //  cout << " vvv Image " << pIn << "/" << pOut << endl;
            dataO[s2->offset(pOut)] = interpolateVoxelValue(s1, dataI, Point3d(pIn));

/*
            difCount++;
            dif += norm(pOut-pIn);
            dif3 += pOut-pIn;*/
          } 
            
        }
      }
    }
    
    //dif/=(double)difCount;
    //dif3/=(double)difCount;

    //typedef pair<Point3i, Point3i> P3iP3i;

    //std=sqrt(std/(double)difCount);

    //cout << "diff " << dif << "/" << std << "/" << dif3 << endl;

    output->changed();
    output->copyMetaData(input);

    return true;
  }
  REGISTER_PROCESS(ProjectLeafPrimStack);

   bool CellAtlasStack::run(Stack* s1, Store* store1, Store* store2, Mesh* m, double binSize, double radMin, double radMax, QString radType, QString filename, int cubeSize)
  {

    CellAtlasAttr *cellAtlasAttr;
    cellAtlasAttr = &m->attributes().attrMap<int, CellAtlasData>("Cell Atlas 3D Data");

    CellAtlasConfigAttr *cellAtlasConfigAttr;
    cellAtlasConfigAttr = &m->attributes().attrMap<int, CellAtlasConfig>("Cell Atlas 3D Config");

    const HVecUS& dataStore = store1->data();
    //HVecUS& dataStore2 = store2->data();
    Point3i imgSize(s1->size());

    int xDim = imgSize.x();
    int yDim = imgSize.y();
    int zDim = imgSize.z();

    RootCellAnalyzing rca(cellAtlasAttr, cellAtlasConfigAttr);
    //primDataStructure& pds = rcp.prim1;
    //rca.generateSurfaceMap(rcp.prim1);

    std::map<int, double> histogram, histcount;

    //std::cout << "start loop " << std::endl;

    // calculate length of the bezier line (for converting longitudinal coord into um)
    double totalBezLength;
    for(uint j=1; j<rcp.prim1.bez.size(); j++){
      totalBezLength += norm(rcp.prim1.bez[j]-rcp.prim1.bez[j-1]);
    }

    // go through every voxel, check if under surface, find its bin
    #pragma omp parallel for schedule(guided)
    for(int x=0; x<xDim-cubeSize/2; x=x+cubeSize){
      //std::cout << "x " << x << std::endl;
      for(int y=0; y<yDim-cubeSize/2; y=y+cubeSize){
        //int z = zDim/2 +10;
        for(int z=0; z<zDim-cubeSize/2; z=z+cubeSize){
          Point3i pPos (x,y,z);
          Point3i pOut (x+cubeSize/2,y+cubeSize/2,z+cubeSize/2); // point of O
          if(dataStore[s1->offset(pOut)] == 0) continue;

        
          Point3d pOutWorld(s1->imageToWorld(pOut));
          Point3d organC;// = rca.cartesianToOrganCoords(pOutWorld, rcp.prim1); // long rad circ
          Point3d closest;

          organC.x() = cartesianToOrganCoordsLong(pOutWorld, rcp.prim1.bez, closest);
          organC.y() = norm(closest - pOutWorld);

          if(organC.y() < radMin or organC.y() > radMax) continue;

          int bin = organC.x() * totalBezLength / binSize;
          if(bin < 0) bin = 0;

          double value = 0;

          for(int cubeX = 0; cubeX < cubeSize; cubeX++){
            if(pPos.x() + cubeX >= xDim) continue;
            for(int cubeY = 0; cubeY < cubeSize; cubeY++){
              if(pPos.y() + cubeY >= yDim) continue;
              for(int cubeZ = 0; cubeZ < cubeSize; cubeZ++){
                if(pPos.z() + cubeZ >= zDim) continue;
                value+=dataStore[s1->offset(pPos+Point3i(cubeX,cubeY,cubeZ))];
              }
            }
          }

          #pragma omp critical
          {
            histogram[bin] += value;
            histcount[bin]++;
          }
        }
      }
    }

    //std::cout << "end loop " << std::endl;
    typedef std::pair<int, double> IntDouble;
    //forall(IntDouble p, histogram){
    //  std::cout << " h " << p.first << "/" << p.second << std::endl;
    //}

    // write output to file
    if(filename != ""){

      QFile file(filename);
      if(!file.open(QIODevice::WriteOnly))
      {
        std::cout << "File cannot be opened for writing";
        //setErrorMessage(QString("File '%1' cannot be opened for writing").arg(filename));
        return false;
      }
      QTextStream out(&file);

      out << "Bin,Sum Signal,Nr Voxels With Signal";
      //out << "Int,Int,Double";
      out << endl;

      //typedef std::pair<std::pair<int,int>, double> surfaceMapT;
      //forall(surfaceMapT p, rca.surfaceMap){
      //  out << p.first.first << "," << p.first.second << "," << p.second;
      //  out << endl;
      //}

      forall(IntDouble p, histogram){
        out << p.first << "," << p.second  << "," << histcount[p.first];
        out << endl;
      }

    }

    store2->changed();
    store2->copyMetaData(store1);

    return true;
  }
  REGISTER_PROCESS(CellAtlasStack);




}
