//============================================================================================//
//    The OpenCV-CamHeadSystem-Project                                                   
//    by                                                                   
//    Rytta Communications Inc.                                           
//    (c) Version 0.1. June 2009                                       
//
//    EXPERIMENTAL                                                             
//===========================================================================================//
//    headtrack01.cpp 
//    ---------------
//
//    begin     : Mon. 29. June 10.30:00 GMT 2009                           
//    copyright : (C) 2008/2009 by  s.morf                                         
//    email     : 
//
//    compile with   
//    opencv & opengl: 
//    g++ -I/usr/include/GL -I/usr/local/include/opencv -L /usr/local/lib -lcxcore -lcv -lhighgui -lcvaux -lml -I/usr/X11R6/include -L/usr/X11R6/lib -o headtrack01 headtrack01.cpp -lglui -lglut -lGLU -lGL                 
   
//    run as    : ./headtrack01
//
//    for: Basics of: ComputerVisions with logitech QuickCam(s) E3500 Plus
//         - Base from lk_glDemo3.cpp         (lucasKanade-Algo)
//         - with OpenGL-Unterstuetung Living 2D-Textures (WebCam-Clip)
//         - lucas-Kanade Algorithmus with selectable MousePoints
//                         
//    ONLY RESEARCH
//   
/**********************************************************************************
  References
           
  verwendete programme:
  from /root/Desktop/OPENCV/opencv-tutorial.pdf (german)
  <Bildverarbeitung mit OpenCV, author Johannes Wienke, Uni Bielefeld 2008>

OpenCV_Beispiele From RoboWeb
Mehrere Bilder in einem Fenster
from http://opencvlibrary.sourceforge.net/DisplayManyImages.

  Display video from webcam

  lucasKanadeAlgorithmus: from
  cvLkDemo1.cpp from samples/c/ (lkdemo.c -> erkennt face (frontal)
  "Learning OpenCV-Book"

  Author  Nash
  License GPL
  Website http:/nashruddin.com

  OpticalFlow from
  David Stavens
  Standford Artificial Intelligence Lab (PDF)
 
  This file demonstrates how to stream live video from a video capture
  device to an OpenGL texture using OpenCV and GLUT.

  Copyright (C) 2007 Julian Oliver
  http://julianoliver.com
 
  Modifications
  Copyright (C) 2008 Jean-Philippe Deblonde
  http://www.lifl.fr/~deblonde/

/************************************************************************************************
REFERNECE STARTING WEBCAM:
1) on console
   if you have to reset you cam for any reason, do:
   1. unplug your cam
   2. modprobe -r uvcvideo
   3. modprobe -r snd_usb_audio
   4. replug your cam 

/************************************************************************************************
   THIS PROGRAMM IS UNDER THE GNU-LICENCE                             
/**************************************************************************************************** 
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

**************************************************************************************************/

#include<iostream>

// openCV
#include<cv.h>
#include<cvaux.h>
#include<highgui.h>
// openGL & GLUI
#include <GL/glut.h>   // into usr/include/GL/
#include <GL/glui.h>   // into usr/include/GL/

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

#include <fcntl.h>
#include <string.h>
#include <ctype.h>

#include <setjmp.h>
#include <termios.h>
#include <unistd.h>
#include <sys/signal.h>
#include <sys/types.h>
#include <errno.h>

#include <ctype.h>
#include <sys/io.h>
// glui
#include <sys/stat.h>
#include <stdarg.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <time.h>

#include <cstdlib>
#include <cmath>
#include <stdio.h>
#include <ctype.h>
#include <vector>

#include <sstream>

using namespace std;

static float aspect_ratio = 1.0f;

static const double pi = 3.14159265358979323846;

#define IsRGB(s) ((s[0] == 'R') && (s[1] == 'G') && (s[2] == 'B'))
#define IsBGR(s) ((s[0] == 'B') && (s[1] == 'G') && (s[2] == 'R'))

#ifndef GL_CLAMP_TO_BORDER
#define GL_CLAMP_TO_BORDER 0x812D
#endif
#define GL_MIRROR_CLAMP_EXT 0x8742

#define MAX_FET 10  // 40

int key = 0;

int x = 640;
int y = 480;

bool boolInit;
bool boolNightMode = false;
bool boolSelectedPoints = false;

// from lk_glDemo3.cpp
int height, width, counts, init;
float fps, rot;
CvCapture *capture = 0;
GLenum format;
GLuint imageID;

IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp;
IplImage* trace;

