#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <GL/glut.h>

char key[256] = { 0 }; /* Хранит флаги о нажатых кнопках */

/* Наша матрица позиции камеры */
/* В начале инициализируем в единичную */
/* Т.е. перемещаем камеру в координаты 0 по X и 0 по Y */
/* Направленную вдоль оси -Z */
GLdouble matrix_camera[4][4] = {
  { 1.0, 0.0, 0.0, 0.0 },
  { 0.0, 1.0, 0.0, 0.0 },
  { 0.0, 0.0, 1.0, 0.0 },
  { 0.0, 0.0, 0.0, 1.0 },
};

void MatrixPrint(GLdouble (*m)[4])
{
  printf(
    "%12.5lf %12.5lf %12.5lf %12.5lf\n"
    "%12.5lf %12.5lf %12.5lf %12.5lf\n"
    "%12.5lf %12.5lf %12.5lf %12.5lf\n"
    "%12.5lf %12.5lf %12.5lf %12.5lf\n"
    , m[0][0], m[1][0], m[2][0], m[3][0]
    , m[0][1], m[1][1], m[2][1], m[3][1]
    , m[0][2], m[1][2], m[2][2], m[3][2]
    , m[0][3], m[1][3], m[2][3], m[3][3]
  );
}

GLdouble h = 0.0;


/* Обработчик события изменении размеров окна */
/* Для инициализации матрицы проэкции */
void Reshape(int width, int height)
{
  glViewport(0, 0, width, height);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(45.0, ((GLdouble)width) / ((GLdouble)height), 0.1, 50.0);
}

/* Обработчик управлении камеры */
/* Проверяет нажатие клавиши и выполнении действия над ней */
void Control()
{
  char is_render = 0; /* Флаг перерисовке сцены */
  if (key[27] || key['q'] || key['Q']) exit(0); /* Если нажата клавиша Esc или клавиша Q, то выход */
  if (0
    || key['a'] || key['s'] || key['w'] || key['d']
    || key['o'] || key['p']
    || key['k'] || key['m']
  ) /* Клавиши управлением камеры */
  {
    glMatrixMode(GL_MODELVIEW); /* Выбираем модельную матрицу */
    glPushMatrix(); /* Сохраняем её содержимое */
    glLoadIdentity(); /* Подготавливаем её для формирования матрицы изменения положения */
//    glLoadMatrixd((GLdouble*)matrix_camera); /* Инициализируем текущим положением камеры */
    if (key['s']) { glRotated( 0.1, 1.0, 0.0, 0.0); is_render = 1; } /* Вращаем камеру вниз */
    if (key['w']) { glRotated(-0.1, 1.0, 0.0, 0.0); is_render = 1; } /* Вращаем камеру вверх */
    if (key['d']) { glRotated( 0.1, 0.0, 1.0, 0.0); is_render = 1; } /* Поворот камеры вправо */
    if (key['a']) { glRotated(-0.1, 0.0, 1.0, 0.0); is_render = 1; } /* Поворот камеры влево */
    if (key['p']) { glRotated( 0.1, 0.0, 0.0, 1.0); is_render = 1; } /* Наклон камеры вправо */
    if (key['o']) { glRotated(-0.1, 0.0, 0.0, 1.0); is_render = 1; } /* Наклон камеры влево */
    /* После таких не хитроумных изменений мы сформировали матрицу шага для камеры по нужным направлениям */

    GLdouble a[3] = { matrix_camera[3][0], matrix_camera[3][1], matrix_camera[3][2] };
    matrix_camera[3][0] = matrix_camera[3][1] = matrix_camera[3][2] = 0.0;

    glMultMatrixd((GLdouble*)matrix_camera); /* Происходит движение камеры */

    glGetDoublev(GL_MODELVIEW_MATRIX, (GLdouble*)matrix_camera); /* Сохраняем новое положение камеры */

    matrix_camera[3][0] = a[0];
    matrix_camera[3][1] = a[1];
    matrix_camera[3][2] = a[2];

    glLoadIdentity(); /* Подготавливаем её для формирования матрицы изменения положения */
    if (key['k']) { glTranslated( 0.0, 0.0,  0.01); is_render = 1; } /* Движение камеры вперед */
    if (key['m']) { glTranslated( 0.0, 0.0, -0.01); is_render = 1; } /* Движение камеры назад */
    glMultMatrixd((GLdouble*)matrix_camera); /* Происходит движение камеры */
    glGetDoublev(GL_MODELVIEW_MATRIX, (GLdouble*)matrix_camera); /* Сохраняем новое положение камеры */

    glPopMatrix(); /* Восстанавливаем предыдущее значение модельной матрицы */
  }
  if (0
    || key['-']
    || key['=']
  )
  {
    if (key['-']) h -= 1.0;
    if (key['=']) h += 1.0;
    is_render = 1;
  }
  if (is_render) glutPostRedisplay(); /* Если было движение! то перерисовываем дисплей */
}

