acoustic_vr.cpp

Go to the documentation of this file.

/*===============================================================================================
 3D Demo Example
 Copyright (c), Firelight Technologies Pty, Ltd 2005-2011.

 Example to show occlusion
===============================================================================================*/
#include <iostream>
#include <fmod/fmod.hpp>
#include <fmod/fmod_errors.h>
#include <fmod/fmod_event.hpp>
#include <fmod/fmod_event_net.hpp>
//#include <windows.h>
#include <boost/algorithm/string.hpp>

#include <GL/glut.h>
#include <SOIL/SOIL.h>

#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stack>
#include <string>
#include <fstream>
#include <sstream>
#include <time.h>

#include <ros/ros.h>
#include <boost/thread.hpp>
#include <sensor_msgs/Joy.h>
#include <auv_msgs/NavSts.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/PointStamped.h>

#include <labust/gui/AAGui.hpp>

#include <auv_msgs/BodyForceReq.h>

namespace LABUST
{
struct JoystickData
{
        std::vector<short> axes;
        std::vector<bool> buttons;
};
};

//#include <crtdbg.h>
boost::mutex joyMux;
LABUST::JoystickData joystickData;

auv_msgs::NavSts state, target;
labust::gui::AAGui::Ptr gui;

ros::Publisher infopub;
std::vector<float> configInfo(8,0.0);

namespace FMOD
{

//#define SHOW_GUI_DEBUG_TEXT
#define GL_CLAMP_TO_EDGE 0x812F
#define FMOD_INIT_DSOUND_HRTFFULL 0x00000800

double Head;
double HeadIni=0.0f;

unsigned char key1 =0;

void ERRCHECK(FMOD_RESULT result)
{
        if (result != FMOD_OK)
        {
                printf("FMOD error! (%d) %s\n", result, FMOD_ErrorString(result));
                exit(-1);
        }
}

int INTERFACE_UPDATETIME = 15; // milliseconds

int intMax = 32767;
int r;
int iDiter = 0;
int KDiterTime = 1;
double AmpDiter = 0.0f;
double FreqDiter = 1.0f;
double tDiter[13] ={10,15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70};
double FiDiter[13] = {0, 15, -10, -25, 0, 5, -15, 15, -10, -3, -10, 5, 0};
double ti=0;
double Integral=0.0f;
bool fullscreen = false;
bool wait = true;
float LogData[12]={0,0,0,0,0,0,0,0,0,0,0,0};

double forw=0.0f;
double InputStoh=0.0f;
int InputStoh1=0;
double lastObjectLong = 0.0;
double zPos = 0.0;
double aSQR = 1.0f;
double bSQR = 0.0f;
double cSQR = 0.0f;
// varijable koje se trebaju primiti iz MOOSa
double Altitude = 2.5f;
double ROVLat = 43.5f;
double ROVLong = 15.5f;
double ROVHeading = 0.0f;
double ROVRoll = 0.0f;
double ROVPitch = 0.0f;
double Depth = 0.0f;
double VelVec = 0.0f;
double HE = 0.0f;

// varijable koje se upisu "Rucno"
double AltitudeSetPoint = 10.0f;
bool TrajectoryTracking = false;
bool WPupdateFlage=true;
double ObjectLat = 43.5;
double ObjectLong = 15.5;
double ObjectDepth = 20.0f;
double Doppler = 100.0f;
double SilenceAngle = 0.00f;
double VerbalAngle = 5.00f;
double NonLinearCoeff = 1.0f;
double NonLinearCoeffDist = 1.0f;
double TransformAngle = 0.00f;
double PathVelocity = 0.0f;
double Ki = 0.0f;
int Sound=0;
int GuidanceMode = 1;
int TaskMode = 1;
int JoyStickMode=1;
int SpeedProfile='C';
double velAtitude = 0.0f;
double lastvelAtitude = 0.0f;
double PathStartX = 10.0f;
double PathStartY = 10.0f;
double PathStartX1 = 0.0f;
double PathStartY1 = 0.0f;
double PathX = 0.0f;
double PathY = 0.0f;
double DistanceToPath = 0.0f;
double RabbitDistance = 0.0f;
double RabbitNorth = 0;
double RabbitNorth1 = 0;
double RabbitNorth2 = 0;
double RabbitNorthOld = 0;
double RabbitEast = 0;
double RabbitEastOld = 0;
double HeadingErr = 0.0f;
double TauX = 0.0f;
double TauY = 0.0f;
double TauZ = 0.0f;
double TauK = 0.0f;
double TauM = 0.0f;
double TauN = 0.0f;
double frwVelVector = 0.0f;
double latVelVector = 0.0f;
double zVelVector = 0.0f;

// definiranje Way Pointa
int NumberOfWP = 15;
const int MaxNumberOfWP = 15;
//int mode = 1;// za lawnmover mode, mode = 0
int WPindex = 1;
int ActualWPindex = 1;
int ActualWPindexOld = 1;
float WPnorth[MaxNumberOfWP];// ={43.823023f, 43.822942f, 43.823321f, 43.823023f};
float WPeast[MaxNumberOfWP];// = {15.570909f, 15.570321f, 15.570435f,15.570909f};
float WPz[MaxNumberOfWP];


// window size
int width = 500;
int height = 500;

// mouse control
bool doRotate = false;
int xMouse = 0;
int yMouse = 0;

// listener orientation
float xRotation = 0.0f; // Roll
float yRotation = 0.0f; //Heading
float zRotation = 0.0f; //Pitch
float lastyRotation = 0.0f;

// listener position
float xListenerPos = 0.0f;
//TONI ustvari su y i z zamjenjeni
float yListenerPos = 50.0f; //ovo je Z
float zListenerPos = 20.0f; //ovo je Y
float DirectionToTarget = 0.0f;
float zListenerPosRel = 0.0f;
float xListenerPosRel = 0.0f;

// keyboard control
bool moveForward    = false;
bool moveBackward   = false;
bool moveRotateClock    = false;
bool moveRotateAntiClock   = false;
bool moveLeft       = false;
bool moveRight      = false;
bool moveUp         = false;
bool moveDown       = false;
bool moveFast       = false;
bool ambientVolUp   = false;
bool ambientVolDown = false;
bool masterVolUp    = false;
bool masterVolDown  = false;

#undef PI
const float PI = 3.14159265f;

float accumulatedTime = 0.0f;

GLuint texture;
GLuint skyboxTexture[6];

// sounds placement
struct Object
{
        float xPos;
        float yPos;
        float zPos;
        float intensity;
        int sound;
        FMOD::Event *event;
};

const int NUM_OBJECTS = 8;
Object objects[NUM_OBJECTS] =
{
                //{  -11.0f,    10.0f,    0.0f,    1.0f,    0,    0 },
                {   0.0f,     10.0f,    0.0f,    1.0f,    0,    0 }, //Toni: IZVOR ZVUKA JE ISHODISTE KOORDINATNOG SUSTAVA
                {   0.0f,    10.0f,    0.0f,    1.0f,    1,    0 },
                {   45.0f,    10.0f,    0.0f,    1.0f,    3,    0 },
                {  -30.0f,    1.0f,   21.0f,    1.0f,    2,    0 },
                {  -30.0f,    1.0f,  -21.0f,    1.0f,    3,    0 },
                {   12.0f,    1.0f,  -27.0f,    1.0f,    0,    0 },
                {    0.0f,    10.0f,   0.0f,    1.0f,    0,    0 },
                {    0.0f,    10.0f,   0.0f,    1.0f,    1,    0 },
};

struct Polygon
{
        int numVertices;
        int indicesOffset;
        float directOcclusion;
        float reverbOcclusion;
        FMOD_VECTOR normal;
};

struct Mesh
{
        int numVertices;
        FMOD_VECTOR *vertices;
        float (*texcoords)[2];
        int numPolygons;
        Polygon* polygons;
        int numIndices;
        int *indices;
        FMOD::Geometry *geometry;
};
//
class GlutCloseClass
{
public:
        GlutCloseClass() {};
        ~GlutCloseClass();
};


GlutCloseClass gCloseObject;

Mesh walls;
Mesh rotatingMesh;

// fmod sounds structures
FMOD::EventSystem     *fmodEventSystem    = 0;
FMOD::EventProject    *fmodEventProject   = 0;
FMOD::EventGroup      *fmodEventGroup     = 0;
FMOD::EventParameter  *fmodEventParameter = 0;
FMOD::System          *fmodSystem         = 0;
FMOD::Geometry        *geometry           = 0;
FMOD::DSP             *global_lowpass     = 0;

float   ambientVolume   = 0.2f;
float   masterVolume;

//#include <AAGui.cpp>

/*
    Global stack of strings to render
 */
#ifdef SHOW_GUI_DEBUG_TEXT
std::stack<std::string> debugText;
std::stack<std::string> statusText;
#endif

GLenum  rendermode = GL_FILL;

bool showdebug = false;
bool showhelp = false;

void outputText(int x, int y, std::string text)
{
        int i;
        const char *txt = text.c_str();

        glRasterPos2f(x, y);
        for (i = 0; i < (int)text.length(); i++)
        {
                glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, txt[i]);
        }
}

#ifdef SHOW_GUI_DEBUG_TEXT
void renderText(int x, int y, std::stack<std::string> *text)
{
        glColor3f(1.0f, 1.0f, 1.0f);
        for (int count = 0; count < text->size(); count++)
        {
                outputText(x, y, text->top());
                text->pop();

                y -= 17;
        }
}
#endif

void renderUiText()
{
        /*
        Render help text
         */
        if (showhelp)
        {
                int x = 10;
                int y = height - 20;

                glColor3f(1.0f, 1.0f, 1.0f);
                outputText(x, y,     "F1   - Toggle help");
                outputText(x, y-=18, "F2   - Toggle fullscreen");
                outputText(x, y-=18, "F3   - Toggle wireframe rendering");
                outputText(x, y-=18, "F11  - Toggle debug info");
                outputText(x, y-=18, "--");
                outputText(x, y-=18, "w     - Move forward");
                outputText(x, y-=18, "s     - Move backward");
                outputText(x, y-=18, "a     - Move left");
                outputText(x, y-=18, "d     - Move right");
                outputText(x, y-=18, "space - Move up");
                outputText(x, y-=18, "c     - Move down");
                outputText(x, y-=18, "Mouse (hold left button) - look direction");
                outputText(x, y-=18, "--");
                //      outputText(x, y-=18, "V/v   - Master volume up/down");
                outputText(x, y-=18, "Z/z   - Ambient sound volume up/down");
        }
        else
        {
                glColor3f(1.0f, 1.0f, 1.0f);
                outputText(10, height - 20, "F1 - Help");
        }

        /*
        Render debug text
         */
#ifdef SHOW_GUI_DEBUG_TEXT
        if (showdebug)
        {
                renderText(width - (width/2), height - 20, &debugText);
        }
        else
        {
                /*
            Otherwise just pop everything off the stack
                 */
                for (int count = 0; count < debugText.size(); count++)
                {
                        debugText.pop();
                }
        }

        /*
        Render status text
         */
        renderText(10, 20, &statusText);
#endif
}