int win_size = 10;
const int MAX_COUNT = 500;
CvPoint2D32f* points[2] = {0,0}, *swap_points;

// lucasKanade Points
CvPoint myLKPointsArr[3]; // demo4 [16]; //[10];

char* status = 0;
int countLK = 0;
int need_to_init = 0;
int night_mode = 0;
int flags = 0;
int add_remove_pt = 0;
CvPoint pt;
int i, k, c;
int ii;

bool boolLucasKanade;
bool setting_LuKan2dPoints;
bool boolSetPoints;

struct points
  {
     GLfloat x;
     GLfloat y;
     GLfloat z;
  } gl3dPoints[3];   

GLfloat centreScreenX = 320.0f;
GLfloat centreScreenY = 240.0f;   

int mouseClick = 0;
int win_x,win_y,win_z;
int z = 0;

// USER INTERFACE GLOBALS
int LeftButtonDown=0;    // MOUSE STUFF
int OldX,OldY,NewX,NewY;

// 3dAxis rotations 
static GLfloat axisTurnX;
static GLfloat axisTurnY;
static GLfloat axisTurnZ;

int window1 = 0;
int window2 = 0;

int plmx,plmy; // simMousePos for 3dof Plattform turns

struct point
  {
     int x;
     int y;
  } gl2dPoint[3];

/*** functions *********************************************************************************/

void window2_Init3dAxis();

/*** special Func (1) conio.h ******************************************************************/
void gotoxy(int x, int y) { printf("\033[%d;%dH", y, x); fflush(stdout); }
void clrscr(void)         { printf("\033[H\033[J");      fflush(stdout); }

// delay (nnn)

void delay(long millisek)
{
   int mikrosek = millisek*1000;
   struct timeval  timeout;
   
   timeout.tv_sec  = mikrosek / 1000000L;
   timeout.tv_usec = mikrosek % 1000000L;
   select(0, NULL, NULL, NULL, &timeout);
}
/*************************************************************************************************/
void gltDrawUnitAxes(void)
{
   GLUquadricObj *pObj; // Temporary, used for quadrics
   
   // Measurements
   float fAxisRadius = 0.025f;
   float fAxisHeight = 1.0f;
   float fArrowRadius = 0.06f;
   float fArrowHeight = 0.1f;
   
   // Setup the quadric object
   pObj = gluNewQuadric();
   gluQuadricDrawStyle(pObj, GLU_FILL);
   gluQuadricNormals(pObj, GLU_SMOOTH);
   gluQuadricOrientation(pObj, GLU_OUTSIDE);
   gluQuadricTexture(pObj, GLU_FALSE);
   ///////////////////////////////////////////////////////
   // Draw the blue Z axis first, with arrowed head
   glColor3f(0.0f, 0.0f, 1.0f);
   gluCylinder(pObj, fAxisRadius, fAxisRadius, fAxisHeight, 10, 1);
   glPushMatrix();
   glTranslatef(0.0f, 0.0f, 1.0f);
   gluCylinder(pObj, fArrowRadius, 0.0f, fArrowHeight, 10, 1);
   // original old
   // glRotatef(180.0f, 1.0f, 0.0f, 0.0f);
   glRotatef(axisTurnZ, 1.0f, 0.0f, 0.0f);

   gluDisk(pObj, fAxisRadius, fArrowRadius, 10, 1);
   glPopMatrix();
   ///////////////////////////////////////////////////////
   // Draw the Red X axis 2nd, with arrowed head
   glColor3f(1.0f, 0.0f, 0.0f);
   glPushMatrix();
   glRotatef(90.0f, 0.0f, 1.0f, 0.0f);
   gluCylinder(pObj, fAxisRadius, fAxisRadius, fAxisHeight, 10, 1);
   glPushMatrix();
   glTranslatef(0.0f, 0.0f, 1.0f);
   gluCylinder(pObj, fArrowRadius, 0.0f, fArrowHeight, 10, 1);
   // original
   // glRotatef(180.0f, 0.0f, 1.0f, 0.0f);
   glRotatef(axisTurnY, 0.0f, 1.0f, 0.0f);
   
   gluDisk(pObj, fAxisRadius, fArrowRadius, 10, 1);
   glPopMatrix();
   glPopMatrix();
   ///////////////////////////////////////////////////////
   // Draw the Green Y axis 3rd, with arrowed head
   glColor3f(0.0f, 1.0f, 0.0f);
   glPushMatrix();
   glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
   gluCylinder(pObj, fAxisRadius, fAxisRadius, fAxisHeight, 10, 1);
   glPushMatrix();
   glTranslatef(0.0f, 0.0f, 1.0f);
   gluCylinder(pObj, fArrowRadius, 0.0f, fArrowHeight, 10, 1);
   // original
   // glRotatef(180.0f, 1.0f, 0.0f, 0.0f);
   glRotatef(axisTurnX, 1.0f, 0.0f, 0.0f);
   
   gluDisk(pObj, fAxisRadius, fArrowRadius, 10, 1);
   glPopMatrix();
   glPopMatrix();
   ////////////////////////////////////////////////////////
   // White Sphere at origin
   glColor3f(1.0f, 1.0f, 1.0f);
   gluSphere(pObj, 0.05f, 15, 15);
   
   // Delete the quadric
   gluDeleteQuadric(pObj);
}
/************************************************************************************************/
void window1_glutMouse(int button, int state, int x, int y)
  {
     int win_x,win_y,win_z;
     int z = 0;
     
     if( !image )
        return;
     
     if( image->origin )
        y = image->height - y;
     
     if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN  )
        {
           pt = cvPoint(x,y);
           add_remove_pt = 1;
        }
  }
