/*
 * veloshow implementation
 *
 * Copyright (C) Andreas Nuechter, Li Wei, Li Ming
 *
 * Released under the GPL version 3.
 *
 */

/**
 * @file
 * @brief Implementation for displaying of a matched 3D scene
 * @author Andreas Nuechter. Jacobs University Bremen, Germany
 * @author Li Wei, Wuhan University, China
 * @author Li Ming, Wuhan University, China
 */
#ifdef WITH_GLEE
#include <GLee.h>
#endif

#include "show/show.h"
#include "show/show_Boctree.h"
#include "show/compacttree.h"
#include "show/NurbsPath.h"
#include "show/vertexarray.h"
#include "slam6d/scan.h"
#include "veloslam/veloscan.h"
#include "glui/glui.h"  /* Header File For The glui functions */
#include <fstream>
using std::ifstream;
#include <stdexcept>
using std::exception;

#ifdef _MSC_VER
#include "XGetopt.h"
#else
#include <getopt.h>
#endif

#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif


#ifdef _MSC_VER
#ifdef OPENMP
#define _OPENMP
#endif
#endif

#ifdef _OPENMP
#include <omp.h>
#endif

#include "slam6d/point_type.h"
#include "show/display.h"

/**
 * This vector contains the pointer to a vertex array for
 * all colors (inner vector) and all scans (outer vector)
 */
vector< vector<vertexArray*> > vvertexArrayList;

vector< ::SDisplay*> displays;
/**
 * the octrees that store the points for each scan
 */
//Show_BOctTree **octpts;
vector<colordisplay*> octpts;
/**
 * Storing the base directory
 */
string scandirectory;

/**
 * Storing the ID of the main windows
 */
int window_id;

/**
 * Size of points
 */
GLfloat pointsize          = 1.0;
int     anim_delay         = 5;

/**
 * Select Color Buffer
 */
GLenum buffermode = GL_BACK;

/**
 * Indicator whether and how the drawing window
 * has to be updated.
 *
 * haveToUpdate == 1 redisplay
 * haveToUpdate == 2 reshape
 * haveToUpdate == 3 animation scan matching
 * haveToUpdate == 4 stop animation scan matching
 * haveToUpdate == 6 path animation
 * haveToUpdate == 7 force redisplay with all points
 */
int haveToUpdate         = 0;

/**
 * Flag for invert the scene
 */
bool invert              = false;

/**
 * Flag for indicating brid eyes view
 */  
bool showTopView         = false;

/**
 * Flag for idicating camera add mode
 */
bool addCameraView       = false;         //Is the view in add box mode?

/**
 * Storing the apex angle of the camera
 */ 
GLfloat cangle          = 60.0;          // Current camera opening mode
GLfloat cangle_old      = cangle; 

/**
 * Current rotation axis of the scene as quaternion
 */
GLdouble quat[4]         ={0.0, 0.0, 0.0, 1.0};
GLdouble Rquat[4]         ={0.0, 0.0, 0.0, 1.0};

/**
 * Current translation of the scene 
 */
GLdouble X               = 0.0, Y               = 0.0, Z               = 0.0;
GLdouble RVX = 0.0, RVY = 0.0, RVZ = 0.0;

/**
 * parallel zoom (similar to apex angle) for parallel projection
 */
GLfloat pzoom = 2000.0;
GLfloat pzoom_old = pzoom; 


/**
 * Mode of the fog (exp, exp2, linear)
 */ 
GLint fogMode            = GL_EXP;

/**
 * Indicates if fog should be shown
 */
int show_fog             = 1;

/**
 * Indicates if the points should be shown
 */
int show_points          = 1;             // Show data points in the viewer?

/**
 * Indicates if camera boxes should be shown
 */
int show_cameras         = 1;             // Show the camera boxes in the viewer?

/**
 * Indicates if camera path or robot path should be shown
 */
int show_path            = 1;             // Show the camera movement path ?

/**
 * Camera navigation by mouse or by panel
 */
int cameraNavMouseMode  = 1;

int mouseNavX, mouseNavY;
int mouseNavButton = -1;

double mouseRotX = 0.0;
double mouseRotY = 0.0;
double mouseRotZ = 0.0;

