/*
 * accumulator implementation
 *
 * Copyright (C) Dorit Borrmann
 *
 * Released under the GPL version 3.
 *
 */

#include "shapes/accumulator.h"
#include <math.h>
#include "slam6d/globals.icc"
#include <iostream>


#ifdef _MSC_VER
#define isnan(_X) ((_X) != (_X))
#endif

double* polar2normal(double theta, double phi) {
  double * n = new double[4];
  n[0] = cos(theta)*sin(phi);
  n[1] = sin(theta)*sin(phi);
  n[2] = cos(phi);
  Normalize3(n);
  return n;
}

AccumulatorSimple::AccumulatorSimple(ConfigFileHough myCfg) {
 
  count = 0;
  myConfigFileHough = myCfg;
  accumulator = new int**[myConfigFileHough.Get_RhoNum()];

  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    accumulator[i] = new int*[myConfigFileHough.Get_PhiNum()];
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      accumulator[i][j] = new int[myConfigFileHough.Get_ThetaNum()];
      for(unsigned int k = 0; k < myConfigFileHough.Get_ThetaNum(); k++) {
        accumulator[i][j][k] = 0;
      }
    }
  }
}

AccumulatorSimple::~AccumulatorSimple() {

  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      delete[] accumulator[i][j];
    }
    delete[] accumulator[i];
    
  }
  delete[] accumulator;
  
}  

void AccumulatorSimple::printAccumulator() {
  
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_ThetaNum(); j++) {
      double phi1 = i * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
      double phi2 = (i+1) * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
      double theta1 = j * 2*M_PI / myConfigFileHough.Get_ThetaNum();
      double theta2 = (j+1) * 2*M_PI / myConfigFileHough.Get_ThetaNum();
      if(theta2 > 2*M_PI) theta2 = 2*M_PI;
      if(phi2 > M_PI) {
        phi2 = M_PI;
      }
      int rhosum = 0;
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        rhosum += accumulator[k][i][j];
      }
      cout << phi1 << " " << theta1 << " " << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta1 << " " << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta2 << " " << " " << rhosum << " 40" << endl;
      cout << phi1 << " " << theta2 << " " << " " << rhosum << " 40" << endl;
      cout << endl;
    }

    cout << endl;
  }
  
  /*
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_ThetaNum(); j++) {
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        if( accumulator[i][j][k] > 20 ) cout << i << " " << j << " " << k << endl;
      }
    }
  }
  */

}

void AccumulatorSimple::resetAccumulator() {
  count = 0;
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      for(unsigned int k = 0; k < myConfigFileHough.Get_ThetaNum(); k++) {
        accumulator[i][j][k] = 0;
      }
    }
  }
}

bool AccumulatorSimple::accumulate(double theta, double phi, double rho) {
  count++;
  //cout << phi << " " << theta << " " << rho << " ";
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)((rho*(double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
  //cout << (rhoindex + 0.5) * myConfigFileHough.Get_RhoMax()/myConfigFileHough.Get_RhoNum() << endl;
  
  int phiindex = (int)(phi*((myConfigFileHough.Get_PhiNum()*0.99999999)/(M_PI)));
  int thetaindex = (int)(theta*((myConfigFileHough.Get_ThetaNum()*0.9999999)/(2*M_PI)));
  
  //thetaindex = thetaindex % ballNr[phiindex];
  accumulator[rhoindex][phiindex][thetaindex]++;
  return ((unsigned int)accumulator[rhoindex][phiindex][thetaindex] >= myConfigFileHough.Get_AccumulatorMax());
}

void AccumulatorSimple::accumulate(Point p) {
//  cout << p.x << " " << p.y << " " << p.z << endl;

  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    //TODO 0.99 vielleicht nicht gut
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999999);
  
    for(unsigned int j = 0; j < myConfigFileHough.Get_ThetaNum(); j++) {
      double theta = (j+0.5) * 2*M_PI / myConfigFileHough.Get_ThetaNum();
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
        double n[3];
        n[0] = cos(theta)*sin(phi);
        n[1] = sin(theta)*sin(phi);
        n[2] = cos(phi);
        Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
        }

      }
    }
  }

//  cout << "fertig" << endl;
  
}

double* AccumulatorSimple::accumulateRet(Point p) {
  count++;
  // rho theta phi
  double* angles = new double[3]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.9999999);
  
    for(unsigned int j = 0; j < myConfigFileHough.Get_ThetaNum(); j++) {
      double theta = (j+0.5) * 2*M_PI / myConfigFileHough.Get_ThetaNum();
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
        double n[3];
        n[0] = cos(theta)*sin(phi);
        n[1] = sin(theta)*sin(phi);
        n[2] = cos(phi);
        Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(((unsigned int)accumulator[k][i][j] > myConfigFileHough.Get_AccumulatorMax() && (unsigned int)accumulator[k][i][j] > count*myConfigFileHough.Get_PlaneRatio())
          || (unsigned int)accumulator[k][i][j] > myConfigFileHough.Get_AccumulatorMax()) { 
            angles[0] = rho;
            angles[1] = theta;
            angles[2] = phi;
            return angles;
          }
        }

      }
    }
  }

  angles[0] = -1;
  return angles;
  
}

int* AccumulatorSimple::accumulateAPHT(Point p) {

  int tmpInt = 0;
  // rho theta phi
  int* angles = new int[3]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
  
    for(unsigned int j = 0; j < myConfigFileHough.Get_ThetaNum(); j++) {
      double theta = (j+0.5) * 2*M_PI / myConfigFileHough.Get_ThetaNum();
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
        double n[3];
        n[0] = cos(theta)*sin(phi);
        n[1] = sin(theta)*sin(phi);
        n[2] = cos(phi);
        Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(accumulator[k][i][j] > tmpInt) {
            angles[0] = k;
            angles[1] = j;
            angles[2] = i;
            tmpInt = accumulator[k][i][j];
          }
        }

      }
    }
  }

  return angles;
  
}