/************************************************************************************************/
// helpText
void initText()
  {
     clrscr();
     gotoxy(2,1);
     printf("Head-Initialize\n");
     gotoxy(2,2);
     printf("Look into WebCam - Your Nose at red MiddlePoint\n");
     gotoxy(2,3);
     printf("Setting LucasKanade Points:\n");
     gotoxy(2,4);
     printf(" 1.MouseClick on the rand of your leftEyeGlasses\n");
     gotoxy(2,5);
     printf(" 2.MouseClick on the rand of your rightEyeGlasses\n");
     gotoxy(2,6);
     printf(" 3.MouseClick on Center at your Nose\n");
  }
/***************************************************************************************/
void window2_MotionCalcXY()
  {
     NewX = plmx;    // is the last lucasKanade-Point   
     NewY = plmy;    // is the last lucasKanadePoint
     
     printf("NewX : %d NewY: %d\n",NewX,NewY);
     printf("OldX : %d OldY: %d\n",OldX,OldY);

     float RelX = (NewX-OldX) / (float)glutGet((GLenum)GLUT_WINDOW_WIDTH);
     float RelY = (NewY-OldY) / (float)glutGet((GLenum)GLUT_WINDOW_HEIGHT);
     
     printf("RELX: %f Deg.   RELY: %f Deg\n", RelX,RelY);

     axisTurnX = axisTurnX + RelX * 180;
     axisTurnY = axisTurnY + RelY * -180;
     axisTurnZ = axisTurnZ + RelY * -90;

     printf("Into Calc AxisTurnX: %f Deg, AxisTurnY: %f Deg, AxisTurnZ: %f Deg\n",axisTurnX,axisTurnY,axisTurnZ);
     printf("\n\n*************************************************\n");

      OldX = NewX;
      OldY = NewY;

  }
