#include "app.h"


App::App(Stomp* st) {
    st_ = st;
    st_->connectRequest("sdi.hevs.ch", 61614, "/", "sdi10", "809c02f36becb0868da98761fe3209f6");
    st_->subscribeRequest("/topic/sdi10.gem.field", 1);

}

void App::connectConfirmation(bool success, QString version) {

}

void App::sendConfirmation(bool success) {

}

void App::subscribeConfirmation(bool success) {
    st_->sendRequest("/topic/sdi10.gem.command", "up");
}

void App::receiveIndication(int id, QString destination, QString body) {
    //qDebug() << "Indication " << id << " : " << destination << Qt::endl << body << Qt::endl;
    if(destination.contains("field")){
        fillField(body);
    }
    computeMove();
}

void App::disconnectConfirmation() {

}

void App::disconnectIndication() {

}

void App::addGem(int x, int y, int pts) {
    Gem g;
    g.coordinate = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
    g.pts = pts;
    gems_.append(g);
}

void App::fillField(QString body) {
    int x = 0;
    int y = 0;
    gems_.clear();
    for(int i = 0; i<body.length(); i++) {
        const QChar c = body.at(i);
        if(c == 'Y') {
            myVehicle_ = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
        }
        if(c == 'g') addGem(x, y, 100);
        if(c == 'G') addGem(x, y, 250);
        if(c == 'D') addGem(x, y, 500);
        if(c == '\n') {
            y++;
            x = 0;
        } else {
            x++;
        }
    }
}

Vector2D App::computeRelativePts(Gem g) {
    Vector2D v = g.coordinate-myVehicle_;
    v.reverse();
    v.normalize(MAX_LENGHT);

    return Vector2D(v.lenght()*g.pts*v.lenght(), v.angle());
}

void App::computeMove() {
    static const double PI = 3.14159265358979323846;
    Vector2D myRelativePts;
    for(Gem g : gems_) {
        myRelativePts = myRelativePts+computeRelativePts(g);
    }
    double angle = myRelativePts.angle();

    if(angle <0) angle = angle+2*PI;
    angle *= 180/PI;

    qDebug() << "Angle: " << angle << "°";
    qDebug() << "Length: " << myRelativePts.lenght() << Qt::endl;

    if(angle > 360) {
        qDebug() << "Error angle: " << angle << Qt::endl;
    }
    else if(angle <= -360) {
        qDebug() << "Error angle: " << angle << Qt::endl;
    }

    else if(angle > 315) { // angle > 315°
        st_->sendRequest("/topic/sdi10.gem.command", "right");
    }
    else if(angle <= -315) { // angle <= -315° || angle <= 45°
        st_->sendRequest("/topic/sdi10.gem.command", "right");
    }

    else if(angle > 225) { // angle > 225°
        st_->sendRequest("/topic/sdi10.gem.command", "down");
    }
    else if(angle <= -225) { // angle <= -225° || angle <= 135°
        st_->sendRequest("/topic/sdi10.gem.command", "up");
    }

    else if(angle > 135) { // angle > 135°
        st_->sendRequest("/topic/sdi10.gem.command", "left");
    }
    else if(angle <= -135) { // angle <= -135° || angle <= 225°
        st_->sendRequest("/topic/sdi10.gem.command", "left");
    }

    else if(angle > 45) { // angle > 45°
        st_->sendRequest("/topic/sdi10.gem.command", "up");
    }
    else if(angle <= -45) { // angle <= -45° || angle <= 315°
        st_->sendRequest("/topic/sdi10.gem.command", "down");
    }

    else {
        st_->sendRequest("/topic/sdi10.gem.command", "right");
    }
}