void initGeometry(const char* szFileName, Mesh& mesh, bool alter = false)
{
        FMOD_RESULT result;

        FILE* file = fopen(szFileName, "rb");
        if (file)
        {
                // read vertices
                fread(&mesh.numVertices, sizeof (mesh.numVertices), 1, file);
                mesh.vertices = new FMOD_VECTOR[mesh.numVertices];
                mesh.texcoords = new float[mesh.numVertices][2];
                fread(mesh.vertices, sizeof (float) * 3, mesh.numVertices, file);
                fread(mesh.texcoords, sizeof (float) * 2, mesh.numVertices, file);


                fread(&mesh.numIndices, sizeof (mesh.numIndices), 1, file);
                mesh.indices = new int[mesh.numIndices];
                fread(mesh.indices, sizeof (int), mesh.numIndices, file);


                fread(&mesh.numPolygons, sizeof (mesh.numPolygons), 1, file);
                mesh.polygons = new Polygon[mesh.numPolygons];

                // read polygons
                for (int poly = 0; poly < mesh.numPolygons; poly++)
                {
                        Polygon* polygon = &mesh.polygons[poly];


                        fread(&polygon->numVertices, sizeof (polygon->numVertices), 1, file);
                        fread(&polygon->indicesOffset, sizeof (polygon->indicesOffset), 1, file);
                        fread(&polygon->directOcclusion, sizeof (polygon->directOcclusion), 1, file);
                        fread(&polygon->reverbOcclusion, sizeof (polygon->reverbOcclusion), 1, file);

                        int* indices = &mesh.indices[polygon->indicesOffset];

                        // calculate polygon normal
                        float xN = 0.0f;
                        float yN = 0.0f;
                        float zN = 0.0f;
                        // todo: return an error if a polygon has less then 3 vertices.
                        for (int vertex = 0; vertex < polygon->numVertices - 2; vertex++)
                        {
                                float xA = mesh.vertices[indices[vertex + 1]].x -mesh.vertices[indices[0]].x;
                                float yA = mesh.vertices[indices[vertex + 1]].y -mesh.vertices[indices[0]].y;
                                float zA = mesh.vertices[indices[vertex + 1]].z -mesh.vertices[indices[0]].z;
                                float xB = mesh.vertices[indices[vertex + 2]].x -mesh.vertices[indices[0]].x;
                                float yB = mesh.vertices[indices[vertex + 2]].y -mesh.vertices[indices[0]].y;
                                float zB = mesh.vertices[indices[vertex + 2]].z -mesh.vertices[indices[0]].z;
                                // cross product
                                xN += yA * zB - zA * yB;
                                yN += zA * xB - xA * zB;
                                zN += xA * yB - yA * xB;
                        }
                        float fMagnidued = (float)sqrt(xN * xN + yN * yN + zN * zN);
                        if (fMagnidued > 0.0f) // a tollerance here might be called for
                        {
                                xN /= fMagnidued;
                                yN /= fMagnidued;
                                zN /= fMagnidued;
                        }
                        polygon->normal.x = xN;
                        polygon->normal.y = yN;
                        polygon->normal.z = zN;
                }
                fclose(file);
        }

        result = fmodSystem->createGeometry(mesh.numPolygons, mesh.numIndices, &mesh.geometry);
        ERRCHECK(result);

        /*
        Tell FMOD about the geometry
         */
        for (int poly = 0; poly < mesh.numPolygons; poly++)
        {
                Polygon* polygon = &mesh.polygons[poly];
                FMOD_VECTOR vertices[16];
                for (int i = 0; i < polygon->numVertices; i++)
                        vertices[i] = mesh.vertices[mesh.indices[polygon->indicesOffset + i]];
                int polygonIndex = 0;

                if (alter && polygon->directOcclusion == 0.85f)
                {
                        //       polygon->directOcclusion = 0.95f;
                }

                result = mesh.geometry->addPolygon(
                                polygon->directOcclusion,
                                polygon->reverbOcclusion,
                                false, // single sided
                                polygon->numVertices,
                                vertices,
                                &polygonIndex);
                ERRCHECK(result);
        }
}


void freeGeometry(Mesh& mesh)
{
        mesh.geometry->release();

        delete [] mesh.vertices;
        delete [] mesh.texcoords;
        delete [] mesh.polygons;
        delete [] mesh.indices;
}


void inWater()
{
        FMOD_RESULT result;
        FMOD_REVERB_PROPERTIES reverbprops;
        static bool inwater = false;

        if (xListenerPos > -14.75f && xListenerPos < -7.6f
                        && zListenerPos > -10.85f && zListenerPos < -3.75f
                        && yListenerPos < 5.0f)
        {
                /*
            Use opengl fog to make it look like we are in water
                 */
                if (!inwater)
                {
                        glEnable(GL_FOG);

                        result = fmodEventSystem->getReverbPreset("UnderWater", &reverbprops);
                        ERRCHECK(result);
                        result = fmodEventSystem->setReverbProperties(&reverbprops);
                        ERRCHECK(result);
                        result = global_lowpass->setBypass(false);
                        ERRCHECK(result);

                        inwater = true;
                }
        }
        else
        {
                /*
            Disable fog (water)
                 */
                if (inwater)
                {
                        glDisable(GL_FOG);

                        result = fmodEventSystem->getReverbPreset("StdReverb", &reverbprops);
                        ERRCHECK(result);
                        result = fmodEventSystem->setReverbProperties(&reverbprops);
                        ERRCHECK(result);
                        if (global_lowpass)
                        {
                                result = global_lowpass->setBypass(true);
                                ERRCHECK(result);
                        }

                        inwater = false;
                }
        }
}