bool keymap[256];

//@@@
//int animate_both         = 0;             // Animate both scan matchin and path?

int frameNr = 0;

/**
 * Storing of all transformation (frames for animation) of all scans
 */
vector < vector <double*> > MetaMatrix;

/**
 * Storing of AlgoType for all frames
 */
vector < vector <Scan::AlgoType> > MetaAlgoType;

/**
 * Window position
 */
int START_X              = 0;
int START_Y              = 0;
int START_WIDTH          = 720;
int START_HEIGHT         = 576;
GLdouble aspect          = (double)START_WIDTH/(double)START_HEIGHT;          // Current aspect ratio
bool advanced_controls = false;

bool fullscreen = false;
int current_width = START_WIDTH;
int current_height = START_HEIGHT;


// the following values are scale dependant, i.e. all values are in m
float neardistance = 0.10;
double oldneardistance = 0.10;
float maxfardistance = 400.0;; 
double fardistance = 400.0;
double oldfardistance = 40000.0;
double movementSpeed = 0.1;
double defaultZoom = 20.0;
GLfloat fogDensity       = 0.1;
double voxelSize = 0.20;


float adaption_rate = 1.0;
float LevelOfDetail = 0.0001;


// Defines for Point Semantic
#define TYPE_UNKNOWN         0x0000
#define TYPE_OBJECT          0x0001
#define TYPE_GROUND          0x0002 
#define TYPE_CEILING         0x0003 

unsigned int cam_choice             = 0;
  
static unsigned int path_iterator = 0;
static int oldcamNavMode = 0;

/**
 * Animation sould be saved to file
 */
int save_animation         = 0;
/**
 * If true, interpolation for camera path is based on distance, else always
 * the same number of images between two cameras.
 */
int inter_by_dist          = 1;

/**some variables for the camera path**/
vector<PointXY> path_vectorX, path_vectorZ,  lookat_vectorX, lookat_vectorZ, ups_vectorX, ups_vectorZ;
vector<Point> cams;
vector<Point> lookats;
vector<Point> ups;

NurbsPath cam_nurbs_path;
char *path_file_name;
char *pose_file_name;

/** Factor for saved image size */
int factor = 1;

/**
 * program tries to have this framerate
 */
float idealfps = 20.0;       
/**
 * value of the listBox fo Color Value and Colormap
 */
int listboxColorVal = 0;
int listboxColorMapVal = 0;
int colorScanVal = 0;
ScanColorManager *cm;
float mincolor_value = 0.0;
float maxcolor_value = 0.0;
//unsigned int  types = Point::USE_HEIGHT;
PointType pointtype;

/**
 * Contains the selected points for each scan
 */
set<sfloat *> *selected_points;
/**
 * Select single points?  
 */
int select_voxels         = 0;
/**
 * Select or unselect points ? 
 */
int selectOrunselect         = 1;
/** octree depth for selecting groups of points */
int selection_depth = 1;
int brush_size = 0;
char *selection_file_name;

int current_frame = 0;

#include "../show/show_menu.cc"
#include "../show/show_animate.cc"
#include "../show/show_gl.cc"

/**
 * Explains the usage of this program's command line parameters
 * @param prog name of the program
 */
