#ifndef POINTSET_H 
#define POINTSET_H 

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

class PointSet{

public:
  float (*_point)[3];
  float (*_normal)[3];
  float *_value;
  int _pointN;
  
public:
  PointSet(){
    _pointN = 0;
    _point = NULL;
    _normal = NULL;
    _value = NULL;
  }
  
  PointSet(int N, float (*point)[3], float (*normal)[3]){
    setPointSize(N);
    for(int i=0; i<N; i++){
      setPoint(i, point[i][0], point[i][1], point[i][2]);
      setNormal(i, normal[i][0], normal[i][1], normal[i][2]);
    }
  }
  
  ~PointSet(){
    if(_point != NULL)
      delete[] _point;
    if(_normal != NULL)
      delete[] _normal;
    if(_value != NULL)
      delete[] _value;
  }
  
  void setPointSize(int N){
    _pointN = N;
    _point = new float[N][3];
    _normal = new float[N][3];
  }
  
  inline void swapIndex(int i, int j){
    float tmp = _point[i][0];
    _point[i][0] = _point[j][0];
    _point[j][0] = tmp;
    
    tmp = _point[i][1];
    _point[i][1] = _point[j][1];
    _point[j][1] = tmp;
    
    tmp = _point[i][2];
    _point[i][2] = _point[j][2];
    _point[j][2] = tmp;
    
    if(_normal != NULL){
      tmp = _normal[i][0];
      _normal[i][0] = _normal[j][0];
      _normal[j][0] = tmp;
      
      tmp = _normal[i][1];
      _normal[i][1] = _normal[j][1];
      _normal[j][1] = tmp;
    
      tmp = _normal[i][2];
      _normal[i][2] = _normal[j][2];
      _normal[j][2] = tmp;
    }
    
    if(_value != NULL){
      tmp = _value[i];
      _value[i] = _value[j];
      _value[j] = tmp;
    }
  }
  
  void setPoint(int i, float x, float y, float z){
    _point[i][0] = x;
    _point[i][1] = y;
    _point[i][2] = z;
  }
  
  void setNormal(int i, float x, float y, float z){
    if(_normal == NULL)
      _normal = new float[_pointN][3];
    _normal[i][0] = x;
    _normal[i][1] = y;
    _normal[i][2] = z;
  }
  
  void setValue(int i, float v){
    if(_value == NULL)
      _value = new float[_pointN];
    _value[i] = v;
  }
  
  void getBound(float min[3], float max[3], float rate){
    for(int i=0; i<3; i++){
      max[i] = min[i] = _point[0][i];
      for(int j=1; j<_pointN; j++){
        if(min[i] > _point[j][i])
          min[i] = _point[j][i];
        if(max[i] < _point[j][i])
          max[i] = _point[j][i];
      }
      float s = max[i]-min[i];
      float m = 0.5f*(max[i]+min[i]);
      max[i] = m + 0.5f*rate*s;
      min[i] = m - 0.5f*rate*s;
    }
  }
  
  void getBound(float min[3], float max[3], int start, int end){
    for(int i=0; i<3; i++){
      max[i] = min[i] = _point[start][i];
      for(int j=start+1; j<end; j++){
        if(min[i] > _point[j][i])
          min[i] = _point[j][i];
        if(max[i] < _point[j][i])
          max[i] = _point[j][i];
      }
      float s = max[i]-min[i];
      float m = 0.5f*(max[i]+min[i]);
      max[i] = m + 0.5f*s;
      min[i] = m - 0.5f*s;
    }
  }
  
  //Index "end" is not taken int account
  void centroid(float c[3], int start, int end){
    c[0] = c[1] = c[2] = 0;
    for(int i=start; i<end; i++){
      c[0] += _point[i][0];
      c[1] += _point[i][1];
      c[2] += _point[i][2];
    }
    c[0] /= (end-start);
    c[1] /= (end-start);
    c[2] /= (end-start);
  }
  
  //Index "end" is not taken int account
  void averagedNormal(float n[3], int start, int end){
    n[0] = n[1] = n[2] = 0;
    for(int i=start; i<end; i++){
      n[0] += _normal[i][0];
      n[1] += _normal[i][1];
      n[2] += _normal[i][2];
    }
    double len = sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
    if((float)len != 0){
      n[0] = (float)(n[0]/len);
      n[1] = (float)(n[1]/len);
      n[2] = (float)(n[2]/len);
    }
  }
  
  void rescale(float max_axis){
    float min[3], max[3];
    getBound(min, max, 1.0);
    float size = max[0] - min[0];
    if(max[1] - min[1] > size)
      size = max[1] - min[1];
    if(max[2] - min[2] > size)
      size = max[2] - min[2];
    float s = max_axis/size;
    int i;
    for(i=0; i<_pointN; i++){
      float* p = _point[i];
      p[0] *= s;
      p[1] *= s;
      p[2] *= s;
    }
  }
};

#endif