/*** LUCAS-KANADE **********************************************************************/
void lucasKanade(IplImage* frame)
  {
     if( !image )
        {
           // allocate all the buffers
           image = cvCreateImage( cvGetSize(frame), 8, 3 );

           // muss drinn sein
           // cvResize(frame, image); // have to scale to power of two. will work out padding to avoid scale distortion later.
           image->origin = frame->origin;
      
           grey = cvCreateImage( cvGetSize(frame), 8, 1 );
           prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
           pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
           prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
           points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
           points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
           status = (char*)cvAlloc(MAX_COUNT);
           flags = 0;
        }

     cvCopy( frame, image, 0 );
     cvCvtColor( image, grey, CV_BGR2GRAY );

     if( boolNightMode || night_mode )
        cvZero( image );
     
     if(countLK < 3)  // kann auch 2 sein
        {
           // here Initialize HeadDirection RED
           cvRectangle( image,cvPoint(160,60),
                        cvPoint(460,410),
                        cvScalar(0,0,255)
                      );
           
           // fadenkreuz vertikal
           cvLine( image,cvPoint(320,60),
                   cvPoint(320,410),
                   cvScalar(0,0,255),
                   1, 8
                 );
           
           // fadenkreuz horizontal
           cvLine( image,cvPoint(160,240),
                   cvPoint(460,240),
                   cvScalar(0,0,255),
                   1, 8
                 );
           
        } // end Inital
       
     if( need_to_init )
        {
           printf("NEED TO INIT\n");

           // automatic initialization
           IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
           IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
           double quality = 0.01;
           double min_distance = 10;
           
           countLK = MAX_COUNT;
           
           cvGoodFeaturesToTrack( grey, eig, temp, points[1], &countLK,
                                  quality, min_distance, 0 );     

           cvFindCornerSubPix( grey, points[1], countLK,
                               cvSize(win_size,win_size), cvSize(-1,-1),
                               cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
        }
     else if( countLK > 0 )
        {
           printf("Count_LK: %d\n",countLK);
           
           cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
                                   points[0], points[1], countLK,
                                   cvSize(win_size,win_size), 3, status, 0,
                                   cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,
                                   20,0.03), flags
                                 );
               
           for( i = k = 0; i < countLK; i++ )
              {
                 if( add_remove_pt )
                    {
                       double dx = pt.x - points[1][i].x;
                       double dy = pt.y - points[1][i].y;
                     
                       if( dx*dx + dy*dy <= 25 )
                          {
                             add_remove_pt = 0;
                             continue;
                          }
                    }
               
                 if( !status[i] )
                    continue;

                 points[1][k++] = points[1][i];
                 
                 // original function
                 cvCircle( image, cvPointFrom32f(points[1][i]), 4, CV_RGB(0,255,255), -1, 8,0);

                 // here working with points & testing
                 ////
                 CvPoint mypoint;
                 mypoint.x = points[0][i].x;
                 mypoint.y = points[1][i].y;

                 printf("LK_X: %d, Y: %d\n",mypoint.x,mypoint.y);
                 
                 cvRectangle( image,cvPoint(mypoint.x,mypoint.y),
                              cvPoint(mypoint.x+10,mypoint.y+10),
                              cvScalar(255,0,255)
                            );
                 // new eigene lk-mouse-points
                 myLKPointsArr[i].x = mypoint.x;
                 myLKPointsArr[i].y = mypoint.y;

                 printf("\nXXXXXXXXXXXXXXXXXXXXXXXXXX\n");
           
                 if(countLK == 1)
                    {
                       myLKPointsArr[0].x = mypoint.x;
                       myLKPointsArr[0].y = mypoint.y;
 
                       // for Rotations
                       gl2dPoint[0].x = mypoint.x;
                       gl2dPoint[0].y = mypoint.y; 
                     
                       printf("Click: %d, gl2dPoint[0].X: %d, gl2dPoint[0].Y: %d\n",countLK,gl2dPoint[0].x,gl2dPoint[0].y);

                       // here einfaerben: yellow
                       cvCircle( image, cvPoint(gl2dPoint[0].x,gl2dPoint[0].y), 5, CV_RGB(255,255,0), -1, 8,0);
                    }
                 if(countLK == 2)
                    {
                       // for Rotations
                       gl2dPoint[1].x = mypoint.x;
                       gl2dPoint[1].y = mypoint.y; 
                 
                       printf("Click: %d, gl2dPoint[1].X: %d, gl2dPoint[1].Y: %d\n",countLK,gl2dPoint[1].x,gl2dPoint[1].y);

                       // here einfaerben: yellow
                       cvCircle( image, cvPoint(gl2dPoint[1].x,gl2dPoint[1].y), 5, CV_RGB(255,255,0), -1, 8,0);
                    }
                 if(countLK == 3)
                    {
                       // for Rotations
                       gl2dPoint[2].x = mypoint.x;
                       gl2dPoint[2].y = mypoint.y; 

                       printf("Click: %d, gl2dPoint[2].X: %d, gl2dPoint[2].Y: %d\n",countLK,gl2dPoint[2].x,gl2dPoint[2].y);

                       // here einfaerben: yellow
                       cvCircle( image, cvPoint(gl2dPoint[2].x,gl2dPoint[2].y), 5, CV_RGB(255,255,0), -1, 8,0);
                                         
                       boolSetPoints = true;     

                       /////////
                       plmx = gl2dPoint[2].x;  // old: plmx = wx;
                       plmy = gl2dPoint[2].y;  // old: plmy = wy;
                       printf("\nPLMX: %d, PLMY: %d\n",plmx,plmy);

                    }
                 if(boolSetPoints == true)
                    {
                       printf("\nBoolSetPoints True\n");
                       printf("OldX: %d, OldY: %d\n",OldX,OldY);
                       printf("PLMX: %d, PLMY: %d\n",plmx,plmy);

                       printf("LucasKanade: Before: AxisTurns: X: %f, Y: %f, Z: %f\n",axisTurnX,axisTurnY,axisTurnZ);
               
                       // here 3D-Axis 3DOF
                       window2_MotionCalcXY();

                       printf("LucasKanade: After: AxisTurns: X: %f, Y: %f, Z: %f\n",axisTurnX,axisTurnY,axisTurnZ);
                       
                       glutPostRedisplay();
                    }   

              }  // end for-loop
           
           countLK = k;
        }
     
     if( add_remove_pt && countLK < MAX_COUNT )
        {
           points[1][countLK++] = cvPointTo32f(pt);
           
           cvFindCornerSubPix( grey, points[1] + countLK - 1, 1,
                               cvSize(win_size,win_size), cvSize(-1,-1),
                               cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)
                             );
             
           add_remove_pt = 0;
        }

     CV_SWAP( prev_grey, grey, swap_temp );
     CV_SWAP( prev_pyramid, pyramid, swap_temp );
     CV_SWAP( points[0], points[1], swap_points );
     need_to_init = 0;
     
     glutPostRedisplay();
     
  }
