#include "MyTCPClient.h"

MyTCPClient::MyTCPClient(const char *serverIP, int port) : TCPClient(serverIP, port), pca() {
    this->pwm_init();
}

void MyTCPClient::handleMessage(const std::string &message) {
    std::vector<std::string> token = Modelec::split(message, ";");

    if (token.size() != 4) {
        std::cerr << "Message invalide: " << message << std::endl;
        return;
    }

    if (token[1] == "servo_moteur" || token[1] == "all") {
        std::cout << message << std::endl;
        if (token[2] == "ping") {
            this->sendMessage("servo_moteur;ihm;pong;1");
        }
        else if (token[2] == "ouvrir pince") {
            int pince = std::stoi(token[3]);
            this->ouvrir_pince(pince);
        }
        else if (token[2] == "ouvrir total pince") {
            int pince = std::stoi(token[3]);
            this->ouvrir_total_pince(pince);
        }
        else if (token[2] == "fermer pince") {
            int pince = std::stoi(token[3]);
            this->fermer_pince(pince);
        }
        else if (token[2] == "middle pince") {
            int pince = std::stoi(token[3]);
            this->middle_pince(pince);
        }
        else if (token[2] == "baisser bras") {
            this->fermer_pince(0);
            this->fermer_pince(2);
            this->baisser_bras();
        }
        else if(token[2] == "transport bras"){
            this->fermer_pince(0);
	        this->fermer_pince(2);
	        this->transport_bras();
        } 
        else if (token[2] == "lever bras") {
            this->fermer_pince(0);
            this->fermer_pince(2);
            this->lever_bras();
        }
        else if (token[2] == "check panneau") {
            int bras = std::stoi(token[3]);
            this->check_panneau(bras);
        }
        else if (token[2] == "uncheck panneau") {
            int bras = std::stoi(token[3]);
            this->uncheck_panneau(bras);
        }
        else if (token[2] == "panneau") {
            std::vector<std::string> args = Modelec::split(token[3], ",");

            int bras = std::stoi(args[0]);
            int pourcentage = std::stoi(args[1]);

            this->percentage_panneau(bras, 100 - pourcentage);
        }
        else if (token[2] == "angle") {
            std::vector<std::string> args = Modelec::split(token[3], ",");
            if (args.size() != 2) {
                std::cerr << "Nombre d'arguments invalide" << std::endl;
                return;
            }
            int servo = std::stoi(args[0]);
            int angleDeg = static_cast<int>(std::stof(args[1]));
            this->pwm_setServoPosition(servo, angleDeg);
        } else if (token[2] == "clear") {
            this->pwm_clear();
        }
    }
}

void MyTCPClient::pwm_setFrequency(float freq) {
    pca.set_pwm_freq(freq);
}

void MyTCPClient::pwm_init() {
    pwm_setFrequency(50.0);
    this->pwm_clear();
    this->fermer_pince(0, true);
    this->fermer_pince(1, true);
    this->fermer_pince(2, true);
    this->pwm_clear();
}


void MyTCPClient::pwm_setServoPosition(int servo, int position) {
    int on_time = SERVO_MIN + (SERVO_MAX - SERVO_MIN) * position / 180 - 1;//temps ou le servo est allumé par rapport à 4096
    pca.set_pwm(servo, 0, on_time);
}

