/* 
 * File:   main.cpp
 * Author: Alexandre Kaspar
 *
 * Created on 9. janvier 2012, 14:34
 */
 
#include <cstdlib>
#include <iostream>
#include <cmath>
// OpenCV
#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
//// GL and GLU
//#include <GL/gl.h>
//#include <GL/glu.h>
//#include <GL/freeglut.h>
// our library
#include <Capture.h>
#include <Frame.h>
#include <Geometry.h>
#include <Marker.h>
#include <algorithm>
 
// namespaces
 
using namespace cv;
using namespace std;
 
// global variables
bool smooth = false; //< whether to smooth the camera image
bool game = false; //< whether the game is running
bool paused = false; //< running status
 
int getFilterFlag() {
    int flag = Capture::FLIP;
    if (smooth) {
        flag |= Capture::GAUSS_SMOOTH;
    }
    return flag;
}
 
/**
 * Mouse event handler
 * 
 * @param event the mouse event
 * @param x the new mouse abscissa
 * @param y the new mouse ordinate
 */
void onMouse(int event, int x, int y, int, void*) {
    // the mouse event
    switch (event) {
        case CV_EVENT_LBUTTONUP:
            // we restart the game
            game = true;
            break;
    }
}
 
/**
 * Process a keyboard event
 * 
 * @param histimg the histogram image
 * @return whether to keep running or not (then exit)
 */
bool pollKeys() {
    char c = (char) waitKey(10);
    if (c == 27) {
        // we're done, let's exit
        return false;
    }
    // possible key events
    switch (c) {
        case 's':
        case 'g':
            smooth = !smooth;
            break;
        case 'p':
            paused = !paused;
            break;
        default:
            ;
    }
    return true;
}
 
typedef Point2d PVect;
 
PVect computeSpeed(PVect v, double s) {
    // v has values in [-1;+1]
    double angle = atan2(v.y, v.x);
    return PVect(s * cos(angle), s * sin(angle));
}
 
PVect processSpeed(PVect v, double dy) {
    double Y = v.y + dy;
    double norm = sqrt(v.x * v.x + Y * Y);
    return PVect(-v.x / norm, Y / norm); // -x for the x-bounce
}
 
void drawMargin(Mat& image, int x) {
    line(image, Point(x, 0), Point(x, image.rows),
            Scalar(0xFF, 0xFF, 0xFF), 2, CV_AA);
}
 
/*
 * Boot method
 */