/************************************************************************************************/
void window1_Draw()
  {
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
     IplImage *frame = 0;

     frame = cvQueryFrame( capture );

     // NEW STUFF without boolean
     // here, we do the real LucasKanade stuff on the image grabbed from the camera

     lucasKanade(frame);

     cvSaveImage( "lk_image.jpg",image ); 
     need_to_init = 0; 
     
     glLoadIdentity();
     glTranslatef(0.0f, 0.0f,-1.0f); 

     // must be done else wrong upside down (steht auf dem kopf
     glRotatef(180.0f, 1, 0, 0); // rotate the matrix to rotate the video texture.

     GLenum format = IsBGR(image->channelSeq) ? GL_BGR_EXT : GL_RGBA;

     // upload to openGL texture
     if(!init && !boolInit)
        {
           glGenTextures(1, &imageID);
           glBindTexture(GL_TEXTURE_2D, imageID);
           glPixelStorei(GL_UNPACK_ALIGNMENT, 1);

           // GL_LINEAR is better looking than GL_NEAREST but seems slower..
           // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
           // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
           glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
           glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
           glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image->width, image->height, 0, format, GL_UNSIGNED_BYTE, image->imageData);
           
           init= 1;         // init lucas-kanade is over
           boolInit = true;  // init opticalFlow is over
        }
     else
        {
           glBindTexture(GL_TEXTURE_2D, imageID);
           glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, image->width, image->height, format, GL_UNSIGNED_BYTE, image->imageData);
        }
     
     glEnable(GL_TEXTURE_2D);

     // make a quad
       glBegin(GL_QUADS);
        glTexCoord2f(0, 0); glVertex3f(-1, -1, 0);
        glTexCoord2f(1, 0); glVertex3f(1, -1, 0);
        glTexCoord2f(1, 1); glVertex3f(1, 1, 0);
        glTexCoord2f(0, 1); glVertex3f(-1, 1, 0);
     glEnd();
     
     glLoadIdentity();
     glutSwapBuffers();// swap the draw buffer
     
     glFlush();

     // stopping make texture

     glDisable(GL_TEXTURE_2D);

     // texture stop
     // here can do opengl-drawing
   
     if(setting_LuKan2dPoints == false)
        {
          // initText();
        }

     if(setting_LuKan2dPoints == true)
        {
           // ev. glDrawing ???
        }
     
     // glutPostRedisplay();

  }
/************************************************************************************************/
// help-functions from demo 54
void exitGL(int returnCode)
  {
     exit(returnCode);
  }
/************************************************************************************************/
// ex glutKeyboard from demo 54
// glutKeyboardFunc(window1_KeyboardFunc);
void window1_KeyboardFunc(unsigned char key,int x,int y)
  {
     if(key == 27)
        exitGL(0);

     switch(key)
        {
           case 'r':  // funzzt nich seqfault wegen own selected points
              // need_to_init = 1;
              boolInit = false;
           break;
           case 'c':
              countLK = 0;
              boolSelectedPoints = false;
              printf("Please Setting the LK-Points\n");
              initText();

              window2_Init3dAxis();
           break;
           case 'n':
              night_mode ^= 1;
              boolNightMode ^= true;
           break;
           case '1': 
              printf("Pressed <1>\n");
           break;
           case '2':
              printf("Pressed <2>\n");
           break;
           case '3':
              printf("Pressed <3> = State Stop\n");
             // bool_makePic = false;
           break;
           
        } // end switch Key 
  }