//      void drawSkyBox()
//      {
//              glPushMatrix();
//              glTranslatef(xListenerPos, 0.0f, yListenerPos);
//              glDisable(GL_LIGHTING);
//              /*
//            Walls
//               */
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[0]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(-150.0f, -150.0f, -150.0f);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(-150.0f, 150.0f, -150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(150.0f, 150.0f, -150.0f);
//              glTexCoord2f(0.0f, 1.0f);  glVertex3f(150.0f, -150.0f, -150.0f);
//              glEnd();
//
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[1]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(150.0f, -150.0f, -150.0f);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(150.0f, 150.0f,-150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(150.0f, 150.0f, 150.0f);
//              glTexCoord2f(0.0f, 1.0f); glVertex3f(150.0f, -150.0f, 150.0f);
//              glEnd();
//
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[2]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(0.0f, 1.0f); glVertex3f(-150.0f, -150.0f, 150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(-150.0f, 150.0f, 150.0f);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(150.0f, 150.0f, 150.0f);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(150.0f, -150.0f, 150.0f);
//              glEnd();
//
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[3]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(0.0f, 1.0f); glVertex3f(-150.0f, -150.0f, -150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(-150.0f, 150.0f, -150.0f);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(-150.0f, 150.0f, 150.0f);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(-150.0f, -150.0f, 150.0f);
//              glEnd();
//
//              /*
//            Top
//               */
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[4]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(-150.0f, 150.0f, -150.0f);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(150.0f, 150.0f, -150.0f);
//              glTexCoord2f(0.0f, 1.0f); glVertex3f(150.0f, 150.0f, 150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(-150.0f, 150.0f, 150.0f);
//              glEnd();
//
//              /*
//            Bottom
//               */
//              glBindTexture(GL_TEXTURE_2D, skyboxTexture[5]);
//              glBegin(GL_QUADS);
//              glTexCoord2f(0.0f, 1.0f); glVertex3f(-150.0f, -150.0f, -150.0f);
//              glTexCoord2f(0.0f, 0.0f); glVertex3f(150.0f, -150.0f, -150.0f);
//              glTexCoord2f(1.0f, 0.0f); glVertex3f(150.0f, -150.0f, 150.0f);
//              glTexCoord2f(1.0f, 1.0f); glVertex3f(-150.0f, -150.0f, 150.0f);
//              glEnd();
//
//              glEnable(GL_LIGHTING);
//              glPopMatrix();
//      }

void drawWaterRoom()
{
        glBlendFunc( GL_SRC_ALPHA, GL_ONE );
        glEnable(GL_BLEND);

        glPushMatrix();
        glColor4f(0.0f,0.1f,0.8f,1.0f);
        glDisable(GL_LIGHTING);

        glBegin(GL_QUADS);
        glVertex3f(-14.72f, 4.0f, -10.85f);
        glVertex3f(-7.68f, 4.0f, -10.85f);
        glVertex3f(-7.68f, 4.0f, -3.85f);
        glVertex3f(-14.72f, 4.0f, -3.85f);
        glEnd();

        glEnable(GL_LIGHTING);
        glPopMatrix();

        glDisable(GL_BLEND);
}

void drawGeometry(Mesh& mesh)
{
        FMOD_RESULT result;

        FMOD_VECTOR pos;
        result = mesh.geometry->getPosition(&pos);
        ERRCHECK(result);

        glPushMatrix();
        // create matrix and set gl transformation for geometry
        glTranslatef(pos.x, pos.y, pos.z);
        FMOD_VECTOR forward;
        FMOD_VECTOR up;
        result = mesh.geometry->getRotation(&forward, &up);
        ERRCHECK(result);
        float matrix[16] =
        {
                        up.y * forward.z - up.z * forward.y,            up.x,           forward.x,              0.0f,
                        up.z * forward.x - up.x * forward.z,            up.y,           forward.y,              0.0f,
                        up.x * forward.y - up.y * forward.x,            up.z,           forward.z,              0.0f,
                        0.0f,                                                                           0.0f,           0.0f,                   1.0f,
        };
        glMultMatrixf(matrix);

        // draw all polygons in object
        glEnable(GL_LIGHTING);
        glPolygonMode(GL_FRONT_AND_BACK, rendermode);
        for (int poly = 0; poly < mesh.numPolygons; poly++)
        {
                Polygon* polygon = &mesh.polygons[poly];
                if (polygon->directOcclusion == 0.0f)
                {
                        continue; // don't draw because it is an open door way
                }
                glBegin(GL_TRIANGLE_FAN);
                glNormal3fv(&polygon->normal.x);

                for (int i = 0; i < polygon->numVertices; i++)
                {
                        int index = mesh.indices[polygon->indicesOffset + i];
                        glTexCoord2f(mesh.texcoords[index][0], mesh.texcoords[index][1]);
                        glVertex3fv(&mesh.vertices[index].x);
                }
                glEnd();
        }
        glPopMatrix();
}

void initObjects()
{
        FMOD_RESULT result;
        // TONI pozivanje zvukova
        if (GuidanceMode < 3)
        {if (Sound==0)
        {result = fmodEventGroup->getEvent("PinkNew 500m", FMOD_EVENT_DEFAULT, &objects[0].event);
        ERRCHECK(result);

        result = fmodEventGroup->getEvent("Click1 500m", FMOD_EVENT_DEFAULT, &objects[1].event);
        ERRCHECK(result);}
        else
        {
                {
                        result = fmodEventGroup->getEvent("PinkNew 500m", FMOD_EVENT_DEFAULT, &objects[1].event);
        ERRCHECK(result);

        result = fmodEventGroup->getEvent("Click1 500m", FMOD_EVENT_DEFAULT, &objects[0].event);
        ERRCHECK(result);
                }
        }
        }

        if (GuidanceMode > 1 && GuidanceMode < 4)
        {
                result = fmodEventGroup->getEvent("up", FMOD_EVENT_DEFAULT, &objects[2].event);
                ERRCHECK(result);

                result = fmodEventGroup->getEvent("down", FMOD_EVENT_DEFAULT, &objects[3].event);
                ERRCHECK(result);
        }

        if (GuidanceMode > 2)
        {result = fmodEventGroup->getEvent("left", FMOD_EVENT_DEFAULT, &objects[4].event);
        ERRCHECK(result);

        result = fmodEventGroup->getEvent("right", FMOD_EVENT_DEFAULT, &objects[5].event);
        ERRCHECK(result);
        }

        if (TaskMode == 3)
        {//result = fmodEventGroup->getEvent("3DSoundEmit", FMOD_EVENT_DEFAULT, &objects[6].event);
                if (Sound==0)
                {result = fmodEventGroup->getEvent("PinkNew", FMOD_EVENT_DEFAULT, &objects[6].event);
                ERRCHECK(result);
                result = fmodEventGroup->getEvent("Click1", FMOD_EVENT_DEFAULT, &objects[7].event);
                ERRCHECK(result);}
                else
                {result = fmodEventGroup->getEvent("PinkNew", FMOD_EVENT_DEFAULT, &objects[7].event);
                ERRCHECK(result);
                result = fmodEventGroup->getEvent("Click1", FMOD_EVENT_DEFAULT, &objects[6].event);
                ERRCHECK(result);}
        }


        for (int i=0; i < NUM_OBJECTS-1; i++)
        {
                FMOD_VECTOR pos = { objects[i].xPos, objects[i].yPos, objects[i].zPos };
                FMOD_VECTOR vel = { 0.0f, 0.0f, 0.0f };

                if (objects[i].event)
                {
                        //TONI izgleda kao prerada zvuka u 3D na osnovu pozicije i velocity (brzina kretanja) izvora zvuka?
                        result = objects[i].event->set3DAttributes(&pos, &vel);
                        ERRCHECK(result);

                        //TONI izgleda kao pokretanje zvuka
                        result = objects[i].event->start();
                        ERRCHECK(result);
                }
        }
}


void updateObjectSoundPos(Object* object)
{
        FMOD_RESULT result;

        if (object->event)
        {

                //TONI nova pozicija izvora zvuka
                FMOD_VECTOR pos = { object->xPos, object->yPos, object->zPos };
                FMOD_VECTOR oldPos;
                object->event->get3DAttributes(&oldPos, 0);

                //TONI racunanje brzine kretanja izvora zvuka
                FMOD_VECTOR vel;
                vel.x = 0;//(pos.x - oldPos.x) *  (1000.0f / (float)INTERFACE_UPDATETIME);
                vel.y = 0;//(pos.y - oldPos.y) *  (1000.0f / (float)INTERFACE_UPDATETIME);
                vel.z = 0;//(pos.z - oldPos.z) *  (1000.0f / (float)INTERFACE_UPDATETIME);
                result = object->event->set3DAttributes(&pos, &vel);

                //      ERRCHECK(result);
        }
}


void mouseFunc(int button, int state, int x, int y)
{
        switch (button)
        {
        case GLUT_LEFT_BUTTON:
                if (state == GLUT_DOWN)
                {
                        doRotate = true;
                        xMouse = x;
                        yMouse = y;
                }
                else
                        if (state == GLUT_UP)
                        {
                                doRotate = false;
                        }
                break;

        default:
                break;
        }
}

void motionFunc(int x, int y)
{
        //TONI s misom se moze rotirati a ne kretati naprijed-nazad
        std::string SingleLog;
        int test = 0;

        int dx = x - xMouse;
        int dy = y - yMouse;

        // view rotation about y-axis
        yRotation += (float)dx * 0.5f;
        if (yRotation > 180.0f)
                yRotation -= 360.0f;
        else
                if (yRotation < -180.0f)
                        yRotation += 360.0f;

        // view rotation about x-axis
        const float xExtent = 88.0f;
        //TONI izmjena u redu ispod, da se ne mice gore-dolje vec samo lijevo-desno
        xRotation += 0.0f;//(float)dy * 0.5f;
        if (xRotation > xExtent)
                xRotation = xExtent;
        else
                if (xRotation < -xExtent)
                        xRotation = -xExtent;

        xMouse = x;
        yMouse = y;
}

void doGeometryMovement()
{
        FMOD_RESULT result;

        // example of moving individual polygon vertices
        int xGeometryWarpPos = -30.0f;
        int zGeometryWarpPos = -21.0f;
        int dx = xListenerPos - xGeometryWarpPos;
        int dz = zListenerPos - zGeometryWarpPos;
        if (dx * dx + dz * dz < 30.0f * 30.0f)
        {
                if (sin(accumulatedTime * 1.0f) > 0.0f)
                {
                        static FMOD_VECTOR lastOffset = { 0.0f, 0.0f, 0.0f };
                        FMOD_VECTOR offset = { sin(accumulatedTime * 2.0f), 0.0f, cos(accumulatedTime * 2.0f) };
                        for (int poly = 0; poly < walls.numPolygons; poly++)
                        {
                                Polygon* polygon = &walls.polygons[poly];
                                for (int i = 0; i < polygon->numVertices; i++)
                                {
                                        FMOD_VECTOR& vertex = walls.vertices[walls.indices[polygon->indicesOffset + i]];

                                        dx = vertex.x - xGeometryWarpPos;
                                        dz = vertex.z - zGeometryWarpPos;
                                        if (dx * dx + dz * dz > 90.0f)
                                                continue;
                                        vertex.x -= lastOffset.x;
                                        vertex.y -= lastOffset.y;
                                        vertex.z -= lastOffset.z;

                                        vertex.x += offset.x;
                                        vertex.y += offset.y;
                                        vertex.z += offset.z;
                                        result = walls.geometry->setPolygonVertex(poly, i, &vertex);
                                        ERRCHECK(result);
                                }
                        }
                        lastOffset = offset;
                }
        }

        // example of rotation and a geometry object
        FMOD_VECTOR up = { 0.0f, 1.0f, 0.0f };
        FMOD_VECTOR forward = { (float)sin(accumulatedTime * 0.5f), 0.0f, (float)cos(accumulatedTime * 0.5f) };
        result = rotatingMesh.geometry->setRotation(&forward, &up);
        ERRCHECK(result);
        FMOD_VECTOR pos;
        pos.x = 12.0f;
        pos.y = (float)sin(accumulatedTime) * 0.4f + 0.1f;
        pos.z = 0.0f;
        result = rotatingMesh.geometry->setPosition(&pos);
        ERRCHECK(result);
        //drawGeometry(rotatingMesh);
}

void doSoundMovement()
{
        //TONI micanje izvora zvuka
        //NonLinearCoeffDist is nonlineariti coeff for distance cue
        float azim = 0, DistanceErr = 0;
        float distance = sqrt((xListenerPos-RabbitEast)*(xListenerPos-RabbitEast)+(zListenerPos-RabbitNorth)*(zListenerPos-RabbitNorth)); //distance from target(rabbit) to the listener
        float TransformDistance=1.0f, NewDist=distance; //NewDist is supernormal distance, TransformDistance is realtionship betwean supernormal and real distance.

        //Nova distanca uzrokovana uvodjenjem nelinearnosti na isti nacin kao i za azimut
        DistanceErr = distance - 10;//RabbitDistance;
        if (ActualWPindex==1)
        {Integral=0;}
        else
        {Integral += DistanceErr*INTERFACE_UPDATETIME/1000;}
        NewDist = 10/PI*atan2((2*NonLinearCoeffDist*sin(2*(PI/2)*DistanceErr/10)),(1-NonLinearCoeffDist*NonLinearCoeffDist+(1+NonLinearCoeffDist*NonLinearCoeffDist)*cos(2*(PI/2)*DistanceErr/10)))+10;
        NewDist += Ki*Integral;
        //Nova supernormalna distanca sa mrtvom zonom +/-0.5 i linearno (promjenjeni koeficijent nagiba) u ostalom dijelu
        /*if (fabs(distance - RabbitDistance)>0.5)
        {
                if (fabs(distance - RabbitDistance)<5)
                {
                        if (distance<RabbitDistance)
                        {NewDist=5/NonLinearCoeffDist+(9.5-5/NonLinearCoeffDist)/4.5*(distance-5);}
                        else
                        {NewDist=10.5+(9.5-5/NonLinearCoeffDist)/4.5*(distance-10.5);}
                }
                else
                {
                        if (distance<RabbitDistance)
                        {NewDist=distance/(5/NonLinearCoeffDist);}
                        else
                        {NewDist=distance+10.5+(9.5-5/NonLinearCoeffDist)-15;}
                }
        }*/
        if (ActualWPindex==1)
        {TransformDistance = 1;}//RabbitDistance/distance;}
        else
        {TransformDistance = NewDist/RabbitDistance;}//distance;}

        //InputStoh1 = (0*sin(2*PI*accumulatedTime)+ sin(PI/8*accumulatedTime)+ 1.5*sin(4*PI/8*accumulatedTime)+ 1.5*sin(7*PI/8*accumulatedTime)+ sin(10*PI/8*accumulatedTime)+ 0*sin(13*PI/8*accumulatedTime))/3;
        //InputStoh = 12*(r-1);//InputStoh1;

        // Slip angle - Pomak zbog vodjenja po vektoru brzine a ne headingu, za male brzine nepouzdan
        std::cout<<latVelVector<<","<<frwVelVector<<std::endl;
        VelVec = atan2(latVelVector,frwVelVector);
        if      (fabs(frwVelVector) < 0.3)
        {VelVec = fabs(frwVelVector)/0.3*VelVec;}
        HeadingErr = DirectionToTarget - yRotation - VelVec*180/PI;

        if (HeadingErr<-180.0f)
        {HeadingErr = HeadingErr + 360;
        }

        //uvodjenje azimut pomaka rabbita zbog nelinearnosti heading skale i vodjenja po brzine a ne headingu
        HE = HeadingErr;
        azim = HE*PI/180;
        if (fabs(HeadingErr) < 90)// || HeadingErr > 270)
        {
                // uvodjenje nelinearnosti u heading skalu
                TransformAngle = 0.5*atan2((2*NonLinearCoeff*sin(2*azim)),(1-NonLinearCoeff*NonLinearCoeff+(1+NonLinearCoeff*NonLinearCoeff)*cos(2*azim)));//-azim;// ovo nije novi kut vec kut degeneracije headingerrora zbog uvodjenja nelinearnosti
        }
        else
        {TransformAngle = azim;//0.0f;
        }

        //std::cout<<180/PI*TransformAngle<<" "<<DirectionToTarget<<" "<<yRotation<<" "<<HeadingErr<<" "<<fabs(HE)<<" "<<zListenerPosRel<<" "<<xListenerPosRel<<std::endl;



        if (TaskMode == 1)
        {
                if ((accumulatedTime > KDiterTime * tDiter[iDiter]) && (iDiter < 12))
                {iDiter++;}
                objects[0].xPos = xListenerPos*(1-cos(TransformAngle))+zListenerPos*sin(TransformAngle);//-25*(1-cos((FiDiter[iDiter]+AmpDiter*sin(accumulatedTime*2*PI*FreqDiter))*PI/180));
                objects[0].zPos = zListenerPos*(1-cos(TransformAngle))-xListenerPos*sin(TransformAngle);//+25*sin((FiDiter[iDiter]+AmpDiter*sin(accumulatedTime*2*PI*FreqDiter))*PI/180);
                objects[1].xPos = 0;
                objects[1].zPos = 0;
        }
        else
        {objects[0].xPos = xListenerPos + RabbitDistance*sin(TransformAngle + yRotation*PI/180 + VelVec);//RabbitEast+(xListenerPos-RabbitEast)*(1-cos(TransformAngle))+(zListenerPos-RabbitNorth)*sin(TransformAngle);
        objects[0].zPos = zListenerPos - RabbitDistance*cos(TransformAngle + yRotation*PI/180 + VelVec);//RabbitNorth+(zListenerPos-RabbitNorth)*(1-cos(TransformAngle))-(xListenerPos-RabbitEast)*sin(TransformAngle);
        objects[0].xPos = xListenerPos + (objects[0].xPos - xListenerPos)*TransformDistance;
        objects[0].zPos = zListenerPos + (objects[0].zPos - zListenerPos)*TransformDistance;
        objects[1].xPos = RabbitEast;
        objects[1].zPos = RabbitNorth;
        objects[6].xPos = objects[0].xPos;
        objects[6].zPos = objects[0].zPos;
        }



        if (fabs(HeadingErr) < SilenceAngle)// || HeadingErr > 360-SilenceAngle)
        {
                objects[0].yPos = yListenerPos + 5000;
                objects[1].yPos = yListenerPos + 5000;
        }
        else
        {
                if (TaskMode<3) // for target and path use sound source of long distance (500m)
                {
                        objects[0].yPos = yListenerPos;// Toni: ovo znaci da je objekt (izvor zvuka) uvjek u ROV ravnini po z (dubini)
                        objects[6].yPos = yListenerPos + 5000;
                }
                else
                {
                        objects[6].yPos = yListenerPos;
                        objects[0].yPos = yListenerPos + 5000;
                }
                objects[1].yPos = yListenerPos + 5000;
        }

        // target or last WP won
        if ((fabs(xListenerPos) < 2 && fabs(zListenerPos) < 2 && TaskMode==1) || ((ActualWPindex > 2)))//NumberOfWP + 1)))// && JoyStickMode==1))// || fabs(zListenerPos)>15 || fabs(xListenerPos)>15)
        {
                objects[0].yPos = yListenerPos + 5000;
                objects[6].yPos = yListenerPos + 5000;
                objects[1].yPos = yListenerPos;
        }

        std::cout<<distance<<" "<<HE<<" "<<yRotation<<" "<<RabbitNorth<<" "<<RabbitEast<<" "<<ActualWPindex<<std::endl;
        std::cout<<std::endl;
        /*std::cout<<objects[0].xPos<<"  "<<xListenerPos<<std::endl;
        std::cout<<std::endl;
        std::cout<<objects[0].zPos<<"  "<<zListenerPos<<std::endl;
        std::cout<<std::endl;
        std::cout<<objects[0].yPos<<"  "<<yListenerPos<<std::endl;
        std::cout<<std::endl;*/


        updateObjectSoundPos(&objects[0]);
        updateObjectSoundPos(&objects[6]);
        updateObjectSoundPos(&objects[1]);

        //std::cout<<zListenerPosRel<<" "<<xListenerPosRel<<" "<<HE<<" "<<DirectionToTarget<<" "<<objects[0].zPos<<" "<<objects[0].xPos<<std::endl;

        // Upute gore-dolje
        if (Altitude < AltitudeSetPoint - 1.0f) //OVAJ RED UBACITI ZA TEREN UMJESTO REDA ISPOD
                //if (yListenerPos < AltitudeSetPoint - 0.5f)
        {
                objects[2].xPos = xListenerPos;
                objects[2].zPos = zListenerPos;
                objects[2].yPos = yListenerPos + 5;
        }
        else
        {
                objects[2].yPos = yListenerPos + 5000;
        }
        updateObjectSoundPos(&objects[2]);
        if (Altitude > AltitudeSetPoint + 1.0f) //OVAJ RED UBACITI ZA TEREN UMJESTO REDA ISPOD
                //if (yListenerPos > AltitudeSetPoint + 0.5f)
        {
                objects[3].xPos = xListenerPos;
                objects[3].zPos = zListenerPos;
                objects[3].yPos = yListenerPos - 5;//listenerVector.x;// = ;//-22 + 8.0f * sin(accumulatedTime)
        }
        else
        {
                objects[3].yPos = yListenerPos + 5000;
        }
        updateObjectSoundPos(&objects[3]);

        // Upute lijevo-desno


        if (HeadingErr > VerbalAngle && HeadingErr < 360-VerbalAngle && sin(HeadingErr*PI/180) < 0)
        {
                objects[4].xPos = xListenerPos;
                objects[4].zPos = zListenerPos;
                objects[4].yPos = yListenerPos;
        }
        else
        {
                objects[4].yPos = yListenerPos + 5000;
        }
        updateObjectSoundPos(&objects[4]);
        if (HeadingErr > VerbalAngle && HeadingErr < 360-VerbalAngle && sin(HeadingErr*PI/180) > 0)
        {
                objects[5].xPos = xListenerPos;
                objects[5].zPos = zListenerPos;
                objects[5].yPos = yListenerPos;
        }
        else
        {
                objects[5].yPos = yListenerPos + 5000;
        }
        updateObjectSoundPos(&objects[5]);

}

void doListenerMovement()
{
        // Update user movement

        std::string SingleLog;
        float step;
        const float MOVEMENT_SPEED = 0.1f;
        float forward = 0.0f;
        switch (SpeedProfile)
        {
        case 'V':
                step=(float)INTERFACE_UPDATETIME/1000*PathVelocity+0.3*(float)INTERFACE_UPDATETIME/1000*PathVelocity*(float)sin(accumulatedTime/3);
                break;
        case 'S':
                if (ActualWPindex > 1 && sqrt((xListenerPos - WPeast[ActualWPindex])*(xListenerPos - WPeast[ActualWPindex]) + (zListenerPos-WPnorth[ActualWPindex])*(zListenerPos-WPnorth[ActualWPindex])) < 40)
                {
                        step=(float)INTERFACE_UPDATETIME/1000*PathVelocity*1.2;
                }
                else
                {
                        step=(float)INTERFACE_UPDATETIME/1000*PathVelocity;
                }
                break;
        default:
                step=(float)INTERFACE_UPDATETIME/1000*PathVelocity;
        }

        Head = 0.0f;

        // OVAJ DIO TREBA UBACITI ZA TEREN
        //Pozicija ROVa (Listener-a) u metrima, ishodiste je u izvoru zvuka (cilja)

        //Ovdje ide iz simulatora dodavanje
        boost::mutex::scoped_lock lock(joyMux);
        if ((ObjectLong != target.position.north) || (ObjectLat != target.position.east))
        {WPupdateFlage=true;}
        ObjectLong = target.position.north;// + RabbitDistance*cos(target.orientation.yaw);
        ObjectLat = target.position.east;// + RabbitDistance*sin(target.orientation.yaw);
        ObjectDepth = 20;//target.position.depth;

        if (JoyStickMode==2)
        {yListenerPos = 20;}
        else
        {
                if (TaskMode == 1)
                {
                        zListenerPos = (state.position.north /*latLonMap["Northing"]*/ - ObjectLong); //relative to the target
                        xListenerPos = -(state.position.east /*latLonMap["Easting"]*/ - ObjectLat); //relative to the target
                        yListenerPos = 20;//state.position.depth; //rovStatus["Depth"];
                }
                else
                {
                        zListenerPos = -(state.position.north /*latLonMap["Northing"]*/ - 0*ObjectLong); //relative to the target
                        xListenerPos = (state.position.east /*latLonMap["Easting"]*/ - 0*ObjectLat); //relative to the target
                        yListenerPos = 20;//state.position.depth; //rovStatus["Depth"];}
                }
        }

                NumberOfWP = 2;
                if (WPupdateFlage)
                {
                        ActualWPindex = 1;
                        WPnorth[2] = -ObjectLong;
                        WPeast[2] = ObjectLat;
                        /*double zsign=1;
                        double xsign=1;
                        if (zListenerPos) zsign = fabs(zListenerPos)/(zListenerPos);
                        if (xListenerPos) xsign = fabs(xListenerPos)/(xListenerPos);
                        WPnorth[1] = zListenerPos - zsign*5;
                        WPeast[1] = xListenerPos - xsign*5;*/
                        if (TaskMode==3)
                        {
                                WPnorth[1] = zListenerPos + cos(atan2((WPeast[2]-xListenerPos),(WPnorth[2]-zListenerPos)))*15;
                                WPeast[1] = xListenerPos + sin(atan2((WPeast[2]-xListenerPos),(WPnorth[2]-zListenerPos)))*15;
                        }
                        else
                        {
                                WPnorth[1] = zListenerPos + cos(atan2((WPeast[2]-xListenerPos),(WPnorth[2]-zListenerPos)))*5;
                                WPeast[1] = xListenerPos + sin(atan2((WPeast[2]-xListenerPos),(WPnorth[2]-zListenerPos)))*5;
                        }

                        WPupdateFlage=false;
                }

                zRotation = state.orientation.roll; //rovStatus["Roll"]; //sve je izmjesano z-naprijed, x-lateral, y-gore/dolje
                xRotation = state.orientation.pitch; //rovStatus["Pitch"];
                if (JoyStickMode==1)
                {
                        yRotation = 180*state.orientation.yaw/M_PI; //rovStatus["Heading"];}
                        //std::cout<<yRotation<<" "<<yListenerPos<<" "<<TaskMode<<" "<<GuidanceMode<<std::endl;
                        ROS_ERROR("Output: %f %f %f %f %f %f %f %f %i",zListenerPos,xListenerPos,state.position.north,state.position.east,WPnorth[ActualWPindex],WPeast[ActualWPindex],RabbitNorth,RabbitEast,ActualWPindex);
                        //ROS_ERROR("Output: %f %f %f %f %f %f %f %f %i",zListenerPos,xListenerPos,RabbitNorth,RabbitEast,RabbitNorth1,RabbitNorth2,PathX,PathY,ActualWPindex);
                        TauX = 0; //rovStatus["TauX"];
                        TauY = 0; //rovStatus["TauY"];
                        TauZ = 0; //rovStatus["TauZ"];
                        TauK = 0; //rovStatus["TauK"];
                        TauM = 0; //rovStatus["TauM"];
                        TauN = 0; //rovStatus["TauN"];
                        frwVelVector = state.body_velocity.x; //rovStatus["u"];
                        latVelVector = state.body_velocity.y; //rovStatus["v"];
                }
                Altitude = state.altitude; //dvlStatus["Altitude"];
                if (fabs(xListenerPos - WPeast[ActualWPindex]) < 2 && fabs(zListenerPos-WPnorth[ActualWPindex]) < 2 && ActualWPindex > 1)
                {
                        ActualWPindex += 1;}
                if (TaskMode==3)
                {if (ActualWPindex == 1 && sqrt((xListenerPos - WPeast[ActualWPindex])*(xListenerPos - WPeast[ActualWPindex]) + (zListenerPos-WPnorth[ActualWPindex])*(zListenerPos-WPnorth[ActualWPindex])) < RabbitDistance)
                {
                        ActualWPindex += 1;}
                }
                else
                {if (ActualWPindex == 1 && sqrt((xListenerPos - WPeast[ActualWPindex])*(xListenerPos - WPeast[ActualWPindex]) + (zListenerPos-WPnorth[ActualWPindex])*(zListenerPos-WPnorth[ActualWPindex])) < 2)
                {
                        ActualWPindex += 1;}
                }
                // Set virtual target (rabbit) position

                if (TaskMode>1 && ActualWPindex>1)
                {
                        PathStartY = WPeast[ActualWPindex-1] - WPeast[ActualWPindex];
                        PathStartX = WPnorth[ActualWPindex-1] - WPnorth[ActualWPindex];
                }
                float kLine = 1000000;
                float kLineAngle = PI/2;
                if (PathStartX != 0)
                {
                        kLine = PathStartY / PathStartX;
                        kLineAngle = atan2(PathStartY,PathStartX);
                }
                aSQR = 1 + kLine*kLine;

                // PathX i Path Y su tocka projekcije ROV pozicije na path (pravi kut)
                if (TaskMode>1)
                {
                        if (PathStartX == 0)
                        {
                        PathX = WPnorth[ActualWPindex];
                        PathY =xListenerPos;
                        }
                        else
                        {
                        PathX = (kLine*(xListenerPos - WPeast[ActualWPindex] + kLine*WPnorth[ActualWPindex]) + zListenerPos)/aSQR;
                        PathY = WPeast[ActualWPindex] + kLine*PathX - kLine*WPnorth[ActualWPindex];
                        }
                }
                else
                {
                        PathX = PathStartX*(PathStartY * xListenerPos + PathStartX * zListenerPos)/(PathStartX * PathStartX + PathStartY * PathStartY);
                        PathY = PathStartY*(PathStartY * xListenerPos + PathStartX * zListenerPos)/(PathStartX * PathStartX + PathStartY * PathStartY);
                }

                DistanceToPath = sqrt((xListenerPos - PathY) * (xListenerPos - PathY) + (zListenerPos - PathX) * (zListenerPos - PathX));


                if ((TaskMode > 1 && sqrt((zListenerPos-WPnorth[NumberOfWP])*(zListenerPos-WPnorth[NumberOfWP]) + (xListenerPos-WPeast[NumberOfWP])*(xListenerPos-WPeast[NumberOfWP])) > RabbitDistance))
                        //Da li smo dosli do targeta, odnosno zadnjeg WPa
                {

                                if (TaskMode==3) //meaning trajectory tracking
                                {
                                        if (ActualWPindex>2 && ActualWPindex>ActualWPindexOld) //if new WP
                                        {
                                                RabbitNorth = WPnorth[ActualWPindex-1] - cos(kLineAngle)*RabbitDistance;
                                                RabbitEast = WPeast[ActualWPindex-1] + sin(kLineAngle)*RabbitDistance;
                                                wait = true;
                                        }
                                        else
                                        {
                                                if ((wait==false || (wait==true && fabs(HE)<10)) && ActualWPindex>1) // if WP is changed recently, wait until heading is cca. inline with path segment
                                                {
                                                        RabbitNorth = RabbitNorthOld - cos(kLineAngle)*step;
                                                        RabbitEast = RabbitEastOld - sin(kLineAngle)*step;
                                                        wait=false;
                                                }
                                        }
                                        RabbitNorthOld = RabbitNorth;
                                        RabbitEastOld = RabbitEast;
                                        ActualWPindexOld=ActualWPindex;
                                        //std::cout<<kLineAngle*180/PI<<" "<<step<<" "<<sin(kLineAngle)*step<<" "<<RabbitEast<<" "<<ActualWPindex<<std::endl;
                                }
                                else
                                {
                                        if (DistanceToPath > RabbitDistance)
                                                                {
                                                                        RabbitNorth = PathX;
                                                                        RabbitEast = PathY;
                                                                        RabbitNorthOld = RabbitNorth;
                                                                        RabbitEastOld = RabbitEast;
                                                                }
                                        else
                                        {
                                                if (PathStartX == 0)
                                                {
                                                        RabbitNorth = WPnorth[ActualWPindex];
                                                        if (PathStartY<0)
                                                        {
                                                                RabbitEast = xListenerPos + sqrt(RabbitDistance*RabbitDistance-(zListenerPos - RabbitNorth)*(zListenerPos - RabbitNorth));
                                                        }
                                                        else
                                                        {
                                                                RabbitEast = xListenerPos - sqrt(RabbitDistance*RabbitDistance-(zListenerPos - RabbitNorth)*(zListenerPos - RabbitNorth));
                                                        }
                                                }
                                                else
                                                {
                                                        bSQR = -2*((PathX-WPnorth[ActualWPindex]) + kLine*(PathY-WPeast[ActualWPindex]));
                                                        cSQR = (PathX-WPnorth[ActualWPindex])*(PathX-WPnorth[ActualWPindex])+(PathY-WPeast[ActualWPindex])*(PathY-WPeast[ActualWPindex])+DistanceToPath*DistanceToPath-RabbitDistance*RabbitDistance;
                                                        RabbitNorth1 = (-bSQR+sqrt(bSQR*bSQR-4*aSQR*cSQR))/(2*aSQR);
                                                        RabbitNorth2 = (-bSQR-sqrt(bSQR*bSQR-4*aSQR*cSQR))/(2*aSQR);
                                                        if (fabs(RabbitNorth1)<fabs(RabbitNorth2))
                                                                {
                                                                RabbitNorth = RabbitNorth1 + WPnorth[ActualWPindex];
                                                                RabbitEast = kLine * RabbitNorth1 + WPeast[ActualWPindex];
                                                                }
                                                        else
                                                        {
                                                                RabbitNorth= RabbitNorth2 + WPnorth[ActualWPindex];
                                                                RabbitEast = kLine * RabbitNorth2 + WPeast[ActualWPindex];
                                                        }
                                                }
                                        }
                        }
                }
                else
                {
                        RabbitNorth = WPnorth[ActualWPindex];
                        RabbitEast = WPeast[ActualWPindex];
                }
                if (TaskMode == 1) // if guidance to Target, rabbit is target
                {
                        RabbitNorth = 0.0f;
                        RabbitEast = 0.0f;
                }
                if (ActualWPindex == 1) //guidance to the first WP of the path
                {
                        if (TaskMode==3) //meaning trajectory tracking
                                {
                                RabbitNorth = zListenerPos + cos(atan2((WPeast[1]-xListenerPos),(WPnorth[1]-zListenerPos)))*RabbitDistance;
                                RabbitEast = xListenerPos + sin(atan2((WPeast[1]-xListenerPos),(WPnorth[1]-zListenerPos)))*RabbitDistance;
                                }
                        else
                                {
                                RabbitNorth = WPnorth[1];
                                RabbitEast = WPeast[1];
                                RabbitNorthOld = RabbitNorth;
                                RabbitEastOld = RabbitEast;
                                }
                }

                ti += (float)INTERFACE_UPDATETIME / 1000.0f;
                if (ti > 2)
                {r = rand()%3;
                ti = 0.0f;}
                //std::cout<<r-1<<" "<<ti<<std::endl;//<<RabbitNorth<<" "<<RabbitEast<<" "<<PathX<<" "<<PathY<<" "<<PathStartX<<" "<<PathStartY<<" "<<ActualWPindex<<" "<<DistanceToPath<<" "<<zListenerPos<<" "<< xListenerPos<<std::endl;

                //std::cout<<latLonMap["Northing"]<<" "<<latLonMap["Easting"]<<" "<<rovStatus["Depth"]<<" "<<TARGETStatus["Northing"]<<" "<<TARGETStatus["Easting"]<<" "<<TARGETStatus["Depth"]<<" "<<rovStatus["Heading"]<<" "<<dvlStatus["Altitude"]<<" "<<AltitudeSetPoint<<std::endl;
                if (JoyStickMode==2)
                {
                        if (abs(joystickData.axes[1])>2000)
                        {
                                forw = -0.1*joystickData.axes[1]/intMax;
                        }
                        else
                        {
                                forw = 0;
                        }
                }
                if (JoyStickMode==2)
                {
                        if (abs(joystickData.axes[0])>2000)
                        {
                                yRotation += 1.0f*joystickData.axes[0]/intMax;
                                if (yRotation < 0)
                                {yRotation = yRotation + 360;}
                                else
                                {
                                        if (yRotation > 360)
                                        {yRotation = yRotation - 360;}
                                }
                        }

                }

                // OVO OSTAJE I ZA TEREN I ZA SIMULACIJU
                lastyRotation=yRotation; //Toni spremi stvarni heading
                // simulacija

                //Lateral movement, TONI: Isto kao gore samo sa ukljucenim pokretom glave
                float xRight = (float)cos(yRotation * (PI / 180.0f));
                float yRight = 0.0f;
                float zRight = (float)sin(yRotation * (PI / 180.0f));

                //Forward-backward movement TONI: Isto kao gore samo sa ukljucenim pokretom glave
                float xForward = (float)sin(yRotation * (PI / 180.0f)) * cos(xRotation  * (PI / 180.0f));
                float yForward = -(float)sin(xRotation  * (PI / 180.0f));
                float zForward = -(float)cos(yRotation * (PI / 180.0f)) * cos(xRotation  * (PI / 180.0f));

                if (JoyStickMode==2)
                {
                        xListenerPos += xForward * forw;
                        yListenerPos += yForward * forw;
                        zListenerPos += zForward * forw;
                }

                yRotation=lastyRotation; //Toni: vrati pravi heading

                //Up-Down movement simulacija
                /*yListenerPos += up;

        if (yListenerPos < 1.0f)
    {
                yListenerPos = 1.0f;
    }*/

                if (yListenerPos < 0.0f)
                {
                        yListenerPos = 0.0f;
                }

                // cross product TONI????
                float xUp = yRight * zForward - zRight * yForward;
                float yUp = zRight * xForward - xRight * zForward;
                float zUp = xRight * yForward - yRight * xForward;

                /*std::cout<<xForward<<" "<<yForward<<" "<< zForward<<std::endl;
        std::cout<<std::endl;
        std::cout<<xUp<<" "<<yUp<<" "<< zUp<<std::endl;
        std::cout<<std::endl;*/

                // Racunanje heading-a prema target-u ili rabbit-u
                if (TaskMode ==1)
                {
                        if (JoyStickMode == 1)
                        {
                                zListenerPosRel = zListenerPos;
                                xListenerPosRel = xListenerPos;
                        }
                        else
                        {
                                zListenerPosRel = 0;//20*cos(PI/180*InputStoh);
                                xListenerPosRel = -25;//20*sin(PI/180*InputStoh);
                        }

                        zListenerPosRel = zListenerPos;
                        xListenerPosRel = xListenerPos;
                }
                else
                {
                        zListenerPosRel = zListenerPos - RabbitNorth;
                        xListenerPosRel = xListenerPos - RabbitEast;
                }

                DirectionToTarget = atan2(-xListenerPosRel,zListenerPosRel)*180.0f/PI;

                //OVAJ DIO SE KORISTI I ZA TEREN I ZA SIMULACIJU
                // Update listener
                {
                        //TONI nova pozicija slusaca, koristi se i za Teren i simulaciju
                        FMOD_VECTOR listenerVector;
                        if (JoyStickMode==2)
                        {
                                listenerVector.x = xListenerPosRel;
                                listenerVector.y = yListenerPos;
                                listenerVector.z = zListenerPosRel;
                        }
                        else
                        {
                                listenerVector.x = xListenerPos;
                                listenerVector.y = yListenerPos;
                                listenerVector.z = zListenerPos;
                        }

                        static FMOD_VECTOR lastpos = { 0.0f, 0.0f, 0.0f };
                        static bool bFirst = true;

                        FMOD_VECTOR forward;
                        FMOD_VECTOR up;
                        FMOD_VECTOR vel;

                        forward.x = xForward;
                        forward.y = yForward;
                        forward.z = zForward;
                        up.x = xUp;
                        up.y = yUp;
                        up.z = zUp;

                        // ********* NOTE ******* READ NEXT COMMENT!!!!!
                        // vel = how far we moved last FRAME (m/f), then time compensate it to SECONDS (m/s).
                        //TONI brzina slusaca
                        vel.x = (listenerVector.x - lastpos.x) * (Doppler * 1000.0f / (float)INTERFACE_UPDATETIME); // BRZINA UVECANA RADI UVODJENJA POJACANOG DOPPLERA
                        vel.y = (listenerVector.y - lastpos.y) * (Doppler * 1000.0f / (float)INTERFACE_UPDATETIME);
                        vel.z = (listenerVector.z - lastpos.z) * (Doppler * 1000.0f / (float)INTERFACE_UPDATETIME);

                        if (bFirst)
                        {
                                bFirst = false;
                                vel.x = 0;
                                vel.y = 0;
                                vel.z = 0;
                                HeadIni = Head;
                        }
                        if (vel.x==0 && vel.z==0)
                        {velAtitude = lastvelAtitude;}
                        else
                        {
                                if (vel.z < 0.0f)
                                {velAtitude = atan(-vel.x/vel.z);}
                                else
                                {
                                        if (vel.z == 0.0f)
                                        {
                                                if (vel.x < 0.0f)
                                                {velAtitude = 1.5*PI;}
                                                else
                                                {velAtitude = 0.5*PI;}
                                        }
                                        else
                                        {
                                                velAtitude = atan(-vel.x/vel.z)+PI;
                                        }
                                }
                                velAtitude = fmod(180*velAtitude/PI+360,360);
                                lastvelAtitude = velAtitude;
                        }
                        //std::cout<<yRotation<<" "<<forw<<" "<<zListenerPos<<" "<<xListenerPos<<" "<<std::endl;

                        /*static FMOD_VECTOR lastVel = { 0.0f, 0.0f, 0.0f };
                // store pos and vel for next time

                if (lastVel.x != 0.0f || lastVel.y != 0.0f || lastVel.z != 0.0f)
                {
                        if (vel.x == 0.0f && vel.y == 0.0f && vel.z == 0.0f)
                        {
                                int test = 0;
                        }
                }
                lastVel = vel;*/
                        lastpos = listenerVector;
                        lastObjectLong = zPos;

                        FMOD_RESULT result = fmodSystem->set3DListenerAttributes(0, &listenerVector, &vel, &forward, &up);
                        ERRCHECK(result);
                }
        }

        void doUpdateVolume()
        {
                //TONI pojacavanje i stisavanje zvuka tipke Z/z
                if (ambientVolUp)
                {
                        char volumestring[64];

                        ambientVolume += 0.025;
                        if (ambientVolume > 1.0f)
                        {
                                ambientVolume = 1.0f;
                        }

                        sprintf(volumestring, "Ambient Volume: %0.3f", ambientVolume);

#ifdef SHOW_GUI_DEBUG_TEXT
                        statusText.push(volumestring);
#endif
                }
                else if (ambientVolDown)
                {
                        char volumestring[64];

                        ambientVolume -= 0.025;
                        if (ambientVolume < 0.0f)
                        {
                                ambientVolume = 0.0f;
                        }

                        sprintf(volumestring,"Ambient Volume: %0.3f", ambientVolume);

#ifdef SHOW_GUI_DEBUG_TEXT
                        statusText.push(volumestring);
#endif
                }
        }

        void timerFunc(int nValue)
        {
                std::string TimeLog;
                static bool firsttime = true;
                FMOD_RESULT result;

                //doGeometryMovement();

                doSoundMovement();
                doListenerMovement();
                doUpdateVolume();

                result = FMOD::NetEventSystem_Update();
                ERRCHECK(result);
                result = fmodEventSystem->update();
                ERRCHECK(result);

                accumulatedTime += (float)INTERFACE_UPDATETIME / 1000.0f;

                glutPostRedisplay();
                glutTimerFunc(INTERFACE_UPDATETIME, timerFunc, 0);

#if 0
                if (firsttime)
                {
                        SetWindowPos(
                                        GetForegroundWindow(),    // handle to window
                                        HWND_TOPMOST,             // placement-order handle
                                        0,                        // horizontal position
                                        0,                        // vertical position
                                        width,                    // width
                                        height,                   // height
                                        SWP_NOMOVE
                        );
                        firsttime = false;
                }
#endif
        }

        void keyboardFunc(unsigned char key, int x, int y)
        {
                key1 = key;
                switch (key)
                {
                case 'w':
                        moveForward = true;
                        break;
                case 's':
                        moveBackward = true;
                        break;
                case 'j':
                        moveRotateAntiClock = true;
                        break;
                case 'l':
                        moveRotateClock = true;
                        break;
                case 'a':
                        moveLeft = true;
                        break;
                case 'd':
                        moveRight = true;
                        break;
                case ' ':
                        moveUp = true;
                        break;
                case 'c':
                        moveDown = true;
                        break;
                case 'z' :
                        ambientVolDown = true;
                        break;
                case 'Z' :
                        ambientVolUp = true;
                        break;
                case 'v':
                        masterVolDown = true;
                        break;
                case 'V':
                        masterVolUp = true;
                        break;

                case 'p':
                {
                        float value;

                        fmodEventParameter->getValue(&value);

                        fmodEventParameter->setValue(value - 20.0f);
                }
                break;

                case 'P':
                {
                        float value;

                        fmodEventParameter->getValue(&value);

                        fmodEventParameter->setValue(value + 20.0f);
                }
                break;

                case 'f':
                        moveFast = true;
                        break;
                }
        }

        void keyboardUpFunc(unsigned char key, int x, int y)
        {
                key1 = 0;
                switch (key)
                {
                case 'w':
                        moveForward = false;
                        break;
                case 's':
                        moveBackward = false;
                        break;
                case 'j':
                        moveRotateAntiClock = false;
                        break;
                case 'l':
                        moveRotateClock = false;
                case 'a':
                        moveLeft = false;
                        break;
                case 'd':
                        moveRight = false;
                        break;
                case ' ':
                        moveUp = false;
                        break;
                case 'c':
                        moveDown = false;
                        break;
                case 'z' :
                        ambientVolDown = false;
                        break;
                case 'Z' :
                        ambientVolUp = false;
                        break;
                case 'v':
                        masterVolDown = false;
                        break;
                case 'V':
                        masterVolUp = false;
                        break;
                case 'f':
                        moveFast = false;
                        break;
                case 27:
                        exit(1);
                        break;
                }
        }

        void specialKeyUpFunc(int key, int x, int y)
        {
                switch (key)
                {
                case GLUT_KEY_F1:
                        showhelp = !showhelp;
                        break;
                case GLUT_KEY_F2:
                        if(fullscreen)
                        {
                                glutPositionWindow(20,40);
                                glutReshapeWindow(500,500);
                                fullscreen = false;
                        }
                        else
                        {
                                glutFullScreen();
                                fullscreen = true;
                        }
                        break;
                case GLUT_KEY_F3:
                        rendermode = (rendermode == GL_LINE ? GL_FILL : GL_LINE);
                        break;
                case GLUT_KEY_F11:
                        showdebug = !showdebug;
                        break;
                case GLUT_KEY_F8:
                        rotatingMesh.geometry->setActive(false);
                        walls.geometry->setActive(false);
                        break;
                case GLUT_KEY_F9:
                        rotatingMesh.geometry->setActive(true);
                        walls.geometry->setActive(true);
                        break;
                }
        }

        void display(void)
        {
                // Show listener position
                {
                        char text[64];

                        sprintf(text, "Listener Pos: (%.2f, %.2f, %.2f)", xListenerPos, yListenerPos, zListenerPos);
#ifdef SHOW_GUI_DEBUG_TEXT
                        debugText.push(std::string(text));
#endif
                }
                // Show cpu usage position
                {
                        char text[64];
                        float dsp, stream, geometry, update, total;

                        fmodSystem->getCPUUsage(&dsp, &stream, &geometry, &update, &total);
                        sprintf(text, "CPU Usage : (%.2f, %.2f, %.2f, %.2f, %.2f)", dsp, stream, geometry, update, total);
#ifdef SHOW_GUI_DEBUG_TEXT
                        debugText.push(std::string(text));
#endif
                }

                /*
        3D RENDERING
                 */

                // update view
                glMatrixMode(GL_PROJECTION);
                glLoadIdentity();
                gluPerspective(
                                60.0,                                                           // fov
                                (float)width / (float)height,           // aspect
                                0.1,                                                            // near
                                500.0                                                           // far
                );


                glRotatef(xRotation, 1.0f, 0.0f, 0.0f);
                glRotatef(yRotation, 0.0f, 1.0f, 0.0f);
                glTranslatef(-xListenerPosRel, -yListenerPos, -zListenerPosRel);
                glMatrixMode(GL_MODELVIEW);
                glLoadIdentity();

                // clear
                glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
                glClearColor(0.4, 0.6f, 1.0f, 0.0f);

                glEnable(GL_TEXTURE_2D);
                glBindTexture( GL_TEXTURE_2D, texture );

                // draw geometry
                //drawGeometry(walls);
                //drawGeometry(rotatingMesh);

                // draw skybox
                gui->drawSkyBox(xListenerPos, yListenerPos);

                glDisable(GL_TEXTURE_2D);

                // draw sound objects
                int object;
                if (TaskMode == 3)
                {object=6;}
                else
                {object=0;}
                //for (object = 0; object < NUM_OBJECTS-6; object++)
                {
                        char txt[256];


                        float directOcclusion = 1.0f;
                        float reverbOcclusion = 1.0f;

                        // set colour baced on direct occlusion
                        //              objects[object].channel->getOcclusion(&directOcclusion, &reverbOcclusion);
                        float intensity = 1.0f;// - directOcclusion;

                        glPolygonMode(GL_FRONT_AND_BACK, rendermode);
                        glPushMatrix();
                        //glTranslatef(objects[object].xPos, objects[object].yPos, objects[object].zPos);
                        glTranslatef(0, objects[object].yPos, 0);

                        sprintf(txt, "Sound object (%d): %.2f, %.2f, %.2f", object, objects[object].xPos, objects[object].yPos, objects[object].zPos);
#ifdef SHOW_GUI_DEBUG_TEXT
                        debugText.push(std::string(txt));
#endif

                        glPushAttrib(GL_LIGHTING_BIT);

                        intensity *= 0.75f;
                        float color[4] = { intensity, intensity, 0.0f, 0.0f };
                        if(object == 0)
                        {//objekt broj 0 je ofarban crveno
                                color[1] = 0;
                                //std::cout<<objects[0].zPos<<"  "<<objects[0].xPos<<std::endl;
                                //std::cout<<std::endl;
                        }
                        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);
                        intensity *= 0.5f;
                        float ambient[4] = { intensity, intensity, 0.0f, 0.0f };
                        if(object == 0)
                        {//objekt broj 0 je ofarban crveno
                                ambient[1] = 0;
                        }

                        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, ambient);


                        glRotatef(accumulatedTime * 200.0f, 0.0f, 1.0f, 0.0f);
                        {
                                FMOD_VECTOR soundorientation;
                                float rad = (accumulatedTime * 200.0f);

                                rad *= 3.14159f;
                                rad /= 180.0f;

                                soundorientation.x = 0;//sinf(rad);
                                soundorientation.y = 0;
                                soundorientation.z = 0;//cosf(rad);

                                objects[object].event->set3DAttributes(0, 0, &soundorientation);
                        }

                        glutSolidTeapot(0.1f);//SolidTorus(0.15f, 0.6f, 8, 16);
                        glPopAttrib();
                        glPopMatrix();
                }

                /*
        Draw blue transparent blue quads to entry to water room
                 */
                drawWaterRoom();

                /*
        Do water effects if we are in the water room
                 */
                inWater();


                /*
        2D RENDERING
                 */
                glMatrixMode(GL_PROJECTION);
                glLoadIdentity();
                gluOrtho2D(0.0f, (GLsizei)width, 0.0f, (GLsizei)height);
                glMatrixMode(GL_MODELVIEW);
                glLoadIdentity();

                glDisable(GL_LIGHTING);
                glDisable(GL_NORMALIZE);
                glDisable(GL_DEPTH_TEST);

                /*
        Render text
                 */
                renderUiText();


                glEnable(GL_DEPTH_TEST);
                glEnable(GL_LIGHTING);
                glEnable(GL_LIGHT0);
                glEnable(GL_NORMALIZE);
                glShadeModel(GL_SMOOTH);
                glPolygonMode(GL_FRONT_AND_BACK, rendermode);

                // finish
                glutSwapBuffers();
        }

        void reshapeFunc(int w, int h)
        {
                width = w;
                height = h;
                glViewport(0, 0, (GLsizei)w, (GLsizei)h);
        }

        GLuint loadTexturePNG(const char *filename)
        {
                GLuint texture = SOIL_load_OGL_texture(filename, 0, 0, 0);
                glBindTexture(GL_TEXTURE_2D, texture);
                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

                return texture;
        }

        GLuint loadTexture(const char *filename)
        {
                GLuint texture;
                int width;
                int height;
                unsigned char *data;
                FILE *file;

                // open texture data
                file = fopen( filename, "rb" );
                if ( file == NULL )
                        return 0;

                width = 128;
                height = 128;
                data = (unsigned char*)malloc(width * height * 3);

                fread(data, width * height * 3, 1, file );
                fclose(file);

                glGenTextures(1, &texture);
                glBindTexture(GL_TEXTURE_2D, texture);

                glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
                glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_NEAREST );
                glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
                glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
                glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);

                gluBuild2DMipmaps(
                                GL_TEXTURE_2D,
                                3,
                                width,
                                height,
                                GL_RGB,
                                GL_UNSIGNED_BYTE,
                                data);

                free(data);

                return texture;
        }


        void init(void)
        {
                FMOD_RESULT      result;
                bool             listenerflag = true;
                FMOD_VECTOR      listenerpos  = { 0.0f, 0.0f, 0.0f };
                FMOD_SPEAKERMODE speakermode;

                printf("==================================================================\n");
                printf("3D example.  Copyright (c) Firelight Technologies 2004-2011.\n");
                printf("==================================================================\n\n");

                result = FMOD::EventSystem_Create(&fmodEventSystem);
                ERRCHECK(result);
                result = FMOD::NetEventSystem_Init(fmodEventSystem);
                ERRCHECK(result);
                result = fmodEventSystem->getSystemObject(&fmodSystem);
                ERRCHECK(result);
                result = fmodSystem->getDriverCaps(0,0,0,&speakermode);
                ERRCHECK(result);
                result = fmodSystem->setSpeakerMode(speakermode);
                ERRCHECK(result);
                //result = fmodEventSystem->init(32, FMOD_INIT_3D_RIGHTHANDED | FMOD_INIT_SOFTWARE_OCCLUSION | FMOD_INIT_SOFTWARE_HRTF, 0, FMOD_EVENT_INIT_NORMAL);
                result = fmodEventSystem->init(32, FMOD_INIT_3D_RIGHTHANDED, 0, FMOD_EVENT_INIT_NORMAL);
                //result = fmodEventSystem->init(32, FMOD_INIT_3D_RIGHTHANDED | FMOD_INIT_SOFTWARE_OCCLUSION | FMOD_INIT_SOFTWARE_HRTF | FMOD_INIT_DSOUND_HRTFFULL, 0, FMOD_EVENT_INIT_NORMAL);
                ERRCHECK(result);
                //result = fmodEventSystem->setMediaPath("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media1/");
                ERRCHECK(result);
                ros::NodeHandle ph("~");
                std::string mediaPath("files");
                ph.param("MediaPath",mediaPath,mediaPath);
                result = fmodEventSystem->load((mediaPath + "/examples1.fev").c_str(), 0, &fmodEventProject);
                ERRCHECK(result);
                result = fmodEventProject->getGroup("FeatureDemonstration/3D Events", true, &fmodEventGroup);
                ERRCHECK(result);

                /*
        Create a programmer created lowpass filter to apply to everything.
                 */
                result = fmodSystem->createDSPByType(FMOD_DSP_TYPE_LOWPASS, &global_lowpass);
                ERRCHECK(result);

                result = global_lowpass->setParameter(FMOD_DSP_LOWPASS_CUTOFF, 1000);
                ERRCHECK(result);

                result = global_lowpass->setBypass(true);   // turn it off to start with.
                ERRCHECK(result);

                result = fmodSystem->addDSP(global_lowpass, 0);
                ERRCHECK(result);


                initObjects();

                result = fmodSystem->setGeometrySettings(200.0f);
                ERRCHECK(result);

                printf("Loading geometry...");

                // load objects
                //initGeometry("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/walls.bin", walls, true);
                //initGeometry("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/center.bin", rotatingMesh);
                //initGeometry("files/walls.bin", walls, true);
                //initGeometry("files/center.bin", rotatingMesh);

                printf("done.\n");

                /*
        Load textures
                 */
                printf("Loading textures...\n");

                /*texture = loadTexture("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/texture.img");
    skyboxTexture[0] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/front.png");
    skyboxTexture[1] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/right.png");
    skyboxTexture[2] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/back.png");
    skyboxTexture[3] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/left.png");
    skyboxTexture[4] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/top.png");
    skyboxTexture[5] = loadTexturePNG("C:/Program Files/FMOD SoundSystem/FMOD Programmers API Win32/fmoddesignerapi/examples/media/skybox/bluesky/bottom.png");
                 */
                //              texture = loadTexture("files/texture.img");
                //              skyboxTexture[0] = loadTexturePNG("files/front.png");
                //              skyboxTexture[1] = loadTexturePNG("files/right.png");
                //              skyboxTexture[2] = loadTexturePNG("files/back.png");
                //              skyboxTexture[3] = loadTexturePNG("files/left.png");
                //              skyboxTexture[4] = loadTexturePNG("files/top.png");
                //              skyboxTexture[5] = loadTexturePNG("files/bottom.png");
                //
                //
                //              printf("done.\n");
                //
                //              // setup lighting
                //              GLfloat lightDiffuse[] = {1.0, 1.0, 1.0, 1.0};
                //              GLfloat lightPosition[] = {300.0, 1000.0, 400.0, 0.0};
                //              GLfloat lightAmbiant[] = {1.25, 1.25, 1.25, 1.0};
                //              glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
                //              glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
                //              glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbiant);
                //              glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 1.0f);
                //              glEnable(GL_LIGHT0);
                //              glEnable(GL_LIGHTING);
                //
                //              // setup fog(water)
                //              GLfloat fogColor[4] = {0.0f,0.1f,0.9f,1.0f};
                //
                //              glFogi(GL_FOG_MODE, GL_EXP);        // Fog Mode
                //              glFogfv(GL_FOG_COLOR, fogColor);    // Set Fog Color
                //              glFogf(GL_FOG_DENSITY, 0.15f);      // How Dense Will The Fog Be
                //              glHint(GL_FOG_HINT, GL_DONT_CARE);      // Fog Hint Value
                //              glFogf(GL_FOG_START, 0.0f);                     // Fog Start Depth
                //              glFogf(GL_FOG_END, 1.0f);                       // Fog End Depth
                //
                //              glEnable(GL_DEPTH_TEST);
                //
                //              glMatrixMode(GL_MODELVIEW);
                //              glLoadIdentity();
        }

        GlutCloseClass::~GlutCloseClass()
        {
                glDeleteTextures( 1, &texture );

                freeGeometry(walls);
                freeGeometry(rotatingMesh);

                fmodEventSystem->release();
                FMOD::NetEventSystem_Shutdown();

                //_CrtDumpMemoryLeaks();
        }
}