int main(int argc, char** argv) {
 
    cout << "Init [game]..." << endl;
 
    geom::initTimestamp();
 
    Capture cap;
 
    cout << ". init markers" << endl;
    MarkerRef leftMarker = Marker::load("markers/y1.m");
    Scalar leftColor(0x00, 0xFF, 0x00);
    int lastLeftPos = 0;
    MarkerRef rightMarker = Marker::load("markers/r1.m_patch.png");
    Scalar rightColor(0x00, 0x00, 0xFF);
    int lastRightPos = 0;
 
    if (leftMarker && rightMarker) {
        // we're ok
        cout << ". marker loaded !" << endl;
    } else {
        cerr << "Couldn't load the markers..." << endl;
        return 1;
    }
 
    // start camera capture
    cap.open(0);
 
    if (!cap.isOpened()) {
        cerr << "***Could not initialize capturing...***\n";
        return -1;
    }
 
    const string window = "Marker Ping PONG !";
 
    namedWindow(window, 0);
    setMouseCallback(window, onMouse, 0);
 
    // game parameters
    const int margin = 50; //< pixel x-margin
    const int pingsz = 100; //< ping pong p
    const int rectsz = 20; //< ball radius
    const int speed = 30; //< ball velocity
    PVect recPos; //< the ball position
    PVect recSpeed; //< the ball speed
    int win = 0; //< the winner 1=left, 2=right
 
    cout << ". starting the loop" << endl << endl;
    cout << "Click with the mouse to restart the game." << endl << endl;
 
    // the loop
    Frame frame;
    Mat image;
    for (;;) {
 
        if (!paused) {
            // we update the filter flags
            cap(getFilterFlag());
            // we update the frame from the video capture
            cap >> frame;
            // and check that there's something
            // otherwise => we stopped using the camera
            if (frame.empty())
                break;
        }
 
        // stage constants
        const int width = frame.cols();
        const int height = frame.rows();
 
        // the image we'll draw onto
        frame(Channel::BGR).copyTo(image);
 
        // ### 1 = detect the marker => update positions
 
        // the left player
        if (leftMarker->detect(frame)) {
            point target = leftMarker->lastPoint();
 
            // we display the marker location
            rectangle(image,
                    Rect(target.x - 10, target.y - 10, 20, 20),
                    leftColor);
 
            // we set the new pos
            lastLeftPos = target.y;
        }
        // the right player
        if (rightMarker->detect(frame)) {
            point target = rightMarker->lastPoint();
 
            // we display the marker location
            rectangle(image,
                    Rect(target.x - 10, target.y - 10, 20, 20),
                    rightColor);
 
            // we set the new pos
            lastRightPos = target.y;
        }
 
        // ### 2 = draw the players
        drawMargin(image, margin);
        if (leftMarker->getHistorySize() > 0) {
            // the display range
            Rect pad(0, std::max(0, lastLeftPos - pingsz), margin, 2 * pingsz);
            rectangle(image, pad, leftColor, CV_FILLED);
        }
        drawMargin(image, width - margin);
        if (rightMarker->getHistorySize() > 0) {
            // the display range
            Rect pad(width - margin, std::max(0, lastRightPos - pingsz),
                    margin, 2 * pingsz);
            rectangle(image, pad, rightColor, CV_FILLED);
        }
 
        // ### 3 = game logic
        if (!game) {
            // we put the ball at the center
            recPos = PVect(width / 2, height / 2);
            string title;
            Scalar color;
            switch (win) {
                case 0:
                    title = "Click to start";
                    color = Scalar(0x66, 0x66, 0x66);
                    break;
                case 1: // left won
                    title = "Left won !";
                    color = leftColor;
                    break;
                case 2: // right won
                    title = "Right won !";
                    color = rightColor;
                    break;
            }
            // we display the title
            Size textsize = getTextSize(title, CV_FONT_HERSHEY_COMPLEX, 2, 5, 0);
            Point org((width - textsize.width) / 2, 3 * height / 4);
            putText(image, title, org, CV_FONT_HERSHEY_COMPLEX, 2, color, 5, CV_AA);
        }
        // we draw the ball anyway
        circle(image, recPos, rectsz, Scalar(0, 0, 0), CV_FILLED, CV_AA);
 
        // the game in itself is simple
        if (game) {
            // the real thing happens here
            // 0=check won or not
            if (recPos.x < rectsz) {
                // left lost !
                win = 2;
                game = false;
            } else if (width - recPos.x < rectsz) {
                // right lost !
                win = 1;
                game = false;
            } else {
                // we compute the ball displacement
                if (recSpeed.x == 0) {
                    // we reset the direction
                    recSpeed = PVect(1.0, 0.0);
                }
 
                // the new position
                PVect v = computeSpeed(recSpeed, speed);
                recPos = recPos + v;
 
                // computation of the new speed
                PVect nextPos = recPos + v;
                if (nextPos.x < margin + rectsz) {
                    // check for x-bounce on left
                    if (abs(lastLeftPos - nextPos.y) < pingsz + 0.75 * rectsz) {
                        // we bounce !
                        // we use the history to check the pad "speed"
                        // orientation and put that in the ball new speed
                        point disp = leftMarker->lastDisplacement();
                        double orient = disp.y / double(height); // div by disp.t ?
                        orient *= 5.0;
                        // => unit vector of (0,orient) + recSpeed
                        recSpeed = processSpeed(recSpeed + PVect(-1.0, 0.0), orient);
                    }
                } else if (width - nextPos.x < margin + rectsz) {
                    // check for x-bounce on right
                    if (abs(lastRightPos - nextPos.y) < pingsz + 0.75 * rectsz) {
                        // we bounce !
                        point disp = rightMarker->lastDisplacement();
                        double orient = disp.y / double(height); // div by disp.t ?
                        orient *= 5.0;
                        recSpeed = processSpeed(recSpeed + PVect(1.0, 0.0), orient);
                    }
                }
                if (nextPos.y < 0 || nextPos.y > height) {
                    // y-bounce
                    recSpeed = PVect(recSpeed.x, -recSpeed.y);
                }
            }
        }
 
        // ### END = display
        imshow(window, image);
        setMouseCallback(window, onMouse, 0);
 
        // check keyboard events
        if (!pollKeys()) {
            break;
        }
    }
 
    // all's done
    cout << "Done capturing." << endl;
    cout << "Bye." << endl;
    return 0;
}