double* AccumulatorSimple::getMax(double &rho, double &theta, double &phi) {
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)(rho*((double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
  
  int phiindex = (int)(phi*((double)(myConfigFileHough.Get_PhiNum())/(M_PI)));
  int thetaindex = (int)(theta*(((double)myConfigFileHough.Get_ThetaNum())/(2*M_PI)));
  thetaindex = thetaindex % myConfigFileHough.Get_ThetaNum();

  phi = (0.5 + phiindex) * (double)(M_PI / (double)myConfigFileHough.Get_PhiNum());
  theta = (0.5 + thetaindex) * (double)(2.0 * M_PI / (double)myConfigFileHough.Get_ThetaNum());
  rho = (rhoindex + 0.5) * (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  return polar2normal(theta, phi);
}

double* AccumulatorSimple::getMax(int* cell) {
  double phi = (0.5 + cell[3]) * (double)(M_PI / (double)myConfigFileHough.Get_PhiNum());
  double theta = (0.5 + cell[2]) * (double)(2.0 * M_PI /
  (double)myConfigFileHough.Get_ThetaNum());
  double rho = (cell[1] + 0.5) *
  (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  double* polar = polar2normal(theta, phi);
  polar[3] = rho;
  return polar;
}

multiset<int*, maxcompare>* AccumulatorSimple::getMax() {
//void AccumulatorSimple::getMax() {
  multiset<int*, maxcompare>* maxlist = new multiset<int*, maxcompare>();
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      for(unsigned int k = 0; k < myConfigFileHough.Get_ThetaNum(); k++) {
        int* tmp  = new int[4];
        tmp[0] = accumulator[i][j][k];
        tmp[1] = i;
        tmp[2] = k;
        tmp[3] = j;
        maxlist->insert(tmp);
      }
    }
  }
  return maxlist;
}

void AccumulatorSimple::peakWindow(int size) {
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum() - size; i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum() - size; j++) {
      for(unsigned int k = 0; k < myConfigFileHough.Get_ThetaNum() - size; k++) {
        int max = 0;
        for(unsigned int ii = i; ii < i + size; ii++) {
          for(unsigned int ji = j; ji < j + size; ji++) {
            for(unsigned int ki = k; ki < k + size; ki++) {
              if(accumulator[ii][ji][ki] > max) {
                max = accumulator[ii][ji][ki];
              }
            }
          }
        }
        for(unsigned int ii = i; ii < i + size; ii++) {
          for(unsigned int ji = j; ji < j + size; ji++) {
            for(unsigned int ki = k; ki < k + size; ki++) {
              if(accumulator[ii][ji][ki] < max) {
                accumulator[ii][ji][ki] = 0;
              }
            }
          }
        }
      }
    }
  }
}

AccumulatorBall::AccumulatorBall(ConfigFileHough myCfg) {
  count = 0;
  int countCells = 0;
  myConfigFileHough = myCfg;

  double c = 0.0;
  int counter = 0;
  ballNr = new int[myConfigFileHough.Get_PhiNum()];
  double step = 180.0/myConfigFileHough.Get_PhiNum();

  //double MAX_A = M_PI * (2 * fabs(0 - cos(rad(90- step))) + sin(rad(90 - step)) * sin(rad(90 - step)) + 1 * 1); 

  double MAX_A = 2 * M_PI * 1.0;

  double r = sin(rad(0.0));
  double z = cos(rad(0.0));
  double r_next = 0.0;
  double z_next = 0.0;
  
  for(double phi = 0; phi < 180.0; phi+=step) {
    r_next = sin(rad(phi+step));
    z_next = cos(rad(phi+step));
  // Flaecheninhalt der aktuellen Kreisscheibe 
    //double a = (M_PI * (2.0 * 1.0 * fabs(z - z_next) + r*r + r_next*r_next));
    // Umfang des aktuellen Kreises
    double a = 2 * M_PI * (r + r_next)/2.0;
    //a = (r*r - r_next*r_next) * PI; //Flaechenvergleich

    // Berechnung der Schrittgroesse in Grad
    c = ((360.0 * (MAX_A / a)) / (myConfigFileHough.Get_ThetaNum() - 1));
    // Berechnung der Felder im Akkumulatorarray
    int size = (int)(1.0 + 360.0/c);
    //if(true) {
    ballNr[counter] = size;
    //} else {
    //  ballNr[counter] = myConfigFileHough.Get_ThetaNum();
    //}
   
    //cout << counter << endl;
    r = r_next;
    z = z_next;
    counter++;
  }

  
  accumulator = new int**[myConfigFileHough.Get_RhoNum()];

  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    accumulator[i] = new int*[myConfigFileHough.Get_PhiNum()];
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      accumulator[i][j] = new int[ballNr[j]];
      for(int k = 0; k < ballNr[j]; k++) {
        accumulator[i][j][k] = 0;
        countCells++;
      }
    }
  }
  cout << "CountCells " << countCells << endl;
}

AccumulatorBall::~AccumulatorBall() {
  
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      delete[] accumulator[i][j];
    }
    delete[] accumulator[i];
    
  }
  delete[] accumulator;
  delete[] ballNr;
  
}  

void AccumulatorBall::printAccumulator() {
  int sum = 0;
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    sum += ballNr[i];
  }
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    for(int j = 0; j < ballNr[i]; j++) {
      double phi1 = i * M_PI / (myConfigFileHough.Get_PhiNum()*0.9999999999);
      double phi2 = (i+1) * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
      double theta1 = j * 2*M_PI / ballNr[i];
      double theta2 = (j+1) * 2*M_PI / ballNr[i];
      if(theta2 > 2*M_PI) theta2 = 2*M_PI;
      if(phi2 > M_PI) {
        phi2 = M_PI;
      }
      int rhosum = 0;
      /*
      for(int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        rhosum += accumulator[k][i][j];
      }
      */
      rhosum = accumulator[33][i][j];
      cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta1 << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
      cout << phi1 << " " << theta2 << " " << rhosum << " 40" << endl;
      cout << endl;
    }

    //cout << endl;
  }
}

void AccumulatorBall::resetAccumulator() {

  count = 0;
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      int bNr = ballNr[j];
      for(int k = 0; k < bNr; k++) {
        accumulator[i][j][k] = 0;
      }
    }
  }
}

bool AccumulatorBall::accumulate(double theta, double phi, double rho) {
  count++;
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)((rho*(double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  } 
  
  int phiindex = (int)(phi*(((double)myConfigFileHough.Get_PhiNum()*0.9999999)/(M_PI)));
  int bNr = ballNr[phiindex];
  int thetaindex = (int)(theta*((bNr*0.9999999999)/(2*M_PI)));
  if(thetaindex >= bNr) {
    cout << "duet";
  }
  //thetaindex = thetaindex % ballNr[phiindex];
  accumulator[rhoindex][phiindex][thetaindex]++;
  return ((unsigned int)accumulator[rhoindex][phiindex][thetaindex] >= myConfigFileHough.Get_AccumulatorMax());
}

void AccumulatorBall::accumulate(Point p) {
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.999999999);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      n[0] = cos(theta)*sin(phi);
      n[1] = sin(theta)*sin(phi);
      n[2] = cos(phi);
      Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
        }

      }
    }
  }

}
double* AccumulatorBall::accumulateRet(Point p) {
  count++;
  // rho theta phi
  double* angles = new double[3]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      n[0] = cos(theta)*sin(phi);
      n[1] = sin(theta)*sin(phi);
      n[2] = cos(phi);
      Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(
          ((unsigned int)accumulator[k][i][j] > myConfigFileHough.Get_AccumulatorMax() && 
          (unsigned int)accumulator[k][i][j] > count*myConfigFileHough.Get_PlaneRatio()) || 
          (unsigned int)accumulator[k][i][j] > 10*myConfigFileHough.Get_AccumulatorMax()) {
            angles[0] = rho;
            angles[0] = rho;
            angles[1] = theta;
            angles[2] = phi;
            return angles;
          }
        }

      }
    }
  }
  angles[0] = -1;
  return angles;
}

