const ROBOT_URL = "192.168.185.166";
const CURL_URL = "localhost:4444";
const INFO_POSITION = 0x01;
const INFO_LED = 0x02;

const DEBUG = false;

let socketMoteur = undefined;
let socketInfos = undefined;
let moteur = undefined;
let led = undefined;
let position = undefined;
let pid = undefined;
let turtle = undefined;


const connectSockets = () => {
    if ((socketMoteur != undefined) || (socketInfos != undefined)) {
        disconnectSocket();
    }
    socketMoteur = new WebSocket("ws://" + ROBOT_URL + "/motors.ws");
    socketInfos  = new WebSocket("ws://" + ROBOT_URL + "/infos.ws");
    socketInfos.binaryType = "arraybuffer";

    socketMoteur.onclose = function(event) {
        if (event.wasClean) {
            console.log(`[socketMoteur] Connection closed cleanly, code=${event.code} reason=${event.reason}`);
        } else {
            console.log('[socketMoteur] Connection died');
        }
    };

    socketMoteur.onopen = function(e) {
        console.log("[socketMoteur] Connection established");
    };

    socketInfos.onopen = function(e) {
        console.log("[socketInfos] Connection established");
        position.reset();
        led.green();
        new Promise(r => setTimeout(r, 2000)).then(() => {
            led.off();
        });
    };

    socketInfos.onclose = function(event) {
        if (event.wasClean) {
            console.log(`[socketInfos] Connection closed cleanly, code=${event.code} reason=${event.reason}`);
        } else {
            console.log('[socketInfos] Connection died');
        }
    };

    socketMoteur.onerror = function(error) {
        console.log(`[socketMoteur] ERROR : ${error}`);
    };

    socketInfos.onerror = function(error) {
        console.log(`[socketInfos] ERROR : ${error}`);
    };
}

const disconnectSocket = () => {
    if (socketMoteur != undefined) {
        socketMoteur.close();
    }
    if (socketInfos != undefined) {
        socketInfos.close();
    }

    socketMoteur = undefined;
    socketInfos = undefined;
}




class Moteur {

    motors_buf = new Float32Array(2);

    send(d, g, v) {
        if (socketMoteur == undefined) {
            return;
        }
        this.motors_buf[0] = d * v;
        this.motors_buf[1] = g * v;
        
        socketMoteur.send(this.motors_buf);
    }

    avancer(v) {
        this.send(1, 1, v);
    }

    reculer(v) {
        this.send(-1, -1, v);
    }

    droite(v) {
        this.send(1, -1, v);
    }

    gauche(v) {
        this.send(-1, 1, v);
    }

    stop() {
        this.send(0, 0, 0);
    }
}

class PID {

    kpx = 0;
    kix = 0;
    kdx = 0;

    kpy = 0;
    kiy = 0;
    kdy = 0;

    kpr = 0;
    kir = 0;
    kdr = 0;

    px = 0;
    ix = 0;
    dx = 0;

    py = 0;
    iy = 0;
    dy = 0;

    pr = 0;
    ir = 0;
    dr = 0;

    tagetX = 0;
    tagetY = 0;
    targetR = 0;
    run = false;

    lastTime = undefined;

    set(targetX, targetY, targetR) {
        this.targetX = targetX;
        this.px = 0;
        this.ix = 0;
        this.dx = 0;

        this.targetY = targetY;
        this.py = 0;
        this.iy = 0;
        this.dy = 0;

        this.targetR = targetR;
        this.pr = 0;
        this.ir = 0;
        this.dr = 0;

        this.lastTime = Date.now().valueOf();
        this.run = true;
    }
    

    get(currentX, currentY, currentR) {
        if (!this.run) {
            return;
        }
        const currentTime = Date.now().valueOf();
        const deltaTime = (currentTime - this.lastTime) / 1000.0;

        this.p


        console.log("Time delta : " + deltaTime);
        this.lastTime = currentTime;
    }

}

class Led {

    http = undefined;

    async send(color){
        await fetch('http://' + ROBOT_URL + '/led/set_color', {
            method: 'post',
            headers: { "Content-Type": "application/x-www-form-urlencoded" },
            mode: "no-cors",
            body: 'ledcolor=' + color
        });
    }

    red(){
        this.send("#ff0000");
    }

    green(){
        this.send("#00ff00");
    }

    blue(){
        this.send("#0000ff");
    }

    off(){
        this.send("#000000");
    }

}