/* Обработчик от рисовки примитивов */
void Display()
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); /* Очищаем всё что намусорили */
  glMatrixMode(GL_MODELVIEW); /* Выбираем модельную матрицу */
  glPushMatrix();
  glLoadIdentity();
  glTranslated(0.0, 0.0, h);
  glMultMatrixd((GLdouble*)matrix_camera); /* Инициализируем текущим положением камеры */

  GLdouble m[4][4];
  glGetDoublev(GL_MODELVIEW_MATRIX, (GLdouble*)m); /* Сохраняем новое положение камеры */
  MatrixPrint(m);printf("\n");
  printf("%12.5lf %12.5lf\n", h, sqrt(m[3][0] * m[3][0] + m[3][1] * m[3][1] + m[3][2] * m[3][2]));

  /* для примера работоспособности нарисуем куб с разноцветными гранями */
  glBegin(GL_QUADS);
    /* -x */ glColor3d(1.0, 0.0, 0.0);glVertex3d(-1.0, -1.0, -1.0);glVertex3d(-1.0,  1.0, -1.0);glVertex3d(-1.0,  1.0,  1.0);glVertex3d(-1.0, -1.0,  1.0);
    /* +x */ glColor3d(1.0, 0.5, 0.5);glVertex3d( 1.0, -1.0, -1.0);glVertex3d( 1.0,  1.0, -1.0);glVertex3d( 1.0,  1.0,  1.0);glVertex3d( 1.0, -1.0,  1.0);
    /* -y */ glColor3d(0.0, 1.0, 0.0);glVertex3d(-1.0, -1.0, -1.0);glVertex3d( 1.0, -1.0, -1.0);glVertex3d( 1.0, -1.0,  1.0);glVertex3d(-1.0, -1.0,  1.0);
    /* +y */ glColor3d(0.5, 1.0, 0.5);glVertex3d(-1.0,  1.0, -1.0);glVertex3d( 1.0,  1.0, -1.0);glVertex3d( 1.0,  1.0,  1.0);glVertex3d(-1.0,  1.0,  1.0);
    /* -z */ glColor3d(0.0, 0.0, 1.0);glVertex3d(-1.0, -1.0, -1.0);glVertex3d( 1.0, -1.0, -1.0);glVertex3d( 1.0,  1.0, -1.0);glVertex3d(-1.0,  1.0, -1.0);
    /* +z */ glColor3d(0.5, 0.5, 1.0);glVertex3d(-1.0, -1.0,  1.0);glVertex3d( 1.0, -1.0,  1.0);glVertex3d( 1.0,  1.0,  1.0);glVertex3d(-1.0,  1.0,  1.0);
  glEnd();

  glMatrixMode(GL_MODELVIEW); /* Выбираем модельную матрицу */
  glLoadIdentity();
  glTranslated(0.0, 0.0, -0.5);

  glBegin(GL_LINES);
    glColor3d(1.0, 0.0, 0.0);glVertex3d(0.0, 0.0, 0.0);glVertex3d(matrix_camera[0][0], matrix_camera[0][1], matrix_camera[0][2]);
    glColor3d(0.0, 1.0, 0.0);glVertex3d(0.0, 0.0, 0.0);glVertex3d(matrix_camera[1][0], matrix_camera[1][1], matrix_camera[1][2]);
    glColor3d(0.0, 0.0, 1.0);glVertex3d(0.0, 0.0, 0.0);glVertex3d(matrix_camera[2][0], matrix_camera[2][1], matrix_camera[2][2]);
  glEnd();

  glMatrixMode(GL_MODELVIEW); /* Выбираем модельную матрицу */
  glPopMatrix();
  glFinish(); /* Скажем OpenGL "выполняй!" */
  glutSwapBuffers(); /* Поменяем местами дисплеи */
}

void KeyDown(unsigned char code, int x, int y) { key[code] = 1; } /* Обработчик регистрирует нажатие клавиши */
void KeyUp  (unsigned char code, int x, int y) { key[code] = 0; } /* Обработчик регистрирует отпускание клавиши */

int main(int argc, char** argv)
{
/* Скелет OpenGL */
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGBA | GLUT_STENCIL);
  glutInitWindowPosition(-1, -1);
  glutInitWindowSize(640, 480);
  glutCreateWindow("Camera");
  glutSetCursor(GLUT_CURSOR_NONE);
  glClearColor(0.1, 0.1, 0.1, 0.0);

  glEnable(GL_DEPTH_TEST);

  glutKeyboardFunc(KeyDown);
  glutKeyboardUpFunc(KeyUp);

  glutIdleFunc(Control);
  glutDisplayFunc(Display);
  glutReshapeFunc(Reshape);
  glutMainLoop();
  return 0;
}