int* AccumulatorBall::accumulateAPHT(Point p) {

  // rho theta phi
  int tmpMax = 0;
  int* angles = new int[4]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.9999999999);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      n[0] = cos(theta)*sin(phi);
      n[1] = sin(theta)*sin(phi);
      n[2] = cos(phi);
      Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(accumulator[k][i][j] > tmpMax) {
            angles[1] = k;
            angles[2] = j;
            angles[3] = i;
            angles[0] = tmpMax = accumulator[k][i][j];
          }
        }

      }
    }
  }
  return angles;
}

double* AccumulatorBall::getMax(double &rho, double &theta, double &phi) {
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  double tmprho = rho;
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)(rho*0.99999999*((double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
  
  int phiindex = (int)(phi*0.99999999*(((double)myConfigFileHough.Get_PhiNum())/(M_PI)));
  int bNr = ballNr[phiindex];
  int thetaindex = (int)(theta*((bNr + 1)/(2*M_PI)));
  thetaindex = thetaindex % bNr;

  phi = (0.5 + phiindex) * (double)(M_PI / (double)myConfigFileHough.Get_PhiNum());
  theta = (0.5 + thetaindex) * (double)(2.0 * M_PI / bNr);
  rho = (rhoindex + 0.5) * (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  if(fabs(rho - rho) > 0.01) cout << rho << " " << tmprho << " " << rhoindex << endl;
  return polar2normal(theta, phi);
}
double* AccumulatorBall::getMax(int* cell) {
  double phi = (0.5 + cell[3]) * (double)(M_PI / (double)myConfigFileHough.Get_PhiNum());
  int bNr = ballNr[cell[3]];
  double theta = (0.5 + cell[2]) * (double)(2.0 * M_PI / bNr);
  double rho = (cell[1] + 0.5) *
  (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  double* polar = polar2normal(theta, phi);
  if(isnan(polar[0])) {
    cout << cell[1] << " " << cell[2] << " " << cell[3] << endl;
    cout << theta <<" bla " << phi << endl;
  }
  polar[3] = rho;
  return polar;
}

multiset<int*, maxcompare>* AccumulatorBall::getMax() {
//void AccumulatorBall::getMax() {
  multiset<int*, maxcompare>* maxlist = new multiset<int*, maxcompare>();
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      int bNr = ballNr[j];
      for(int k = 0; k < bNr; k++) {
        int * tmp = new int[4];
        tmp[0] = accumulator[i][j][k];
        tmp[1] = i;
        tmp[2] = k;
        tmp[3] = j;
        maxlist->insert(tmp);
      }
    }
  }
  cout << maxlist->size() << endl;
  return maxlist;
}

void AccumulatorBall::peakWindow(int size) {
  //cout << "Wir haben " << sum << "Zellen!" << endl;
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum() - size; i++) {
    //cout << "Peak " << i << endl;
    for(int j = 0; j < ballNr[i] - size; j++) {
      
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum() - size; k++) {
        int max = 0;
        for(unsigned int ii = i; (ii < (i + size)) && (ii < myConfigFileHough.Get_PhiNum()); ii++) {
          for(int ji = j; (ji < (j + size)) && (ji < ballNr[ii]); ji++) {
            for(unsigned int ki = k; (ki < (k + size)) && (ki < myConfigFileHough.Get_RhoNum()); ki++) {
              if(accumulator[ki][ii][ji] > max) {
                max = accumulator[ki][ii][ji];
              }
            }
          }
        }
        for(unsigned int ii = i; ii < i + size && ii < myConfigFileHough.Get_PhiNum(); ii++) {
          for(int ji = j; ji < j + size && ji < ballNr[ii]; ji++) {
            for(unsigned int ki = k; ki < k + size && ki < myConfigFileHough.Get_RhoNum(); ki++) {
              if(accumulator[ki][ii][ji] < max) {
                accumulator[ki][ii][ji] = 0;
              }
            }
          }
        }
      }
    }
  }
}

//BLUB



AccumulatorCube::AccumulatorCube(ConfigFileHough myCfg) {
  int countCells = 0;
  count = 0;
  myConfigFileHough = myCfg;
  nrCells = myConfigFileHough.Get_ThetaNum()/4;
  accumulator = new int***[6];
  for(int i = 0; i < 6; i++) {
    accumulator[i] = new int**[nrCells];
    for(int j = 0; j < nrCells; j++) {
      accumulator[i][j] = new int*[nrCells];
      for(int k = 0; k < nrCells; k++) {
        accumulator[i][j][k] = new int[myConfigFileHough.Get_RhoNum()];
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          accumulator[i][j][k][l] = 0;
          countCells++;
        }
      }
    }
  }
  cout << "countCells " << countCells << endl;
}

AccumulatorCube::~AccumulatorCube() {
  nrCells = myConfigFileHough.Get_ThetaNum()/4;
  
  for(unsigned int i = 0; i < 6; i++) {
    for(int j = 0; j < nrCells; j++) {
      for(int k = 0; k < nrCells; k++) {
        delete[] accumulator[i][j][k];
      }
      delete[] accumulator[i][j];
    }
    delete[] accumulator[i];
    
  }
  delete[] accumulator;
  
}