class Position {

    posX = 0;
    posY = 0;
    Rot = 0;

    constructor(socket) {
        socket.addEventListener("message", e => {
            const view = new DataView(e.data);
            let pos = 0;
        
            const mask = view.getUint8(0);
        
            if (mask & INFO_POSITION) {
                const posX = view.getFloat32(1);
                const poxY = view.getFloat32(5);
                const posR = view.getFloat32(9);
                
                if (DEBUG) {
                    console.log(
                        {"posX": posX, "posY": poxY, "Rot": posR}
                    );
                }

                pid.get(posX, this.posY, posR);
            }
        });
    }

    async reset() {
        await fetch("http://" + ROBOT_URL + "/position/reset", {
            method: 'get',
            headers: { "Content-Type": "application/x-www-form-urlencoded" },
            mode: "no-cors"
        });
    }
}

class Turtle {

    async avancer(distance) {
        const data = {
            'type': 'dist',
            'dist': distance,
        }
        await fetch("http://" + CURL_URL, {
            method: "POST",
            mode: "no-cors",
            body: JSON.stringify(data),
        });
    }

    async tourner(angle) {
        const data = {
            'type': 'angle',
            'angle': angle,
        }
        await fetch("http://" + CURL_URL, {
            method: "POST",
            mode: "no-cors",
            body: JSON.stringify(data),
        });


    }

    

}

const Orientation = Object.freeze({
    HAUT: 0,
    DROITE: 1,
    BAS: 2,
    GAUCHE: 3,
});


class Noeud {
    coordX;
    coordY;
    chemins;

    constructor(coordX, coordY){
        this.coordX = coordX;
        this.coordY = coordY;
        chemins = new Array();
    }

    addCheminPossible(chemin){
        this.chemins.push(chemin);
    }

    getCheminsPossibles(){
        return this.chemins;
    }

    toString(){
        console.log("node : (" + currentNode.getX + "; " + currentNode.getY + ")");
    }

    toStringWithVoisins(){
        console.log("node : (" + currentNode.getX + "; " + currentNode.getY + ")");
        console.log("voisins connus : " + this.chemins.forEach((chemin) => chemin.toString()));
    }
}


class ModeAutomatique {

    cheminsPossibles = new Array(); // liste des listes de noeuds
    noeudsVisites = new Array();

    // valeurs pour la première position
    posX=0;
    posY=0;
    rotation = Orientation.HAUT;
    lastRotation = this.rotation;
    
    run(){
        let iter = 0;
        while(!this.isObjectifTrouve()){
            console.log("Iteration " + iter++);
            console.log(this.cheminsPossibles);
            const currentNode = this.getCaseAnalysee();
            this.analyseEnvironnement(currentNode); // met à jour ses connaissances (à dessiner au fur et à mesure ?)
            this.deplacer(currentNode); // récupère un chemin non encore parcourus en fonction de ses connaissances
            sleep(5000);
        }
    }

    isObjectifTrouve(){
        return false;
    }

    //////////////////////////////////////// ANALYSE ENVIRONNEMENT

    isObstacleDevant (currentNode, rotation) {
        return false;
    }

    analyseEnvironnement(currentNode){
        // SI case déjà visitée, alors on connait ses obstacles   
        if (currentNode == null) {

            // pour les 4 directions possibles
            for (let i = 0; i < 4; i++) {
                if (!this.isObstacleDevant(currentNode, this.rotation)){
                    this.addCheminPossible(currentNode);
                }
                this.tourne(); // changement de l'angle de 90 degrés
            }
            this.noeudsVisites.push(currentNode);
        }   
    }

    tourne(){
        turtle.tourner(-90);
        this.rotation++;
        this.rotation %= 4;
    }

    addCheminPossible(){
        this.chemin = new Array();
        if(this.rotation = Orientation.HAUT){
            this.chemin.push(new Noeud(posX+1, posY));
        } else if(this.rotation = Orientation.BAS){
            this.chemin.push(new Noeud(posX-1, posY));
        }else if(this.rotation = Orientation.GAUCHE){
            this.chemin.push(new Noeud(posX, posY-1));
        }else if(this.rotation = Orientation.DROITE){
            this.chemin.push(new Noeud(posX, posY+1));
        }
        this.cheminsPossibles.push(this.chemin);
    }

    //////////////////////////////////////// MOVE