void usage(char* prog)
{
#ifndef _MSC_VER
  const string bold("\033[1m");
  const string normal("\033[m");
#else
  const string bold("");
  const string normal("");
#endif
  
  cout << endl
	  << bold << "USAGE " << normal << endl
	  << "   " << prog << " [options] directory" << endl << endl;
  cout << bold << "OPTIONS" << normal << endl

	  << bold << "  -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
	  << "         end after scan NR" << endl
	  << endl
	  << bold << "  -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
	  << "         using shared library F for input" << endl
	  << "         (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl
	  << endl
	  << bold << "  -F" << normal << " NR, " << bold << "--fps=" << normal << "NR [default: 20]" << endl
	  << "         will attempt to display points with a framerate of NR" << endl
	  << endl
	  << bold << "  -l" << normal << " FILE, " << bold << "--loadObj=" << normal <<
    "FILE" << endl
	  << "         load objects specified in <FILE>" << endl
	  << endl
	  << endl
	  << bold << "  -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
	  << "         neglegt all data points with a distance larger than NR 'units'" << endl
	  << endl
	  << bold << "  -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
	  << "         neglegt all data points with a distance smaller than NR 'units'" << endl
	  << endl
	  << bold << "  -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
	  << "         use randomized octree based point reduction (pts per voxel=<NR>)" << endl
	  << "         requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
	  << endl
	  << bold << "  -o" << normal << " NR, " << bold << "--origin=" << normal << "NR (optional)" << endl
	  << "         sets the starting and reset position to: " << endl
	  << "           0 = the origin of the coordinate system (default)" << endl
	  << "           1 = the position of the first scan (default if --origin is in argument list)" << endl
	  << "           2 = the center of the first scan" << endl
	  << endl
	  << bold << "  -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
	  << "         turns on octree based point reduction (voxel size=<NR>)" << endl
	  << endl
	  << bold << "  -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
	  << "         start at scan NR (i.e., neglects the first NR scans)" << endl
	  << "         [ATTENTION: counting naturally starts with 0]" << endl
	  << endl
	  << bold << "  -S" << normal << " NR, " << bold << "--scale=" << normal << "NR" << endl
	  << "         scale factor to use (default: 0.01), modifies movement speed etc. " << endl
	  << "         use 1 when point coordinates are in m, 0.01 when in cm and so forth. " << endl
	  << "         " << endl
	  << endl
	  
    << bold << "  -R, --reflectance, --reflectivity" << normal << endl
	  << "         use reflectivity values for coloring point clouds" << endl
	  << "         only works when using octree display" << endl
	  << endl
    << bold << "  -a, --amplitude" << endl << normal
	  << "         use amplitude values for coloring point clouds" << endl
	  << "         only works when using octree display" << endl
	  << endl
    << bold << "  -d, --deviation" << endl << normal
	  << "         use amplitude values for coloring point clouds" << endl
	  << "         only works when using octree display" << endl
	  << endl
    << bold << "  -h, --height" << endl << normal
	  << "         use y-values for coloring point clouds" << endl
	  << "         only works when using octree display" << endl
	  << endl
    << bold << "  -T, --type" << endl << normal
	  << "         use type values for coloring point clouds" << endl
	  << "         only works when using octree display" << endl
    << bold << "  -c, --color" << endl << normal
	  << "         use color RGB values for coloring point clouds" << endl
    << bold << "  --saveOct" << endl << normal
	  << "         stores all used scans as octrees in the given directory" << endl
	  << "         All reflectivity/amplitude/deviation/type settings are stored as well." << endl
	  << "         only works when using octree display" << endl
    << bold << "  --loadOct" << endl << normal
	  << "         only reads octrees from the given directory" << endl
	  << "         All reflectivity/amplitude/deviation/type settings are read from file." << endl
	  << "         --reflectance/--amplitude and similar parameters are therefore ignored." << endl
	  << "         only works when using octree display" << endl
    << endl << endl;
  
  exit(1);
}


/**
 * A function that parses the command-line arguments and sets the respective flags.
 *
 * @param argc the number of arguments
 * @param argv the arguments
 * @param dir parsing result - the directory
 * @param start parsing result - starting at scan number 'start'
 * @param end parsing result - stopping at scan number 'end'
 * @param maxDist parsing result - maximal distance
 * @param minDist parsing result - minimal distance
 * @param readInitial parsing result -  read a file containing a initial transformation matrix
 * @param type parsing result - file format to be read
 * @return 0, if the parsing was successful, 1 otherwise 
 */