void AccumulatorCube::printAccumulator2() {
  
  double **result;
  result = new double*[4];
  result[0] = new double[3]; 
  result[1] = new double[3]; 
  result[2] = new double[3]; 
  result[3] = new double[3]; 
  
  int j;
  int k;
  
  for(int i = 0; i < 6; i+=1) {
    
    for(j = 1, k = 1; j <= nrCells; j++) {

      buffer_point bptmp;
      bptmp.face = i + 1;
      bptmp.i = j;
      bptmp.j = k;

      coords_cube_for_print(bptmp, result, nrCells);
      //double* tmp = coords_cube_to_s2(bptmp, nrCells);
      double polar1[3];
      double polar2[3];
      double polar3[3];
      double polar4[3];
      //toPolar(tmp, polar1);
   
      Normalize3(result[0]);
      Normalize3(result[1]);
      Normalize3(result[2]);
      Normalize3(result[3]);

      toPolar(result[0], polar1);
      toPolar(result[1], polar2);
      toPolar(result[2], polar3);
      toPolar(result[3], polar4);
      double phi1 = polar1[0];
      double theta1 = polar1[1];
      //double phi2 = polar2[0];
      //double theta2 = polar2[1];
      //double phi3 = polar3[0];
      //double theta3 = polar3[1];
      double phi4 = polar4[0];
      double theta4 = polar4[1];

      cout << phi1 << " " << theta1 << " " <<  " 40" << endl;
      cout << phi4 << " " << theta4 << " " <<  " 40" << endl;
    //cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
    }
    for(j--; k <= nrCells; k++) {

      buffer_point bptmp;
      bptmp.face = i + 1;
      bptmp.i = j;
      bptmp.j = k;

      coords_cube_for_print(bptmp, result, nrCells);
      //double* tmp = coords_cube_to_s2(bptmp, nrCells);
      double polar1[3];
      double polar2[3];
      double polar3[3];
      double polar4[3];
      //toPolar(tmp, polar1);
   
      Normalize3(result[0]);
      Normalize3(result[1]);
      Normalize3(result[2]);
      Normalize3(result[3]);

      toPolar(result[0], polar1);
      toPolar(result[1], polar2);
      toPolar(result[2], polar3);
      toPolar(result[3], polar4);
      //double phi1 = polar1[0];
      //double theta1 = polar1[1];
      //double phi2 = polar2[0];
      //double theta2 = polar2[1];
      double phi3 = polar3[0];
      double theta3 = polar3[1];
      double phi4 = polar4[0];
      double theta4 = polar4[1];

      cout << phi4 << " " << theta4 << " " << " 40" << endl;
      cout << phi3 << " " << theta3 << " " << " 40" << endl;
    //cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
    }
    for(k--; j > 0; j--) {

      buffer_point bptmp;
      bptmp.face = i + 1;
      bptmp.i = j;
      bptmp.j = k;

      coords_cube_for_print(bptmp, result, nrCells);
      //double* tmp = coords_cube_to_s2(bptmp, nrCells);
      double polar1[3];
      double polar2[3];
      double polar3[3];
      double polar4[3];
      //toPolar(tmp, polar1);
   
      Normalize3(result[0]);
      Normalize3(result[1]);
      Normalize3(result[2]);
      Normalize3(result[3]);

      toPolar(result[0], polar1);
      toPolar(result[1], polar2);
      toPolar(result[2], polar3);
      toPolar(result[3], polar4);
      //double phi1 = polar1[0];
      //double theta1 = polar1[1];
      double phi2 = polar2[0];
      double theta2 = polar2[1];
      double phi3 = polar3[0];
      double theta3 = polar3[1];
      //double phi4 = polar4[0];
      //double theta4 = polar4[1];

      cout << phi3 << " " << theta3 << " " << " 40" << endl;
      cout << phi2 << " " << theta2 << " " << " 40" << endl;
    //cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
    }
    for(j++; k > 0; k--) {

      buffer_point bptmp;
      bptmp.face = i + 1;
      bptmp.i = j;
      bptmp.j = k;

      coords_cube_for_print(bptmp, result, nrCells);
      //double* tmp = coords_cube_to_s2(bptmp, nrCells);
      double polar1[3];
      double polar2[3];
      double polar3[3];
      double polar4[3];
      //toPolar(tmp, polar1);
   
      Normalize3(result[0]);
      Normalize3(result[1]);
      Normalize3(result[2]);
      Normalize3(result[3]);

      toPolar(result[0], polar1);
      toPolar(result[1], polar2);
      toPolar(result[2], polar3);
      toPolar(result[3], polar4);
      double phi1 = polar1[0];
      double theta1 = polar1[1];
      double phi2 = polar2[0];
      double theta2 = polar2[1];
      //double phi3 = polar3[0];
      //double theta3 = polar3[1];
      //double phi4 = polar4[0];
      //double theta4 = polar4[1];

      cout << phi2 << " " << theta2 << " " <<  " 40" << endl;
      cout << phi1 << " " << theta1 << " " <<  " 40" << endl;
    //cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
    }

    cout << 0.0 << " " << 0.0 << " " <<  " 0" << endl;
  }
  cout << 0.0 << " " << 0.0 << " " <<  " 0" << endl;
}