    deplacer(currentNode){
        // on choisi le prochain noeud
        const nextNode = this.getNextNode();
        if (nextNode != null) {
            console.log("next node : ");
            nextNode.toString();
        }
        // on met à jour la liste des chemins en ajoutant le noeud courant dans les chemins non choisis
        this.updatePaths(nextNode);
        // on met à jour les coordonnées
        this.updateCoordonnees(currentNode, nextNode);
        // move
        this.move();
    }

    // parcours en profondeur : il s'agit de LIFOs
    getNextNode(){
        if (this.cheminsPossibles[this.cheminsPossibles.length - 1] > 0) {
            // on récupère la dernière liste
            const cheminChoisi = this.cheminsPossibles[this.cheminsPossibles.length - 1];
            // on récupère le dernier élément de cette liste
            const nodeChoisi = cheminChoisi[cheminChoisi.length - 1];
            console.log("noeud choisi :");
            nodeChoisi.toString();
            // on créé un nouveau noeud
            return new Noeud(nodeChoisi.posX, nodeChoisi.posY);
        }
        return null;
    }

    updatePaths(nextNode){
        nextNode = this.getCaseAnalysee();
        // on ajoute le noeud visité à toutes les listes sauf la dernière
        for (let i=0; i < this.cheminsPossibles -1; i++){
            const cheminPossible = this.cheminsPossibles[i];
            if (nextNode == null){
                // on ajoute le noeud en cours à la liste des noeuds visités
                cheminPossible.push(nextNode);
            } else {
                // on vire le noeud visité de toutes les listes s'il a déjà été visité avant (on fait demi tour)
                cheminPossible.pop();
            }
        }

        // on supprime les listes vides
        this.cheminsPossibles = this.cheminsPossibles.filter(function (el) {
            return el.length > 0;
         });        
         console.log("nombre de chemins restants : " + this.cheminsPossibles.length)
         console.log("chemins possibles : " + this.cheminsPossibles)
    }

    updateCoordonnees(currentNode, nextNode) {
        console.log("coordonnees : (" + this.posX + "; " + this.posY + ")");
        this.lastRotation = this.rotation;
        if (currentNode.getX > nextNode.getX){
            this.rotation = Orientation.GAUCHE
            this.posY--;
        } else if (currentNode.getX < nextNode.getX){
            this.rotation = Orientation.DROITE
            this.pos++;
        } else if (currentNode.getY > nextNode.getY){
            this.rotation = Orientation.BAS
            this.posX--;
        } else {
            this.rotation = Orientation.HAUT
            this.posX++;
        }
        console.log("new coordonnees : (" + this.posX + "; " + this.posY + ")");
    }

    move(){
        // déplacer robot (les coordonnées sont déjà à jour)
        let aRotater = this.rotation - this.lastRotation;
        if (aRotater > 2) {
            aRotater -= 2;
        }
        if (aRotater < 2) {
            aRotater += 2;
        }
        aRotater *= 90;
        turtle.tourner(aRotater);
        turtle.avancer(100);
    }

    /////////////////////////////////////// GETTERS

    getCaseAnalysee(){
        let index = 0;
        this.noeudsVisites.forEach((noeud) => {
            if (noeud == null) {
                console.error(this.noeudsVisites);
                console.error("INDEX NULL : " + index);
            }
            index++;
            if (noeud.posX == this.posX && noeud.posY == this.posY){
                return noeud;
            }
        });
        return null;
    }
}




async function run() {
    connectSockets();
    moteur = new Moteur();
    led = new Led();
    position = new Position(socketInfos);
    pid = new PID();
    turtle = new Turtle();
    led.red();


/*


    const socket  = new WebSocket("ws://127.0.0.1:1337/ws");
    socket.onclose = function(event) {
        if (event.wasClean) {
            console.log(`[socket] Connection closed cleanly, code=${event.code} reason=${event.reason}`);
        } else {
            console.log('[socket] Connection died');
        }
    };

    socket.onopen = function(e) {
        console.log("[socket] Connection established");
        socket.send("Test");
    };

    socket.onerror = function(error) {
        console.log(`[socket] ERROR : ${error}`);
    };
    
    socket.addEventListener("message", e => {
        const data = e.data;
        console.log(data);
    });

    */
    
}


async function test() {
    let d = 190;
    const data = {
		'type': 'dist',
		'dist': d,
	}
	await fetch("http://127.0.0.1:4444/", {
    method: "POST",
    mode: "no-cors",
    body: JSON.stringify(data),
  });
}