int parseArgs(int argc,char **argv, string &dir, int& start, int& end, int& maxDist, int& minDist, 
              double &red, bool &readInitial, int &octree, PointType &ptype, float &fps, string &loadObj, bool &loadOct, bool &saveOct, int &origin, double &scale, reader_type &type)
{
  unsigned int types = PointType::USE_NONE;
  start   = 0;
  end     = -1; // -1 indicates no limitation
  maxDist = -1; // -1 indicates no limitation
  int  c;
  // from unistd.h
  extern char *optarg;
  extern int optind;

  cout << endl;
  static struct option longopts[] = {
    { "origin",          optional_argument,   0,  'o' },
    { "format",          required_argument,   0,  'f' },  
    { "fps",             required_argument,   0,  'F' },  
    { "scale",           required_argument,   0,  'S' },
    { "start",           required_argument,   0,  's' },
    { "end",             required_argument,   0,  'e' },
    { "reduce",          required_argument,   0,  'r' },
    { "max",             required_argument,   0,  'm' },
    { "min",             required_argument,   0,  'M' },
    { "octree",          optional_argument,   0,  'O' },
    { "reflectance",     no_argument,         0,  'R' },
    { "reflectivity",    no_argument,         0,  'R' },
    { "amplitude",       no_argument,         0,  'a' },
    { "deviation",       no_argument,         0,  'd' },
    { "height",          no_argument,         0,  'h' },
    { "type",            no_argument,         0,  'T' },
    { "color",           no_argument,         0,  'c' },
    { "loadObj",         required_argument,   0,  'l' },
    { "saveOct",         no_argument,         0,  '0' },
    { "loadOct",         no_argument,         0,  '1' },
    { "advanced",        no_argument,         0,  '2' },
    { 0,           0,   0,   0}                    // needed, cf. getopt.h
  };

  //while ((c = getopt_long(argc, argv,"F:f:s:e:r:m:M:O:o:l:wtRadhTcC", longopts, NULL)) != -1)
  while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:b:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQp", longopts, NULL)) != -1)
    switch (c)
	 {
	 case 's':
	   start = atoi(optarg);
	   if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
	   break;
	 case 'e':
	   end = atoi(optarg);
	   if (end < 0)     { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
	   if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
	   break;
	 case 'm':
	   maxDist = atoi(optarg);
	   break;
	 case 'M':
	   minDist = atoi(optarg);
	   break;
	 case 'r':
	   red = atof(optarg);
	   break;
	 case 't':
	   readInitial = true;
	   break;
   case 'O':
     if (optarg) {
       octree = atoi(optarg);
     } else {
       octree = 1;
     }
     break;
	 case 'f':
     if (!Scan::toType(optarg, type))
       abort ();
	   break;
	 case '?':
     usage(argv[0]);
     return 1;
   case 'R':
     types |= PointType::USE_REFLECTANCE;
     break;
   case 'a':
     types |= PointType::USE_AMPLITUDE;
     break;
   case 'd':
     types |= PointType::USE_DEVIATION;
     break;
   case 'h':
     types |= PointType::USE_HEIGHT;
     break;
   case 'T':
     types |= PointType::USE_TYPE;
     break;
   case 'c':
     types |= PointType::USE_COLOR;
     break;
   case 'F':
     fps = atof(optarg);
     break;
   case 'S':
     scale = atof(optarg);
     break;
   case 'o':
     if (optarg) {
       origin = atoi(optarg);
     } else {
       origin = 1;
     }
     break;
   case '0':
     saveOct = true;
     break;
   case '1':
     loadOct = true;
     break;
   case 'l':
     loadObj = optarg;
     break;
   case '2':
     advanced_controls = true; 
     break;
   default:
     abort ();
   }

  if (optind != argc-1) {
      cerr << "\n*** Directory missing ***" << endl;
      usage(argv[0]);
  }
  dir = argv[optind];
  
#ifndef _MSC_VER
  if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
  if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif

  ptype = PointType(types);
  return 0;
}

void setResetView(int origin) {
  if (origin == 1) {
    double *transmat = MetaMatrix[0].back();
    cout << transmat << endl;

    RVX = -transmat[12];
    RVY = -transmat[13];
    RVZ = -transmat[14];
    Matrix4ToQuat(transmat, Rquat);
    X = RVX;
    Y = RVY;
    Z = RVZ;
    quat[0] = Rquat[0];
    quat[1] = Rquat[1];
    quat[2] = Rquat[2];
    quat[3] = Rquat[3];
  } else if (origin == 2) {
    double center[3];
#ifdef USE_COMPACT_TREE
    ((compactTree*)octpts[0])->getCenter(center);
#else
    ((Show_BOctTree<sfloat>*)octpts[0])->getCenter(center);
#endif
    RVX = -center[0];
    RVY = -center[1];
    RVZ = -center[2];
    X = RVX;
    Y = RVY;
    Z = RVZ;
  }
}

/*
 * A function that read the .frame files created by slam6D
 *
 * @param dir the directory
 * @param start starting at scan number 'start'
 * @param end stopping at scan number 'end'
 * @param read a file containing a initial transformation matrix and apply it
 */
int readFrames(string dir, int start, int end, bool readInitial, reader_type &type)
{

  double initialTransform[16];
  if (readInitial) {
    cout << "Initial Transform:" << endl;
    string initialTransformFileName = dir + "initital.frame";
    ifstream initial_in(initialTransformFileName.c_str());
    if (!initial_in.good()) {
      cout << "Error opening " << initialTransformFileName << endl;
      exit(-1);
    }
    initial_in >> initialTransform;
    cout << initialTransform << endl;
  }

  ifstream frame_in;
  int  fileCounter = start;
  string frameFileName;
  for (;;) {
    if (end > -1 && fileCounter > end) break; // 'nuf read
    frameFileName = dir + "scan" + to_string(fileCounter++,3) + ".frames";

    frame_in.open(frameFileName.c_str());

    // read 3D scan
    if (!frame_in.good()) break; // no more files in the directory

    cout << "Reading Frames for 3D Scan " << frameFileName << "...";
    vector <double*> Matrices;
    vector <Scan::AlgoType> algoTypes;
    int frameCounter = 0;

    while (frame_in.good()) {
      frameCounter++;	 
      double *transMatOpenGL = new double[16];
      int algoTypeInt;
      Scan::AlgoType algoType;
      try {
        double transMat[16];
        frame_in >> transMat >> algoTypeInt;
        algoType = (Scan::AlgoType)algoTypeInt;

        // convert to OpenGL coordinate system
        double mirror[16];
        M4identity(mirror);
        mirror[10] = -1.0;
        if (readInitial) {
          double tempxf[16];
          MMult(mirror, initialTransform, tempxf);
          memcpy(mirror, tempxf, sizeof(tempxf));
        }
        //@@@
        //	   memcpy(transMatOpenGL, transMat, 16*sizeof(double));
        MMult(mirror, transMat, transMatOpenGL);
      }
      catch (const exception &e) {   
        break;
      }
	 Matrices.push_back(transMatOpenGL);
	 algoTypes.push_back(algoType);
    }

    MetaAlgoType.push_back(algoTypes);
    MetaMatrix.push_back(Matrices);

    if((type == UOS_MAP || type == UOS_MAP_FRAMES || type == RTS_MAP) && fileCounter == start+1) {
      MetaAlgoType.push_back(algoTypes);
      MetaMatrix.push_back(Matrices);
    }

    frame_in.close();
    frame_in.clear();
    cout << MetaMatrix.back().size() << " done." << endl;
    current_frame = MetaMatrix.back().size() - 1;
  }
  if (MetaMatrix.size() == 0) {
    cerr << "*****************************************" << endl;
    cerr << "** ERROR: No .frames could be found!   **" << endl; 
    cerr << "*****************************************" << endl;
    cerr << " ERROR: Missing or empty directory: " << dir << endl << endl;
    return -1;
  }
  return 0;
}

void generateFrames(int start, int end, bool identity) {
  if (identity) {
    cout << "using Identity for frames " << endl;
  } else {
    cout << "using pose information for frames " << endl;
  }
  int  fileCounter = start;
  int index = 0;
  for (;;) {
    if (fileCounter > end) break; // 'nuf read
    fileCounter++;

    vector <double*> Matrices;
    vector <Scan::AlgoType> algoTypes;

    for (int i = 0; i < 3; i++) {
      double *transMat = new double[16];
      
      if (identity) {
        M4identity(transMat);
        transMat[10] = -1.0;
      } else {
        EulerToMatrix4(Scan::allScans[index]->get_rPos(), Scan::allScans[index]->get_rPosTheta(), transMat ); 
      }

      Matrices.push_back(transMat);
      algoTypes.push_back(Scan::ICP);

    }
    index++;
    MetaAlgoType.push_back(algoTypes);
    MetaMatrix.push_back(Matrices);
  }
}



/*
 * create display lists
 * @to do general framework for color & type definitions
 */
void createDisplayLists(bool reduced)
{
  for(unsigned int i = 0; i < Scan::allScans.size() ; i++) {

    // count points
    int color1 = 0, color2 = 0;
    if (!reduced) {
      for (unsigned int jterator = 0; jterator < Scan::allScans[i]->get_points()->size(); jterator++) {
        if (Scan::allScans[i]->get_points()->at(jterator).type & TYPE_GROUND) {
          color1++;
        } else {
          color2++;
        }
      }
    } else {
      color2 = 3* Scan::allScans[i]->get_points_red_size();
    }
    
    // allocate memory
    vertexArray* myvertexArray1 = new vertexArray(color1);
    vertexArray* myvertexArray2 = new vertexArray(color2);

    // fill points
    color1 = 0, color2 = 0;
    if (reduced) {
      for (int jterator = 0; jterator < Scan::allScans[i]->get_points_red_size(); jterator++) {
        myvertexArray2->array[color2] = Scan::allScans[i]->get_points_red()[jterator][0];
        myvertexArray2->array[color2+1] = Scan::allScans[i]->get_points_red()[jterator][1];
        myvertexArray2->array[color2+2] = Scan::allScans[i]->get_points_red()[jterator][2];
          color2 += 3;
      }
    } else {
      for (unsigned int jterator = 0; jterator < Scan::allScans[i]->get_points()->size(); jterator++) {
        if (Scan::allScans[i]->get_points()->at(jterator).type & TYPE_GROUND) {
          myvertexArray1->array[color1] = Scan::allScans[i]->get_points()->at(jterator).x;
          myvertexArray1->array[color1+1] = Scan::allScans[i]->get_points()->at(jterator).y;
          myvertexArray1->array[color1+2] = Scan::allScans[i]->get_points()->at(jterator).z;
          color1 += 3;
        } else {
          myvertexArray2->array[color2] = Scan::allScans[i]->get_points()->at(jterator).x;
          myvertexArray2->array[color2+1] = Scan::allScans[i]->get_points()->at(jterator).y;
          myvertexArray2->array[color2+2] = Scan::allScans[i]->get_points()->at(jterator).z;
          color2 += 3;
        }
      }
    }

    glNewList(myvertexArray1->name, GL_COMPILE);
    //@
    //glColor4d(0.44, 0.44, 0.44, 1.0);	
    //glColor4d(0.66, 0.66, 0.66, 1.0);	
    glVertexPointer(3, GL_FLOAT, 0, myvertexArray1->array);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_POINTS, 0, myvertexArray1->numPointsToRender);
    glDisableClientState(GL_VERTEX_ARRAY);
    glEndList();	

    glNewList(myvertexArray2->name, GL_COMPILE);
    //glColor4d(1.0, 1.0, 1.0, 1.0);	
    //glColor4d(0.0, 0.0, 0.0, 1.0);	
    glVertexPointer(3, GL_FLOAT, 0, myvertexArray2->array);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_POINTS, 0, myvertexArray2->numPointsToRender);
    glDisableClientState(GL_VERTEX_ARRAY);
    glEndList();	
    
    // append to vector
    vector<vertexArray*> vvertexArray;
    vvertexArray.push_back(myvertexArray1);
    vvertexArray.push_back(myvertexArray2);
    vvertexArrayList.push_back(vvertexArray);
  }

}