void AccumulatorCube::printAccumulator() {
 
  //double norm[3];
  //double polar[3];
//  double *result;
  
  double **result;
  result = new double*[4];
  result[0] = new double[3]; 
  result[1] = new double[3]; 
  result[2] = new double[3]; 
  result[3] = new double[3]; 
  
  //buffer_point bp;

  /*
  for(double o = -0.9; o < 1.0; o+=0.1) {
    for(double p = -0.9; p < 1.0; p+=0.1) {
      norm[2] = o;
      norm[1] = p;
      norm[0] = 1.0;
      toPolar(norm, polar);
      
      bp = coords_s2_to_cell(norm, nrCells);
      result = coords_cube_to_s2(bp, nrCells);
      toPolar(result, polar);
      
      cout << polar[0] << " " << polar[1] << " " << 40 << endl;
      //if(fabs(polar[1]) < 0.0001) 
      //  cout << "STOP" << "-1.0 " << p << " " << o << endl;
      delete[] result;
    }
  }
  */

 /* 
  norm[0] = 1.0;  
  norm[1] = 0.0;  
  norm[2] = 0.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl << endl << endl;
  delete[] result;

  norm[0] = -1.0;  
  norm[1] = 0.0;  
  norm[2] = 0.0;

  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl;
  cout << endl << endl;
  delete[] result;

  norm[0] = 0.0;  
  norm[1] = 1.0;  
  norm[2] = 0.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl;
  cout << endl << endl;
  delete[] result;

  norm[0] = 0.0;  
  norm[1] = -1.0;  
  norm[2] = 0.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl;
  cout << endl << endl;
  delete[] result;

  norm[0] = 0.0;  
  norm[1] = 0.0;  
  norm[2] = 1.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl;
  cout << endl << endl;
  delete[] result;

  norm[0] = 0.0;  
  norm[1] = 0.0;  
  norm[2] = -1.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  result = coords_cube_to_s2(bp, nrCells);
  toPolar(result, polar);
  cout << polar[0] << " " << polar[1] << endl;
  cout << endl << endl;
  delete[] result;
*//*
  norm[0] = 0.0;  
  norm[1] = 0.0;  
  norm[2] = -1.0; 
  toPolar(norm, polar);
  cout << polar[0] << " " << polar[1] << endl;
  bp = coords_s2_to_cell(norm, nrCells);
  
  int i = bp.face - 1;
  int j = bp.i;
  int k = bp.j;
 */
  for(int i = 0; i < 6; i+=1) {
    cout << 0.0 << " " << 0.0 << " " << 0 << " 0" << endl;
    //for(int j = 1; j j <= nrCells j+= (nrCells-1)) {
    for(int j = 1; j <= nrCells; j++) {
      //cout << endl;
      //for(int k = 1; j k <= nrCells k+= (nrCells-1)) {
      for(int k = 1; k <= nrCells; k++) {
        //cout << endl;
        int rhosum = 0;
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          rhosum += accumulator[i][j-1][k-1][l];
        }
        buffer_point bptmp;
        bptmp.face = i + 1;
        bptmp.i = j;
        bptmp.j = k;

        coords_cube_for_print(bptmp, result, nrCells);
        //double* tmp = coords_cube_to_s2(bptmp, nrCells);
        double polar1[3];
        double polar2[3];
        double polar3[3];
        double polar4[3];
        //toPolar(tmp, polar1);
       
        Normalize3(result[0]);
        Normalize3(result[1]);
        Normalize3(result[2]);
        Normalize3(result[3]);
  /*     
        cout << result[0][0] << " " << result[0][1] << " " << result[0][2] << endl;
        cout << result[1][0] << " " << result[1][1] << " " << result[1][2] << endl;
        cout << result[2][0] << " " << result[2][1] << " " << result[2][2] << endl;
        cout << result[3][0] << " " << result[3][1] << " " << result[3][2] << endl;
        cout << result[0][0] << " " << result[0][1] << " " << result[0][2] << endl;
    */   
        toPolar(result[0], polar1);
        toPolar(result[1], polar2);
        toPolar(result[2], polar3);
        toPolar(result[3], polar4);
        double phi1 = polar1[0];
        double theta1 = polar1[1];
        double phi2 = polar2[0];
        double theta2 = polar2[1];
        double phi3 = polar3[0];
        double theta3 = polar3[1];
        double phi4 = polar4[0];
        double theta4 = polar4[1];

        /*
        cout << phi - 0.02 << " " << theta - 0.02 << " " << rhosum << endl;
        cout << phi + 0.02 << " " << theta - 0.02 << " " << rhosum << endl;
        cout << phi + 0.02 << " " << theta + 0.02 << " " << rhosum << endl;
        cout << phi - 0.02 << " " << theta + 0.02 << " " << rhosum << endl;
        */

        /*
        cout << (j+1+i*nrCells) << " " << k+1 << " " << rhosum << endl;
        cout << (j+1+i*nrCells) << " " << k << " " << rhosum << endl;
        cout << (j+i*nrCells) << " " << k << " " << rhosum << endl;
        cout << (j+i*nrCells) << " " << k+1 << " " << rhosum << endl;
        */
       /* 
        cout << phi1 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << phi1 << " " << theta2 << " " << rhosum << " 40" << endl;
       */
       
      if(phi1 < M_PI/nrCells && fabs(phi1-phi2) < 0.001 && fabs(phi1-phi3) < 0.001 && fabs(phi1 - phi4) < 0.001) { 
      //if(phi1 < M_PI/nrCells && phi2 < M_PI/nrCells && phi3 < M_PI/nrCells && phi4 < M_PI/nrCells) {
        cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
        cout << 0.0 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
        cout << 0.0 << " " << 0.0 << " " << rhosum << " 40" << endl;
        cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;
        //cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
         
        cout << endl;
        //cout << endl; 

      } //else if(M_PI - phi1 < M_PI/nrCells && M_PI - phi2 < M_PI/nrCells && M_PI - phi3 < M_PI/nrCells && M_PI - phi4 < M_PI/nrCells){
        else if(M_PI - phi1 < M_PI/nrCells && fabs(phi1-phi2) < 0.001 && fabs(phi1-phi3) < 0.001 && fabs(phi1 - phi4) < 0.001) {
        cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
        cout << M_PI << " " << 2*M_PI << " " << rhosum << " 40" << endl;
        cout << M_PI << " " << 0.0 << " " << rhosum << " 40" << endl;
        //cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;
        cout << endl;
        //<< endl; 
         
        
      } else if(fabs(phi1 - phi2) < 2.0 && fabs(theta2 - theta3) < 3.0 && fabs(theta4 - theta3) < 3.0) {
        cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
        cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
        //cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

        //cout << endl;
        cout << endl;
      } else if(fabs(phi1 - phi2) < 2.0) {
        if(fabs(theta2 - theta3) < 3.0) {
          if(theta3 < 1.0) {
            cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << 0.0 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;

          //  cout << endl;
            cout << endl;
        
            cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

            //cout << endl;
            cout << endl;
          } else {
            cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;
        
            cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;

          }
        } else if(theta3 < 1.0) {
            cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << 0.0 << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;
        
            cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
          //  cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;
        } else {
            cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << 2*M_PI << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << 2*M_PI << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;
        
            cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
            cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
            cout << phi3 << " " << 0.0 << " " << rhosum << " 40" << endl;
            cout << phi4 << " " << 0.0 << " " << rhosum << " 40" << endl;
           // cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

           // cout << endl;
            cout << endl;

        }
      } else if(phi1 < 1.0) {
        cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << 0.0 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << 0.0 << " " << theta3 << " " << rhosum << " 40" << endl;
        cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
       // cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

       // cout << endl;
        cout << endl;

        cout << M_PI << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
        cout << M_PI << " " << theta4 << " " << rhosum << " 40" << endl;
       // cout << M_PI << " " << theta1 << " " << rhosum << " 40" << endl;

       // cout << endl;
        cout << endl;


      } else {
        cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << M_PI << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << M_PI << " " << theta3 << " " << rhosum << " 40" << endl;
        cout << phi4 << " " << theta4 << " " << rhosum << " 40" << endl;
       // cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;

       // cout << endl;
        cout << endl;

        cout << 0.0 << " " << theta1 << " " << rhosum << " 40" << endl;
        cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
        cout << phi3 << " " << theta3 << " " << rhosum << " 40" << endl;
        cout << 0.0 << " " << theta4 << " " << rhosum << " 40" << endl;
       // cout << 0.0 << " " << theta1 << " " << rhosum << " 40" << endl;

       // cout << endl;
        cout << endl;
      }
       
       // cout << endl;
       // cout << endl;
        //delete[] tmp;

      }
    }
    cout << endl;
  }
  
  for(int i = 0; i < 3; i++) {
    delete[] result[i];
  }
  delete[] result;

}