/************************************************************************************************/
void window1_glutReshape(int width,int height)
  {
     int winWidth  = 640;
     int winHeight = 480;
     glViewport(0,0,width,height);
     
     // set projection matrix
     glMatrixMode(GL_PROJECTION);
     glLoadIdentity();

     // new wegen -> verschobenen myPoints
     gluPerspective(0.0f,(GLfloat)width/(GLfloat)height,0.1f,200.0f);

     // switch back to modelview matrix
     glMatrixMode(GL_MODELVIEW);
     glLoadIdentity();
  }
/************************************************************************************************/
// eigentlich only 3dTriangle
void window2_Init3dAxis()
  {
     axisTurnX = 180.0f; // 90.0f;
     axisTurnY = 180.0f; // 90.0f;  // 180.0f;
     axisTurnZ = 180.0f; // 90.0f;
  }
/********************************************************************************************/
void window2_Reshape(int w, int h)
  {
     glViewport(0,0,w,h);

     glMatrixMode(GL_PROJECTION);
     glLoadIdentity();
     
    // old 30  jetzt bessere sichtbarkeit von axis
    gluPerspective(60.0f,(float)w/h,0.1,10);

     // here stuff from spain (pdf-source)
     glMatrixMode(GL_MODELVIEW);
     glLoadIdentity();
    // glTranslatef(1.0f,0.5f, -7.0f); 
  }

/********************************************************************************************/
void window2_Display()
  {
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
     
     glMatrixMode(GL_MODELVIEW);
     glLoadIdentity();

     // orig.
     gluLookAt(0,2,4, 0,0.5f,0, 0,1,0);

     glPushMatrix();
        // Move object back and do in place rotation
        glTranslatef(0.0f, 0.0f, 0.0f); //0.75f);
        // X,y dreht horizontal green/red nach vorne
        glRotatef(axisTurnX, 0.0f, 1.0f, 0.0f);
        // Y,x dreht vertikal green aber nach hinten
        glRotatef(axisTurnY, 1.0f, 0.0f, 0.0f);
        // hat eigentlich keinen einfluss
        glRotatef(axisTurnZ, 0.0f, 0.0f, 1.0f);
        printf("Window2_Display Axis: X: %f, Y: %f, Z: %f\n",axisTurnX,axisTurnY,axisTurnZ);
       
        gltDrawUnitAxes();

     glPopMatrix();

     // unbedingt notwendig   
     glutSwapBuffers();
     glFlush();

     // IST UNBEDINGRT NOTWENDIG (4)
     glutPostRedisplay(); 

  }
/********************************************************************************************/
int main(int argc, char *argv[])
  {
     // 3dAxis Init
     window2_Init3dAxis();

     OldX = 320;
     OldY = 240;

     // noch nicht gut geloest
     // initText();

     boolNightMode = false;
     boolInit      = false;
     setting_LuKan2dPoints = false;
     
     long current_frame = 0;
     
     capture = cvCaptureFromCAM( 0 );
   
     if( !capture )
        {
           printf("LK_Error: Can't open Webcam\n");
           return -1;
        }
   
     cvQueryFrame( capture );
     
     glutInit(&argc, argv);
     
     glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
     glutInitWindowSize(640,480);
 
     window1 = glutCreateWindow("HeadTrack Version 0.1.");

     glutPositionWindow(0,0);

     // new version ex: glutReshapeFunc(glutReshape);
     glutReshapeFunc(window1_glutReshape);

     // new version ex: glutKeyboardFunc(glutKeyboard);
     glutKeyboardFunc(window1_KeyboardFunc);

     // new Mouse stuff for opencv
     // ADD Points ex: glutMouseFunc(glutMouse);
     glutMouseFunc(window1_glutMouse);

     // ex: glutDisplayFunc(draw);
     glutDisplayFunc(window1_Draw);

     // ex glutIdleFunc(draw);
     glutIdleFunc(window1_Draw);

     // window 2
     window2 = glutCreateWindow("3D-glWindow 2");
     glutPositionWindow(0,480);
     
     glutReshapeFunc(window2_Reshape);
     glutDisplayFunc(window2_Display);

     // should be here else flickring
     glutIdleFunc(glutPostRedisplay);
     
     glutMainLoop();
     
     return 0;
  }
/************************************************************************************************/