void cycleLOD() {
  LevelOfDetail = 0.00001;
  for (unsigned int i = 0; i < octpts.size(); i++)
    octpts[i]->cycleLOD();
}


void initShow(int argc, char **argv){

  /***************/
  /* init OpenGL */
  /***************/
  glutInit(&argc,argv);
  
  cout << "(wx)show - A highly efficient 3D point cloud viewer" << endl
       << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl
	  << "    University of Osnabrueck, Germany, 2006 - 2009" << endl << endl;

  if(argc <= 1){
    usage(argv[0]);
  }



  double red   = -1.0;
  int start = 0, end = -1, maxDist = -1, minDist = -1;
  string dir;
  bool readInitial = false;
  reader_type type  = UOS;
  int octree = 0;
  bool loadOct = false;
  bool saveOct = false;
  string loadObj;
  int origin = 0;
  double scale = 0.01; // in m

  pose_file_name = new char[1024];
  path_file_name = new char[1024];
  selection_file_name = new char[1024];
   
  strncpy(pose_file_name, "pose.dat", 1024);  
  strncpy(path_file_name, "path.dat", 1024);  
  strncpy(selection_file_name, "selected.3d", 1024);  
  
  parseArgs(argc, argv, dir, start, end, maxDist, minDist, red, readInitial,
  octree, pointtype, idealfps, loadObj, loadOct, saveOct, origin, scale, type);

  // modify all scale dependant variables
  scale = 1.0 / scale; 
  movementSpeed *= scale;
  neardistance *= scale;
  oldneardistance *= scale;
  maxfardistance *= scale;
  fardistance *= scale;
  fogDensity /= scale;
  defaultZoom *= scale;
  voxelSize *= scale;
//  oldfardistance *= scale;

  ////////////////////////
  SDisplay::readDisplays(loadObj, displays);
  ////////////////////

  if (type == OCT) {
    loadOct = true;
  }

  // if we want to load display file get pointtypes from the files first
  if (loadOct) {
    string scanFileName = dir + "scan" + to_string(start,3) + ".oct";
    cout << "Getting point information from " << scanFileName << endl;
    cout << "Attention! All subsequent oct-files must be of the same type!" << endl;

    pointtype = BOctTree<sfloat>::readType(scanFileName);
  }
  scandirectory = dir;

  // init and create display
  M4identity(view_rotate_button);
  obj_pos_button[0] = obj_pos_button[1] = obj_pos_button[2] = 0.0;

  // read frames first, to get notifyied of missing frames before all scans are read in
  int r = readFrames(dir, start, end, readInitial, type);


  // Get Scans
  if (!loadOct) {
#ifndef DYNAMIC_OBJECT_REMOVAL
    Scan::readScans(type, start, end, dir, maxDist, minDist, 0);
#else
    VeloScan::readScans(type, start, end, dir, maxDist, minDist, 0);
#endif
  } else {
    cout << "Skipping files.." << endl;
  }

  if (!loadOct) {
    if (r) generateFrames(start, start + Scan::allScans.size() - 1, false); 
  } else {
    if (r) generateFrames(start, start + octpts.size() - 1, true); 
  }
  
  int end_reduction = (int)Scan::allScans.size();
  #ifdef _OPENMP
  #pragma omp parallel for schedule(dynamic)
  #endif
  for (int iterator = 0; iterator < end_reduction; iterator++) {
    // reduction filter for current scan!
    if (red > 0) {
	    cout << "Reducing Scan No. " << iterator << endl;
      // TODO do another reduction so reflectance values etc are carried over
      Scan::allScans[iterator]->calcReducedPoints(red, octree);
    } // no copying necessary for show!
  }

  cm = new ScanColorManager(4096, pointtype);
  
  if (loadOct) {
    for (int i = start; i <= end; i++) {
      string scanFileName = dir + "scan" + to_string(i,3) + ".oct";
      cout << "Reading octree " << scanFileName << endl;
#ifdef USE_COMPACT_TREE
      octpts.push_back(new compactTree(scanFileName, cm));
#else
      octpts.push_back(new Show_BOctTree<sfloat>(scanFileName, cm));
#endif
    }
  } else {
#if USE_COMPACT_TREE
    cout << "Creating compact display octrees.." << endl;
    for(int i = 0; i < (int)Scan::allScans.size() ; i++) {
      compactTree *tree;
      if (red > 0) {
        tree = new compactTree(Scan::allScans[i]->get_points_red(), Scan::allScans[i]->get_points_red_size(), voxelSize, pointtype, cm);  // TODO remove magic number
      } else {
        unsigned int nrpts = Scan::allScans[i]->get_points()->size();
        sfloat **pts = new sfloat*[nrpts];
        for (unsigned int jterator = 0; jterator < nrpts; jterator++) {
          pts[jterator] = pointtype.createPoint<sfloat>(Scan::allScans[i]->get_points()->at(jterator));
        }
        Scan::allScans[i]->clearPoints();
        tree = new compactTree(pts, nrpts , voxelSize, pointtype, cm);  //TODO remove magic number
        for (unsigned int jterator = 0; jterator < nrpts; jterator++) {
          delete[] pts[jterator];
        }
        delete[] pts;
      }
      if (saveOct) {
        string scanFileName = dir + "scan" + to_string(i+start,3) + ".oct";
        cout << "Saving octree " << scanFileName << endl;
        tree->serialize(scanFileName);
      }
      octpts.push_back(tree);
      cout << "Scan " << i << " octree finished. Deleting original points.." << endl;
    }
#else
    cout << "Creating display octrees.." << endl;
    for(int i = 0; i < (int)Scan::allScans.size() ; i++) {
      Show_BOctTree<sfloat> *tree;
      if (red > 0) {
        tree = new Show_BOctTree<sfloat>(Scan::allScans[i]->get_points_red(), Scan::allScans[i]->get_points_red_size(), voxelSize, pointtype, cm);  // TODO remove magic number
      } else {
        unsigned int nrpts = Scan::allScans[i]->get_points()->size();
        sfloat **pts = new sfloat*[nrpts];
        for (unsigned int jterator = 0; jterator < nrpts; jterator++) {
          pts[jterator] = pointtype.createPoint<sfloat>(Scan::allScans[i]->get_points()->at(jterator));
        }
        Scan::allScans[i]->clearPoints();
        tree = new Show_BOctTree<sfloat>(pts, nrpts , voxelSize, pointtype, cm);  //TODO remove magic number
        for (unsigned int jterator = 0; jterator < nrpts; jterator++) {
          delete[] pts[jterator];
        }
        delete[] pts;
      }
      octpts.push_back(tree);
      if (saveOct) {
        string scanFileName = dir + "scan" + to_string(i+start,3) + ".oct";
        cout << "Saving octree " << scanFileName << endl;
        tree->serialize(scanFileName);
      }
      cout << "Scan " << i << " octree finished. Deleting original points.." << endl;
    }
#endif
  }

  cm->setCurrentType(PointType::USE_HEIGHT);
  //ColorMap cmap;
  //cm->setColorMap(cmap);
  resetMinMax(0);

  selected_points = new set<sfloat*>[octpts.size()];
  
  // sets (and computes if necessary) the pose that is used for the reset button
  setResetView(origin);

  for (unsigned int i = 0; i < 256; i++) {
    keymap[i] = false;
  }
}

