#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "EspaceVoxelsUS.h"
#include "Couleur.h"
#include "Coord3DD.h"
#include "MatriceD.h"
#include "max_min.h"
#include "TestAlloc.h"

EspaceVoxelsUS::EspaceVoxelsUS(int dx,int dy,int dz) {
  this->dx = dx;
  this->dy = dy;
  this->dz = dz;
  p =(unsigned short ***) calloc(dz,sizeof(unsigned short **));
  testAllocAndExit(p,"new EspaceVoxelsUS(int,int,int)");
  if ( p ) {
    for ( int i = 0 ; i < dz ; i++ ) {
      p[i] =(unsigned short **) calloc(dy,sizeof(unsigned short *));
      testAllocAndExit(p[i],"new EspaceVoxelsUS(int,int,int)",i);
       if ( p[i] ) {
        for ( int j = 0 ; j < dy ; j++ ) {
          p[i][j] =(unsigned short *) calloc(dx,sizeof(unsigned short));
          testAllocAndExit(p[i][j],"new EspaceVoxelsUS(int,int,int)",i,j); } } } }
}

EspaceVoxelsUS::EspaceVoxelsUS(char *filename) {
  FILE *num = fopen(filename,"rb");
  if ( num ) {
    fread(&dx,sizeof(int),1,num);
    fread(&dy,sizeof(int),1,num);
    fread(&dz,sizeof(int),1,num);
    p =(unsigned short ***) calloc(dz,sizeof(unsigned short **));
    testAllocAndExit(p,"new EspaceVoxelsUS(char *)");
    if ( p ) {
      for ( int i = 0 ; i < dz ; i++ ) {
        p[i] =(unsigned short **) calloc(dy,sizeof(unsigned short *));
        testAllocAndExit(p[i],"new EspaceVoxelsUS(char *)",i);
        if ( p[i] ) {
          for ( int j = 0 ; j < dy ; j++ ) {
            p[i][j] =(unsigned short *) calloc(dx,sizeof(unsigned short));
            testAllocAndExit(p[i][j],"new EspaceVoxelsUS(char *)",i,j);
            if ( p[i][j] ) {
              fread(p[i][j],1,dx,num); } } } } }
    fclose(num); }
}

EspaceVoxelsUS::EspaceVoxelsUS(EspaceVoxelsUS *ev,int reduction) {
  dx = ev->dx>>reduction;
  dy = ev->dy>>reduction;
  dz = ev->dz>>reduction;
  p =(unsigned short ***) calloc(dz,sizeof(unsigned short **));
  testAllocAndExit(p,"new EspaceVoxelsUS(EspaceVoxelsUS *,int)");
  if ( p ) {
    for ( int i = 0 ; i < dz ; i++ ) {
      p[i] =(unsigned short **) calloc(dy,sizeof(unsigned short *));
      testAllocAndExit(p[i],"new EspaceVoxelsUS(EspaceVoxelsUS *,int)",i);
      if ( p[i] ) {
        for ( int j = 0 ; j < dy ; j++ ) {
          p[i][j] =(unsigned short *) calloc(dx,sizeof(unsigned short));
          testAllocAndExit(p[i][j],"new EspaceVoxelsUS(EspaceVoxelsUS *,int)",i,j); } } } }
  int size = 1<<reduction;
  for ( int z = 0 ; z < dz ; z++ ) {
    for ( int y = 0 ; y < dy ; y++ ) {
      for ( int x = 0 ; x < dx ; x++ ) {
         int r = 0;
         for ( int i = 0 ; i < size ; i++ )
           for ( int j = 0 ; j < size ; j++ )
             for ( int k = 0 ; k < size ; k++ ) {
               unsigned short c;
               ev->cherche((x<<reduction)+i,(y<<reduction)+j,(z<<reduction)+k,&c);
               r += c; }
         if ( p && p[z] && p[z][y] ) {
           p[z][y][x] =(unsigned short) (r>>(reduction*3)); } } } }
}


EspaceVoxelsUS::~EspaceVoxelsUS(void) {
  if ( p ) {
    for ( int i = 0 ; i < dz ; i++ ) {
      for ( int j = 0 ; j < dy ; j++ ) {
        free(p[i][j]); }
      free(p[i]); }
    free(p); }
}