using namespace FMOD;

void handleManual(const sensor_msgs::Joy::ConstPtr& joy)
{
        boost::mutex::scoped_lock l(joyMux);
        joystickData.axes.resize(6);
        joystickData.axes[0] = joy->axes[0]*32768;
        joystickData.axes[1] = joy->axes[1]*32768;
}

void handleTarget(const auv_msgs::NavSts::ConstPtr& ref)
{
        boost::mutex::scoped_lock l(joyMux);
        target = *ref;
}

void handleTarget2(const geometry_msgs::PointStamped::ConstPtr& ref)
{
        boost::mutex::scoped_lock l(joyMux);
        target.position.north = ref->point.x;
        target.position.east = ref->point.y;
        target.position.depth = ref->point.z;
}

void handleEstimates(const auv_msgs::NavSts::ConstPtr& estimate)
{
        boost::mutex::scoped_lock l(joyMux);
        state = *estimate;
        std_msgs::Float32MultiArrayPtr data(new std_msgs::Float32MultiArray());

        LogData[0] = DistanceToPath;
        LogData[1] = DirectionToTarget;
        LogData[2] = RabbitNorth;
        LogData[3] = RabbitEast;
        LogData[4] = objects[0].zPos;
        LogData[5] = objects[0].xPos;
        LogData[6] = objects[6].zPos;;
        LogData[7] = objects[6].xPos;;
        LogData[8] = WPnorth[1];
        LogData[9] = WPeast[1];
        LogData[10] = WPnorth[2];
        LogData[11] = WPeast[2];

        data->data.insert(data->data.begin(), &LogData[0], &LogData[12]);
        data->data.insert(data->data.end(), configInfo.begin(), configInfo.end());
        infopub.publish(data);
}