/**
 * Main function.
 * Reads the scan (scan000.3d, ...) and frames files (scan000.frames, ...) from the data directory.
 * The frames are used for animation of the matching process.
 */

int main(int argc, char **argv){

  initShow(argc, argv);
  initScreenWindow();

  newMenu();
  glutMainLoop();
}


void updateCamControls() {
  cam_spinner->set_int_limits( 1, cams.size());
  cam_spinner->set_int_val(cam_choice);
}

void resetRotationButton() {
  rotButton->reset();
}

void updateTopViewControls() {
  if(showTopView) {
    pzoom_spinner->enable();
    cangle_spinner->disable();
  } else {
    pzoom_spinner->disable();
    cangle_spinner->enable();
  }
}


void updateControls() {
  glui1->sync_live();
  glui1->show();
  glui2->sync_live();
  glui2->show();
}

static bool interrupted = false;
void interruptDrawing() {
  interrupted = true;
}
void checkForInterrupt() {
  interrupted = false;
}
bool isInterrupted() {
#ifdef WITH_FREEGLUT
#ifndef __APPLE__
  glutMainLoopEvent();
#endif
#endif
  glutSetWindow(window_id);
  return interrupted;
}

void updatePointModeControls() {
  switch(pointmode) {
    case -1:
      always_box->set_int_val(0);
      never_box->set_int_val(1);
      break;
    case 0:
      always_box->set_int_val(0);
      never_box->set_int_val(0);
      break;
    case 1:
      always_box->set_int_val(1);
      never_box->set_int_val(0);
      break;
  }
}