// g++ -std=c++11 -Wall -pthread -ggdb -o ihm -lbluetooth  -lGL -lGLU -lglut ihm.cpp && ./ihm

#include <cstdlib>
#include <GL/glut.h>
#include <math.h>
#include <pthread.h>
#include <time.h>
#include <errno.h>

#include "master.hpp"

using namespace std;

GLsizei width, height;
float realPosX = -100, realPosY = -100;
float targetPosX = -100, targetPosY = -100;

CommMaster m("/dev/ttyUSB0", 80, 10);

const float frameOutsideWidthHeightMm = 198;
const float frameInsideWidthHeightMm = 165;
const float windowSize = 640;
const float pixelPerMm = windowSize / frameOutsideWidthHeightMm;
const float stepPerMm = 5;
const float frameWidth = (frameOutsideWidthHeightMm-frameInsideWidthHeightMm)/2*pixelPerMm;
const float frameRadius = 40*pixelPerMm;

void circle(float x, float y, float r, int segments)
{
    glBegin( GL_TRIANGLE_FAN );
        glVertex2f(x, y);
        for( int n = 0; n <= segments; ++n ) {
            float const t = 2*M_PI*(float)n/(float)segments;
            glVertex2f(x + sin(t)*r, y + cos(t)*r);
        }
    glEnd();
}

void rectangle(float x1, float y1, float x2, float y2)
{
    glBegin(GL_QUADS);
        glVertex2f(x1,y1);
        glVertex2f(x1,y2);
        glVertex2f(x2,y2);
        glVertex2f(x2,y1);
    glEnd();
}

void display()
{
    glClear(GL_COLOR_BUFFER_BIT); // clear window

    glColor3ub(0xff, 0xcf, 0x3b); // orange
    circle(frameRadius, frameRadius, frameRadius, 200);
    circle(frameRadius, height-frameRadius, frameRadius, 200);
    circle(width-frameRadius, frameRadius, frameRadius, 200);
    circle(width-frameRadius, height-frameRadius, frameRadius, 200);
    rectangle(frameRadius, 0, width-frameRadius, frameWidth);
    rectangle(frameRadius, height-frameWidth, width-frameRadius, height);
    rectangle(0, frameRadius, frameWidth, height-frameRadius);
    rectangle(width-frameWidth, frameRadius, width, height-frameRadius);
    glColor3ub(0, 0, 0); // black
    circle(2*frameWidth, 2*frameWidth, frameWidth, 200);
    circle(2*frameWidth, height-2*frameWidth, frameWidth, 200);
    circle(width-2*frameWidth, 2*frameWidth, frameWidth, 200);
    circle(width-2*frameWidth, height-2*frameWidth, frameWidth, 200);
    rectangle(2*frameWidth, frameWidth, width-2*frameWidth, height-frameWidth);
    rectangle(frameWidth, 2*frameWidth, width-frameWidth, height-2*frameWidth);
        
    glColor3ub(0xad, 0x05, 0xd7); // purple
    circle(realPosX, realPosY, 25, 80);

    glColor3ub(0x00, 0x00, 0xff); // blue
    circle(targetPosX, targetPosY, 10, 40);

    glutSwapBuffers(); // display newly drawn image in window
}

void reshape( int w, int h )
{
    glViewport( 0, 0, (GLsizei)w, (GLsizei)h ); // set to size of window
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();

    gluOrtho2D( 0.0, (float)w, 0.0, (float)h );
    //glOrtho( 0.0, (float)w, (float)h, 0.0, 0.0, 1.0 );

    width = w;  // records width globally
    height = h; // records height globally

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
}
#include <stdio.h>
void mouse(int button, int state, int x, int y)
{
    switch(button)
    {
    case GLUT_LEFT_BUTTON:

        if(state==GLUT_DOWN)
        {
            float posStepsX = ((float)x-frameWidth)/pixelPerMm*stepPerMm;
            float posStepsY = ((float)(height-y)-frameWidth)/pixelPerMm*stepPerMm;
            //printf("Mouse down at (%d,%d)\n", (int)posStepsX, (int)posStepsY);
            targetPosX = x;
            targetPosY = height-y;
            m.gotoXY((Comm::PosType)posStepsX,(Comm::PosType)posStepsY);
            if (!m.ack()) printf("Error in move command\n");
        }
        if(state==GLUT_UP)
        {
            //printf("Mouse up on (%d,%d)\n", x, height-y);
        }
        glutPostRedisplay();
        break;
    }
}

void motion( int x, int y )
{
    glutPostRedisplay();
}

void refresh(int)
{
    realPosX = ((float)m.info.posX)/stepPerMm*pixelPerMm+frameWidth;
    realPosY = ((float)m.info.posY)/stepPerMm*pixelPerMm+frameWidth;
    //printf("%f, %f\n", realPosX, realPosY);
    glutPostRedisplay();
    glutTimerFunc(50, refresh, 0);
}

void setup()
{
    glClearColor(0.0, 0.0, 0.0, 1.0); // *should* display black background
}

int main(int argc, char** argv)
{
    glutInit(&argc,argv);
    glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGB|GLUT_MULTISAMPLE);
    glutInitWindowSize(windowSize,windowSize);
    glutInitWindowPosition(100,100);
    glutCreateWindow("");

    setup();

    m.setPos(0, 0);
    if (!m.ack()) printf("Error setting position\n");

    // initializing callbacks
    glutReshapeFunc(reshape);
    glutDisplayFunc(display);
    glutMouseFunc(mouse);
    //glutMotionFunc(motion);

    glutTimerFunc(150, refresh, 0);
    glutMainLoop();
    return 0;
}