int     main(int argc, char **argv)
{
        ros::init(argc,argv,"audioex");
        ros::NodeHandle nh;
        ros::Subscriber manualIn = nh.subscribe<sensor_msgs::Joy>("joy",1,&handleManual);

        infopub = nh.advertise<std_msgs::Float32MultiArray>("acousticvr_info",1);
        //Initialze subscribers
        ros::Subscriber targetRef = nh.subscribe<auv_msgs::NavSts>("target_navsts", 1,
                        &handleTarget);
        ros::Subscriber targetRef2 = nh.subscribe<geometry_msgs::PointStamped>("target_point", 1,
                        &handleTarget2);
        ros::Subscriber stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
                        &handleEstimates);
        boost::thread t(static_cast<void(*)(void)>(&ros::spin));
        joystickData.axes.resize(6);


        try
        {
                ros::NodeHandle ph("~");
                ph.param("ObjectLat",ObjectLat,ObjectLat);
                ph.param("ObjectLong", ObjectLong, ObjectLong);
                ph.param("AltitudeSetPoint", AltitudeSetPoint, AltitudeSetPoint);
                ph.param("Doppler", Doppler, Doppler);
                ph.param("Silence_angle", SilenceAngle, SilenceAngle);
                ph.param("Verbal_angle", VerbalAngle, VerbalAngle);
                ph.param("NonLinear_coeff", NonLinearCoeff, NonLinearCoeff);
                ph.param("Guidance_Mode", GuidanceMode, GuidanceMode);
                ph.param("Task_Mode", TaskMode, TaskMode);
                ph.param("K_Diter_Time", KDiterTime, KDiterTime);
                ph.param("AmpDiter", AmpDiter, AmpDiter);
                ph.param("FreqDiter", FreqDiter, FreqDiter);
                ph.param("JoyStick_Mode", JoyStickMode, JoyStickMode);
                ph.param("PathVelocity", PathVelocity, PathVelocity);
                ph.param("SpeedProfile", SpeedProfile, SpeedProfile);
                ph.param("NonLinear_coeff_distance", NonLinearCoeffDist, NonLinearCoeffDist);
                ph.param("Kintegral", Ki, Ki);
                ph.param("Sound", Sound, Sound);
                ph.param("PathStartX", PathStartX1, PathStartX1);
                ph.param("PathStartY", PathStartY1, PathStartY1);
                ph.param("RabbitDistance", RabbitDistance, RabbitDistance);

                float LogConfig[8] = {NonLinearCoeff,TaskMode,PathVelocity,SpeedProfile,RabbitDistance,NonLinearCoeffDist,Ki,Sound};
                configInfo.assign(&LogConfig[0], &LogConfig[8]);

                // Inicijalizacija joysticka

                srand((unsigned)time(0));

                //No glut configuration is needed really
                //              glutInit(&argc, argv);
                //              glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
                //              glutInitWindowSize(width, height);
                //              glutCreateWindow("Audio guidance");
                std::string texturePath("files");
                ph.param("TexturePath",texturePath,texturePath);

                gui.reset(new labust::gui::AAGui(texturePath));
                glutDisplayFunc(display);
                glutReshapeFunc(reshapeFunc);
                glutMouseFunc(mouseFunc);
                glutMotionFunc(motionFunc);
                glutKeyboardFunc(keyboardFunc);
                glutKeyboardUpFunc(keyboardUpFunc);
                glutSpecialUpFunc(specialKeyUpFunc);
                glutTimerFunc(INTERFACE_UPDATETIME, timerFunc, 0);
                init();
                //boost::thread t(boost::bind(&labust::gui::AAGui::start,gui.get()));
                gui->start();
                //glutMainLoop();

                //ros::spin();

                //usleep(1000*1000);
        }
        catch(std::exception &e)
        {
                std::cout<<e.what()<<std::endl;
        }
        return 0;
}