void AccumulatorCube::resetAccumulator() {
  count = 0;
  for(int i = 0; i < 6; i++) {
    for(int j = 0; j < nrCells; j++) {
      for(int k = 0; k < nrCells; k++) {
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          accumulator[i][j][k][l] = 0;
        }
      }
    }
  }
}

bool AccumulatorCube::accumulate(double theta, double phi, double rho) {
  count++;
  double n[3];
  n[0] = cos(theta)*sin(phi);
  n[1] = sin(theta)*sin(phi);
  n[2] = cos(phi);

  buffer_point bp = coords_s2_to_cell(n, nrCells);
  
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)(rho*((double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }

  accumulator[bp.face - 1][bp.i - 1][bp.j - 1][rhoindex]++;
  lastbp = bp;

  return accumulator[bp.face - 1][bp.i - 1][bp.j - 1][rhoindex] > (int)myConfigFileHough.Get_AccumulatorMax(); 
}

double* AccumulatorCube::getMax(double &rho, double &theta, double &phi) {
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)(rho*((double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
 
  double* result;
  result = coords_cube_to_s2(lastbp, nrCells); 
  //cout << "N: " << result[0] << " " << result[1] << " " << result[2] << endl; 
  return result;
}

void AccumulatorCube::accumulate(Point p) {
  for(int i = 0; i < 6; i++) {
    for(int j = 1; j <= nrCells; j++) {
      for(int k = 1; k <= nrCells; k++) {
        buffer_point bptmp;
        bptmp.face = i + 1;
        bptmp.i = j;
        bptmp.j = k;

        double* n = coords_cube_to_s2(bptmp, nrCells);
        Normalize3(n);
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          double rho = (l + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
          double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
          if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
            accumulator[i][j-1][k-1][l]++;
          }

        }
        delete[] n;
      }
    }
  }
}

double* AccumulatorCube::accumulateRet(Point p) {
  // rho theta phi
  count++;
  double * angles = new double[3];
  for(int i = 0; i < 6; i++) {
    for(int j = 1; j <= nrCells; j++) {
      for(int k = 1; k <= nrCells; k++) {
        buffer_point bptmp;
        bptmp.face = i + 1;
        bptmp.i = j;
        bptmp.j = k;

        double* n = coords_cube_to_s2(bptmp, nrCells);
        Normalize3(n);
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          double rho = (l + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
          double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
          if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
            accumulator[i][j-1][k-1][l]++;
            if(((unsigned int)accumulator[i][j-1][k-1][l] > myConfigFileHough.Get_AccumulatorMax() 
            && (unsigned int)accumulator[i][j-1][k-1][l] >
            count*myConfigFileHough.Get_PlaneRatio()) ||
          (unsigned int)accumulator[i][j-1][k-1][l] > 10*myConfigFileHough.Get_AccumulatorMax() 
            ) {
              double polar[3];
              toPolar(n, polar);
              angles[0] = rho;
              angles[1] = polar[1];
              angles[2] = polar[0];
              return angles;
            }
          }

        }
        delete[] n;
      }
    }
  }
  angles[0] = -1.0;
  return angles;
}

int* AccumulatorCube::accumulateAPHT(Point p) {
  // rho theta phi
  int tmpMax = 0;
  int * angles = new int[4];
  for(int i = 0; i < 6; i++) {
    for(int j = 1; j <= nrCells; j++) {
      for(int k = 1; k <= nrCells; k++) {
        buffer_point bptmp;
        bptmp.face = i + 1;
        bptmp.i = j;
        bptmp.j = k;

        double* n = coords_cube_to_s2(bptmp, nrCells);
        Normalize3(n);
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          double rho = (l + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
          double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
          if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
            accumulator[i][j-1][k-1][l]++;
            if(accumulator[i][j-1][k-1][l] > tmpMax) {
              angles[0] = l;
              angles[1] = i;
              angles[2] = j;
              angles[3] = k;
              tmpMax = accumulator[i][j-1][k-1][l];
            }
          }

        }
        delete[] n;
      }
    }
  }
  return angles;
}

double* AccumulatorCube::getMax(int* cell) {
  buffer_point bp;
  bp.face = cell[2] + 1;
  bp.i = cell[3] + 1;
  bp.j = cell[4] + 1;

  
  double rho = (cell[1] + 0.5) *
  (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  double* polar = coords_cube_to_s2(bp, nrCells); 
  double* polar2 = new double[4];
  for(int i = 0; i < 3; i++) {
    polar2[i] = polar[i];
  }
  delete[] polar;
  polar2[3] = rho;
  return polar2;
}

//void AccumulatorCube::getMax() {
multiset<int*, maxcompare>* AccumulatorCube::getMax() {
  multiset<int*, maxcompare>* maxlist = new multiset<int*, maxcompare>();
  for(int i = 0; i < 6; i++) {
    for(int j = 0; j < nrCells; j++) {
      for(int k = 0; k < nrCells; k++) {
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum(); l++) {
          int* tmp = new int[5];
          tmp[0] = accumulator[i][j][k][l];
          tmp[1] = l;
          tmp[2] = i;
          tmp[3] = j;
          tmp[4] = k;
          maxlist->insert(tmp);
        }
      }
    }
  }
  return maxlist;
}

void AccumulatorCube::peakWindow(int size) {
  for(int i = 0; i < 6; i++) {
    for(int j = 0; j < nrCells - size; j++) {
      for(int k = 0; k < nrCells - size; k++) {
        for(unsigned int l = 0; l < myConfigFileHough.Get_RhoNum() - size; l++) {
          int max = 0;
          for(int ji = j; ji < j + size; ji++) {
            for(int ki = k; ki < k + size; ki++) {
              for(unsigned int li = l; li < l + size; li++) {
                if(accumulator[i][ji][ki][li] > max) {
                  max = accumulator[i][ji][ki][li];
                }
              }
            }
          }
          for(int ji = j; ji < j + size; ji++) {
            for(int ki = k; ki < k + size; ki++) {
              for(unsigned int li = l; li < l + size; li++) {
                if(accumulator[i][ji][ki][li] < max) {
                  accumulator[i][ji][ki][li] = 0;
                }
              }
            }
          }
        }
      }
    }
  }
}


/*** HERE BEGINS THE ACCUMULATORBALL IMPROVED ***/