void EspaceVoxelsUS::init(unsigned char v) {
  for ( int i = 0 ; i < dz ; i++ ) {
    for ( int j = 0 ; j < dy ; j++ ) {
      memset(p[i][j],(int) v,dx*sizeof(unsigned short)); } }
}

void EspaceVoxelsUS::cherche(int x,int y,int z,unsigned short *c) {
  *c = p[z][y][x]; 
}

unsigned short *EspaceVoxelsUS::adresse(int x,int y,int z) {
  return(&p[z][y][x]);
}

unsigned short ***EspaceVoxelsUS::adresseTableau(void) {
  return(p);
}

static int xmin = 100000;
static int xmax = -100000;
static int ymin = 100000;
static int ymax = -100000;
static int zmin = 100000;
static int zmax = -100000;
static int cpt = 0;

void EspaceVoxelsUS::sauve(unsigned short c,int x,int y,int z) {
  if ( x < xmin )
    xmin = x;
  if ( y < ymin )
    ymin = y;
  if ( z < zmin )
    zmin = z;
  if ( x > xmax )
    xmax = x;
  if ( y > ymax )
    ymax = y;
  if ( z > zmax )
    zmax = z;
  cpt++;
  p[z][y][x] = c;
}

int EspaceVoxelsUS::sauvegardeFichier(const char *filename) {
  FILE *num = fopen(filename,"wb");
  printf("%p\n",num);
  if ( num ) {
    fwrite(&dx,sizeof(int),1,num);
    fwrite(&dy,sizeof(int),1,num);
    fwrite(&dz,sizeof(int),1,num);
    for ( int i = 0 ; i < dz ; i++ ) {
      for ( int j = 0 ; j < dy ; j++ ) {
        fwrite(p[i][j],1,dx*sizeof(unsigned short),num); } }
    fclose(num);
    return(1); }
  return(0);
}

void EspaceVoxelsUS::traceLigne(float xi,float yi,float zi,float xf,float yf,float zf,unsigned short coul) {
  int x =(int) floor(xi) ;
  int y =(int) floor(yi) ;
  int z =(int) floor(zi) ;
  sauve(coul,x,y,z);
  double px = xi ;
  double py = yi ;
  double pz = zi ;
  double sdx = xf-xi ;
  double sdy = yf-yi ;
  double sdz = zf-zi ;
  int cp = 0 ;
  while ( ( x != floor(xf) ) || ( y != floor(yf) ) || ( z != floor(zf) ) ) {
    cp++ ;
    double max = 10e+10 ;
    int cas = -1 ;
    { double dd = (x-px)/sdx ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 1 ; 
        max = dd ; } }
    { double dd = (x+1.0F-px)/sdx ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 2 ;
        max = dd ; } }
    { double dd = (y-py)/sdy ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 3 ;
        max = dd ; } }
    { double dd = (y+1.0F-py)/sdy ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 4 ;
        max = dd ; } }
    { double dd = (z-pz)/sdz ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 5 ;
        max = dd ; } }
    { double dd = (z+1.0F-pz)/sdz ;
      if ( ( dd > 0.0000001 ) && ( dd <= max ) ) {
        cas = 6 ;
        max = dd ; } }
    px += max*sdx ;
    py += max*sdy ;
    pz += max*sdz ;
    switch(cas) {
      case 1 : x-- ;
               break ;
      case 2 : x++ ;
               break ;
      case 3 : y-- ;
               break ;
      case 4 : y++ ;
               break ;
      case 5 : z-- ;
               break ;
      case 6 : z++ ;
               break ; }
    sauve(coul,x,y,z); } 
}

static void trouveAnglesOrientation(Coord3DD *pt2,double *az,double *ay) {
  Coord3DD *p2 = new Coord3DD(pt2);
  if ( p2->y == p2->x )
    printf("1\n");
  *az = -atan2(p2->y,p2->x);
  MatriceD *mz = new MatriceD();
  mz->toRotationZRadian((float) *az);
  p2->produit(mz);
  *ay = atan2(p2->z,p2->x);
  if ( p2->z == p2->x )
    printf("1\n");
  delete(p2);
  delete(mz);
}