void MyTCPClient::baisser_bras() {
    int angleDebut;
    switch(this->positionBras){
        case BRAS_HAUT:
            angleDebut = angleBrasHaut.servo4;
            break;
        case BRAS_TRANSPORT:
            angleDebut = angleBrasTransport.servo4;
            break;
        case BRAS_BAS:
            this->pwm_setServoPosition(4, angleBrasBas.servo4);
            this->pwm_setServoPosition(5, angleBrasBas.servo5);
            return;
    }
    for(int i = angleDebut; i <= angleBrasBas.servo4;i++){
        this->pwm_setServoPosition(4, i);
        usleep(5'000);
        this->pwm_setServoPosition(5, 180-i);
    }
    this->positionBras = BRAS_BAS;
    this->pwm_setServoPosition(4, angleBrasBas.servo4);
    this->pwm_setServoPosition(5, angleBrasBas.servo5);
}

void MyTCPClient::transport_bras(){
    switch(this->positionBras){
        case BRAS_BAS:
            for (int i = angleBrasBas.servo5; i <= angleBrasTransport.servo5; i++){
                this->pwm_setServoPosition(4, 180-i);
                usleep(5'000);
                this->pwm_setServoPosition(5, i);
            }
            break;
        case BRAS_HAUT:
            for (int i = angleBrasHaut.servo4; i <= angleBrasTransport.servo4; i++){
                this->pwm_setServoPosition(4, i);
                usleep(5'000);
                this->pwm_setServoPosition(5, 180-i);
            }
            break;
        case BRAS_TRANSPORT:
            this->pwm_setServoPosition(4, angleBrasTransport.servo4);
            this->pwm_setServoPosition(5, angleBrasTransport.servo5);
            return;
    }
    this->positionBras = BRAS_TRANSPORT;
    this->pwm_setServoPosition(4, angleBrasTransport.servo4);
    this->pwm_setServoPosition(5, angleBrasTransport.servo5);
}

void MyTCPClient::lever_bras() {
    int angleDebut;
    switch(this->positionBras){
        case BRAS_BAS:
            angleDebut = angleBrasBas.servo5;
            break;
        case BRAS_TRANSPORT:
            angleDebut = angleBrasTransport.servo5;
            break;
        case BRAS_HAUT:
            this->pwm_setServoPosition(4, angleBrasHaut.servo4);
            this->pwm_setServoPosition(5, angleBrasHaut.servo5);
            return;
    }
    for (int i = angleDebut;i <= angleBrasHaut.servo5;i++){
        this->pwm_setServoPosition(4, 180-i);
        usleep(5'000);
        this->pwm_setServoPosition(5, i);
    }
    this->positionBras = BRAS_HAUT;
    this->pwm_setServoPosition(4, angleBrasHaut.servo4);
    this->pwm_setServoPosition(5, angleBrasHaut.servo5);
}

void MyTCPClient::fermer_pince(int pince, bool force) {
    anglePince pinceChoisie;
    int angleDebut;
    if ((etatPince[pince] == PINCE_FERMER && !force) || pince < 0 || pince > 2){
        return;
    }
    switch(pince){
        case 0:
            pinceChoisie = anglePince0;
            break;
        case 1:
            pinceChoisie = anglePince1;
            break;
        case 2:
            pinceChoisie = anglePince2;
            break;
    }
    switch (etatPince[pince]) {
        case PINCE_FERMER:
            this->pwm_setServoPosition(pince, pinceChoisie.fermer);
            return;
        case PINCE_MIDDLE:
            angleDebut = pinceChoisie.middle;
            break;
        case PINCE_OUVERTE:
            angleDebut = pinceChoisie.ouverte;
            break;
    }
    std::cout << "Fermer pince : " << pince << std::endl;
    for(int i = angleDebut; i <= pinceChoisie.fermer;i++){
        this->pwm_setServoPosition(pince, i);
        usleep(5'000);
    }
    etatPince[pince] = PINCE_FERMER;
}

void MyTCPClient::middle_pince(int pince, bool force){
    anglePince pinceChoisie;
    if ((etatPince[pince] == PINCE_MIDDLE && !force) || pince < 0 || pince > 2){
        return;
    }
    switch(pince){
        case 0:
            pinceChoisie = anglePince0;
            break;
        case 1:
            pinceChoisie = anglePince1;
            break;
        case 2:
            pinceChoisie = anglePince2;
            break;
    }
    std::cout << "Middle pince : " << pince << std::endl;
    switch (etatPince[pince]) {
        case PINCE_OUVERTE:
            for(int i = pinceChoisie.ouverte; i <= pinceChoisie.middle;i++){
                this->pwm_setServoPosition(pince, i);
                usleep(5'000);
            }
            break;
        case PINCE_MIDDLE:
            this->pwm_setServoPosition(pince, pinceChoisie.middle);
            return;
        case PINCE_FERMER:
            for (int i = pinceChoisie.fermer; i >= pinceChoisie.middle;i--){
                this->pwm_setServoPosition(pince, i);
                usleep(5'000);
            }
            break;
    }
    etatPince[pince] = PINCE_OUVERTE;
}

void MyTCPClient::ouvrir_pince(int pince, bool force) {
    std::cout << "Ouvrir pince : " << pince << std::endl;
    anglePince pinceChoisie;
    int angleDebut;
    if ((etatPince[pince] == PINCE_OUVERTE && !force) || pince < 0 || pince > 2){
        return;
    }
    switch(pince){
        case 0:
            pinceChoisie = anglePince0;
            break;
        case 1:
            pinceChoisie = anglePince1;
            break;
        case 2:
            pinceChoisie = anglePince2;
            break;
    }
    /*switch (etatPince[pince]) {
        case PINCE_FERMER:
            angleDebut = pinceChoisie.fermer;
            break;
        case PINCE_MIDDLE:
            angleDebut = pinceChoisie.middle;
            break;
        case PINCE_OUVERTE:
            this->pwm_setServoPosition(pince, pinceChoisie.ouverte);
            return;
    }
    std::cout << "Ouvrir pince : " << pince << std::endl;
    for (int i = angleDebut; i >= pinceChoisie.ouverte;i--){
        this->pwm_setServoPosition(pince, i);
        usleep(5'000);
    }*/
    this->pwm_setServoPosition(pince, pinceChoisie.ouverte);
    etatPince[pince] = PINCE_OUVERTE;
}

void MyTCPClient::ouvrir_total_pince(int pince, bool force) {
    std::cout << "Ouvrir total pince : " << pince << std::endl;
    anglePince pinceChoisie;
    switch(pince){
        case 0:
            pinceChoisie = anglePince0;
        break;
        case 1:
            pinceChoisie = anglePince1;
        break;
        case 2:
            pinceChoisie = anglePince2;
        break;
    }
    this->pwm_setServoPosition(pince, pinceChoisie.fullyOuverte);
    etatPince[pince] = PINCE_OUVERTE;
}

void MyTCPClient::check_panneau(int quelBras) {
    std::cout << "Check panneau : " << quelBras << std::endl;
    switch (quelBras) {
        case 6:
            this->pwm_setServoPosition(quelBras, anglePanneauGauche.check);
        break;
        case 7:
            this->pwm_setServoPosition(quelBras, anglePanneauDroit.check);
        break;
        default:
            break;
    }
}

void MyTCPClient::uncheck_panneau(int quelBras) {
    std::cout << "Uncheck panneau : " << quelBras << std::endl;
    switch (quelBras) {
        case 6:
            this->pwm_setServoPosition(quelBras, anglePanneauGauche.uncheck);
        break;
        case 7:
            this->pwm_setServoPosition(quelBras, anglePanneauDroit.uncheck);
        break;
        default:
            break;
    }
}

void MyTCPClient::percentage_panneau(int quelBras, int pourcentage) {
    switch (quelBras) {
        case 6:
            this->pwm_setServoPosition(quelBras, anglePanneauGauche.check + (anglePanneauGauche.uncheck - anglePanneauGauche.check) * pourcentage / 100);
        break;
        case 7:
            this->pwm_setServoPosition(quelBras, anglePanneauDroit.check + (anglePanneauDroit.uncheck - anglePanneauDroit.check) * pourcentage / 100);
        break;
        default:
            break;
    }
}


void MyTCPClient::pwm_clear() {
    pca.set_all_pwm(0,0);
}