AccumulatorBallI::AccumulatorBallI(ConfigFileHough myCfg) {
  count = 0;
  int countCells = 0;
  myConfigFileHough = myCfg;

  int counter = 0;
  ballNr = new int[myConfigFileHough.Get_PhiNum()];
  step = 180.0/myConfigFileHough.Get_PhiNum();

  //double MAX_A = M_PI * (2 * fabs(0 - cos(rad(90- step))) + sin(rad(90 - step)) * sin(rad(90 - step)) + 1 * 1); 

  double h_0 = cos(rad(90 - step));
  double MAX_A = (2.0 * M_PI * h_0) / myConfigFileHough.Get_ThetaNum();
  double h_top = MAX_A /(2.0 * M_PI);
  phi_top_rad = acos(1 - h_top);
  phi_top_deg = deg(phi_top_rad);
  step = (180.0 - 2.0 * phi_top_deg) / (double)(myConfigFileHough.Get_PhiNum() - 2.0);

  double phi;
  ballNr[counter] = 1;
  ballNr[myConfigFileHough.Get_PhiNum() - 1 - counter] = 1;
  counter++;

  for(phi = phi_top_deg; phi < 90; phi+=step) {
    double h_i = cos(rad(phi)) - cos(rad(phi + step));
    double a_i = 2 * M_PI * h_i;
    ballNr[counter] = a_i / MAX_A;
    ballNr[myConfigFileHough.Get_PhiNum() - 1 - counter] = a_i / MAX_A;
    //cout << counter << " " << ballNr[counter] << " " << a_i << " " << MAX_A << endl;
    //cout << myConfigFileHough.Get_PhiNum() - 1 - counter << " " << ballNr[myConfigFileHough.Get_PhiNum() - 1 - counter] << " " << a_i << " " << MAX_A << endl;
    counter++;
  }
  /*
  for(int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    cout << i << ": " << ballNr[i] << endl;
  }
  cout << "BallNR erzeugt" << endl; 
  */

  accumulator = new int**[myConfigFileHough.Get_RhoNum()];

  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    accumulator[i] = new int*[myConfigFileHough.Get_PhiNum()];
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      accumulator[i][j] = new int[ballNr[j]];
      for(int k = 0; k < ballNr[j]; k++) {
        accumulator[i][j][k] = 0;
        countCells++;
      }
    }
  }
  cout << "CountCells " << countCells << endl;
}

AccumulatorBallI::~AccumulatorBallI() {
  
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      delete[] accumulator[i][j];
    }
    delete[] accumulator[i];
    
  }
  delete[] accumulator;
  delete[] ballNr;
  
}  

void AccumulatorBallI::printAccumulator() {
//TODO
  int sum = 0;
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    sum += ballNr[i];
  }
  //cout << "Wir haben " << sum << "Zellen!" << endl;
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    for(int j = 0; j < ballNr[i]; j++) {
      double phi1 = i * M_PI / (myConfigFileHough.Get_PhiNum()*0.99999999);
      double phi2 = (i+1) * M_PI / (myConfigFileHough.Get_PhiNum()*0.999999999);
      double theta1 = j * 2*M_PI / ballNr[i];
      double theta2 = (j+1) * 2*M_PI / ballNr[i];
      if(theta2 > 2*M_PI) theta2 = 2*M_PI;
      if(phi2 > M_PI) {
        phi2 = M_PI;
      }
      int rhosum = 0;
      /*
      for(int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        rhosum += accumulator[k][i][j];
      }
      */
      rhosum = accumulator[33][i][j];
      cout << phi1 << " " << theta1 << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta1 << " " << rhosum << " 40" << endl;
      cout << phi2 << " " << theta2 << " " << rhosum << " 40" << endl;
      cout << phi1 << " " << theta2 << " " << rhosum << " 40" << endl;
      cout << endl;
    }

    //cout << endl;
  }
}

void AccumulatorBallI::resetAccumulator() {

//TODO
  count = 0;
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      int bNr = ballNr[j];
      for(int k = 0; k < bNr; k++) {
        accumulator[i][j][k] = 0;
      }
    }
  }
}

bool AccumulatorBallI::accumulate(double theta, double phi, double rho) {
//TODO
  count++;
  int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex = (int)((rho*(double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
  
  int phiindex = (int)(phi*(((double)myConfigFileHough.Get_PhiNum()*0.9999999999)/(M_PI)));
  int bNr = ballNr[phiindex];
  int thetaindex = (int)(theta*((bNr*0.9999999999)/(2*M_PI)));
  if(thetaindex >= bNr) {
    cout << "duet";
  }
  //thetaindex = thetaindex % ballNr[phiindex];
  accumulator[rhoindex][phiindex][thetaindex]++;
  return ((unsigned int)accumulator[rhoindex][phiindex][thetaindex] >= myConfigFileHough.Get_AccumulatorMax());
}

void AccumulatorBallI::accumulate(Point p) {
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = phi_top_rad + (i-0.5) * rad(step);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      if(i == 0) {
        n[0] = 0.0;
        n[1] = 0.0;
        n[2] = 1.0;
      } else if (i == myConfigFileHough.Get_RhoNum() - 1) {
        n[0] = 0.0;
        n[1] = 0.0;
        n[2] = -1.0;
      } else {
        n[0] = cos(theta)*sin(phi);
        n[1] = sin(theta)*sin(phi);
        n[2] = cos(phi);
        Normalize3(n);
      }
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * (double)myConfigFileHough.Get_RhoMax() / (double)myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
        }

      }
    }
  }

}
double* AccumulatorBallI::accumulateRet(Point p) {
  count++;
//TODO
  // rho theta phi
  double* angles = new double[3]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.999999999);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      n[0] = cos(theta)*sin(phi);
      n[1] = sin(theta)*sin(phi);
      n[2] = cos(phi);
      Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(((unsigned int)accumulator[k][i][j] >
          myConfigFileHough.Get_AccumulatorMax() && (unsigned
          int)accumulator[k][i][j] > count*myConfigFileHough.Get_PlaneRatio())  
          || (unsigned int)accumulator[k][i][j] > 10*myConfigFileHough.Get_AccumulatorMax() ) { 
            angles[0] = rho;
            angles[1] = theta;
            angles[2] = phi;
            return angles;
          }
        }

      }
    }
  }
  angles[0] = -1;
  return angles;
}