void EspaceVoxelsUS::traceCylindre(float xi,float yi,float zi,float xf,float yf,float zf,float r,unsigned short coul) {
  Coord3DD *pp = new Coord3DD(xf,yf,zf);
  pp->x -= xi;
  pp->y -= yi;
  pp->z -= zi;
  double ddx = xf-xi;
  double ddy = yf-yi;
  double ddz = zf-zi;
  double l = sqrt(ddx*ddx+ddy*ddy+ddz*ddz);
  double az;
  double ay;
  trouveAnglesOrientation(pp,&az,&ay);
  MatriceD *mz = new MatriceD();
  mz->toRotationZRadian((float) az);
  MatriceD *my = new MatriceD();
  my->toRotationYRadian((float) ay);
  MatriceD *tr = new MatriceD();
  tr->toTranslation(-xi,-yi,-zi);
  MatriceD *m1 = MatriceD::produitMatriciel(mz,tr);
  MatriceD *m2 = MatriceD::produitMatriciel(my,m1);
  delete(m1);
  delete(mz);
  delete(my);
  delete(tr);
  delete(pp);
  cylindre(xi,yi,zi,xf,yf,zf,r,l,m2,coul);
  delete(m2);
}

static int tot = 0;

void EspaceVoxelsUS::cylindre(float xi,float yi,float zi,float xf,float yf,float zf,float r,double l,MatriceD *m,unsigned short coul) {
  int cxi =(int) floor(min_float(xi,xf)-r);
  int cyi =(int) floor(min_float(yi,yf)-r);
  int czi =(int) floor(min_float(zi,zf)-r);
  int cxf =(int) ceil(max_float(xi,xf)+r);
  int cyf =(int) ceil(max_float(yi,yf)+r);
  int czf =(int) ceil(max_float(zi,zf)+r);
  if ( cxi < 0 )
    cxi = 0;
  if ( cyi < 0 )
    cyi = 0;
  if ( czi < 0 )
    czi = 0;
  if ( cxf > dx-1 )
    cxf = dx-1;
  if ( cyf > dy-1 )
    cyf = dy-1;
  if ( czf > dz-1 )
    czf = dz-1;
  int tx = cxf-cxi+1;
  int ty = cyf-cyi+1;
  int tz = czf-czi+1;
  tot += tx*ty*tz;
  printf("%f %f %f    %f %f %f    %f %lf\n",xi,yi,zi,xf,yf,zf,r,l);
  printf("%4d %4d    %4d %4d    %4d %4d    %6d %7d\n",cxi,cxf,cyi,cyf,czi,czf,tx*ty*tz,tot);
  double r2 =(double) r*r;
  for ( int x = cxi ; x <= cxf ; x++ )
    for ( int y = cyi ; y <= cyf ; y++ )
      for ( int z = czi ; z <= czf ; z++ )
        pixelCylindre(x,y,z,r2,l,m,coul,5);
  printf("\n");
}

void EspaceVoxelsUS::pixelCylindre(int x,int y,int z,double r2,double l,MatriceD *m,unsigned short coul,int disc) {
  float dp = 1.0F/(disc+1.0F);
  int mk = 0;
  Coord3DD pp;
  pp.t = 1.0F;
  for ( int i = 0 ; i < disc ; i++ )
    for ( int j = 0 ; j < disc ; j++ )
      for ( int k = 0 ; k < disc ; k++ ) {
        pp.x = x+i*dp;
        pp.y = y+j*dp;
        pp.z = z+k*dp;
        m->produit(&pp);
        if ( ( pp.x >= 0 ) && ( pp.x <= l ) ) {
          if ( pp.z*pp.z+pp.y*pp.y < r2 ) {
            mk++; } } }
  if ( mk != 0 ) {
    mk = mk * 0xFF /(disc*disc*disc);
    unsigned short co;
    cherche(x,y,z,&co);
    int v = co;
    v += mk;
    coul = ( v > 0xFFFF ) ? 0xFFFF : (unsigned char) v;
    //printf("(%3d %3d) ",mk,v);
    sauve(coul,x,y,z); }
}

unsigned short *EspaceVoxelsUS::getArray(void) {
  unsigned short *pt =(unsigned short *) calloc(dx*dy*dz,sizeof(unsigned short));
  unsigned short *c = pt;
  for ( int z = 0 ; z < dz ; z++ )
    for ( int y = 0 ; y < dy ; y++ ) {
      if ( c ) {
        memcpy(c,p[z][y],dx*sizeof(unsigned short)); }
      c += dx; }
  return(pt);
}