int* AccumulatorBallI::accumulateAPHT(Point p) {

//TODO
  // rho theta phi
  int tmpMax = 0;
  int* angles = new int[4]; 
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum(); i++) {
    double phi = (i+0.5) * M_PI / (myConfigFileHough.Get_PhiNum()*0.9999999);
    for(int j = 0; j < ballNr[i]; j++) {
      double theta = (j+0.5) * 2*M_PI / ballNr[i];
      if(theta > 2*M_PI) theta = 2*M_PI;
      if(phi > M_PI) {
        phi = M_PI;
      }
      double n[3];
      n[0] = cos(theta)*sin(phi);
      n[1] = sin(theta)*sin(phi);
      n[2] = cos(phi);
      Normalize3(n);
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum(); k++) {
        double rho = (k + 0.5) * myConfigFileHough.Get_RhoMax() / myConfigFileHough.Get_RhoNum(); 
        double distance = p.x * n[0] + p.y * n[1] + p.z * n[2];
        if(fabs(distance-rho) < myConfigFileHough.Get_MaxPointPlaneDist()) {
          accumulator[k][i][j]++;
          if(accumulator[k][i][j] > tmpMax) {
            angles[1] = k;
            angles[2] = j;
            angles[3] = i;
            angles[0] = tmpMax = accumulator[k][i][j];
          }
        }

      }
    }
  }
  return angles;
}

double* AccumulatorBallI::getMax(double &rho, double &theta, double &phi) {
//TODO was??
  
  unsigned int rhoindex = myConfigFileHough.Get_RhoNum() - 1;
  
  if(rho < myConfigFileHough.Get_RhoMax()) {
    rhoindex =
    (int)(rho*((double)myConfigFileHough.Get_RhoNum()/(double)myConfigFileHough.Get_RhoMax()));   
  }
  
  unsigned int phiindex = (int)(phi*(((double)myConfigFileHough.Get_PhiNum())/(M_PI)));
  if(phiindex == myConfigFileHough.Get_PhiNum()) phiindex--;
  int bNr = ballNr[phiindex];
  int thetaindex = (int)(theta*((bNr + 1)/(2*M_PI)));
  thetaindex = thetaindex % bNr;

  phi = (0.5 + phiindex) * (double)(M_PI / (double)myConfigFileHough.Get_PhiNum());
  theta = (0.5 + thetaindex) * (double)(2.0 * M_PI / bNr);
  rho = (rhoindex + 0.5) * (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  return polar2normal(theta, phi);
}
double* AccumulatorBallI::getMax(int* cell) {
  double * polar;
  if(cell[3] == 0) {  
            polar = new double[4];
            polar[0] = 0.0;
            polar[1] = 0.0;
            polar[2] = 1.0;
  } else if((unsigned int)cell[3] == myConfigFileHough.Get_PhiNum() - 1) {
            polar = new double[4];
            polar[0] = 0.0;
            polar[1] = 0.0;
            polar[2] = -1.0;
  } else {
            double phi = phi_top_rad + (-0.5 + cell[3]) * rad(step);
            double bNr = ballNr[cell[3]];
            double theta = (0.5 + cell[2]) * (double)(2.0 * M_PI / bNr);
            polar = polar2normal(theta, phi);
  }
  
  double rho = (cell[1] + 0.5) *
  (double)myConfigFileHough.Get_RhoMax()/(double)myConfigFileHough.Get_RhoNum();
  if(isnan(polar[0])) {
    cout << cell[1] << " " << cell[2] << " " << cell[3] << endl;
    //cout << theta <<" bla " << phi << endl;
  }
  polar[3] = rho;
  return polar;
}

multiset<int*, maxcompare>* AccumulatorBallI::getMax() {
//void AccumulatorBallI::getMax() {
  multiset<int*, maxcompare>* maxlist = new multiset<int*, maxcompare>();
  //for(unsigned int i = 0; i < 1; i++) {
  for(unsigned int i = 0; i < myConfigFileHough.Get_RhoNum(); i++) {
    for(unsigned int j = 0; j < myConfigFileHough.Get_PhiNum(); j++) {
      int bNr = ballNr[j];
      for(int k = 0; k < bNr; k++) {
        int * tmp = new int[4];
        tmp[0] = accumulator[i][j][k];
        tmp[1] = i;
        tmp[2] = k;
        tmp[3] = j;
        maxlist->insert(tmp);
        double * polar;
        double phi;
        double theta = 0.0;
        if(tmp[3] == 0) {  
            polar = new double[4];
            polar[0] = 0.0;
            polar[1] = 0.0;
            polar[2] = 1.0;
            phi = 0;
        } else if((unsigned int)tmp[3] == myConfigFileHough.Get_PhiNum() - 1) {
            polar = new double[4];
            polar[0] = 0.0;
            polar[1] = 0.0;
            polar[2] = -1.0;
            phi = M_PI;
        } else {
            phi = phi_top_rad + (-0.5 + tmp[3]) * rad(step);
            int bNr = ballNr[tmp[3]];
            theta = (0.5 + tmp[2]) * (double)(2.0 * M_PI / (double)bNr);
            polar = polar2normal(theta, phi);
        }
        //cout << polar[0] << " " << polar[1] << " " << polar[2] << " " << phi << " " << theta << endl;
      }
    }
  }
  cout << "Maxlist Size:" << maxlist->size() << endl;
  return maxlist;
}
void AccumulatorBallI::peakWindow(int size) {
  //cout << "Wir haben " << sum << "Zellen!" << endl;
  for(unsigned int i = 0; i < myConfigFileHough.Get_PhiNum() - size; i++) {
    //cout << "Peak " << i << endl;
    for(int j = 0; j < ballNr[i] - size; j++) {
      
      for(unsigned int k = 0; k < myConfigFileHough.Get_RhoNum() - size; k++) {
        int max = 0;
        for(unsigned int ii = i; (ii < (i + size)) && (ii < myConfigFileHough.Get_PhiNum()); ii++) {
          for(int ji = j; (ji < (j + size)) && (ji < ballNr[ii]); ji++) {
            for(unsigned int ki = k; (ki < (k + size)) && (ki < myConfigFileHough.Get_RhoNum()); ki++) {
              if(accumulator[ki][ii][ji] > max) {
                max = accumulator[ki][ii][ji];
              }
            }
          }
        }
        for(unsigned int ii = i; ii < i + size && ii < myConfigFileHough.Get_PhiNum(); ii++) {
          for(int ji = j; ji < j + size && ji < ballNr[ii]; ji++) {
            for(unsigned int ki = k; ki < k + size && ki < myConfigFileHough.Get_RhoNum(); ki++) {
              if(accumulator[ki][ii][ji] < max) {
                accumulator[ki][ii][ji] = 0;
              }
            }
          }
        }
      }
    }
  }
}