const ROBOT_URL = "192.168.185.166";
const CURL_URL = "localhost:4444";
const INFOS_POSITION = 0x01;
const INFOS_LED = 0x02;
const INFOS_MOTORS = 0x04;
const INFOS_WHEELS = 0x08;
const INFOS_SPEED = 0x10;
const INFOS_RANGEFINDER = 0x20;

const DEBUG = true;

const MUR_THRESHOLD = 80;
const MASTER_SLEEP_TIME = 500;

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

const sleep = async (time) => {
    await new Promise(r => setTimeout(r, time));
}

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.send(new Uint8Array([0x32, 0x20]));
    };

    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 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;
    capteur_distance = 0;
    distance_mur_moyenne = 0;

    constructor(socket) {
        socket.addEventListener("message", e => {
            const view = new DataView(e.data);
            let pos = 0;

            const mask = view.getUint8(pos);
            pos++;

            if (mask & INFOS_POSITION) {
                this.posX = view.getFloat32(pos + 0);
                this.posY = view.getFloat32(pos + 4);
                this.Rot = view.getFloat32(pos + 8);
                pos += 3*4;
                if (DEBUG) {
                    console.log(
                        {"posX": this.posX, "posY": this.posY, "Rot": this.Rot}
                    );
                }
            }

            if (mask & INFOS_LED) {
                const led_r = view.getUint8(pos + 0);
                const led_g = view.getUint8(pos + 1);
                const led_b = view.getUint8(pos + 2);
                pos += 3;
            }

            if (mask & INFOS_MOTORS) {
                const ml = view.getFloat32(pos + 0);
                const mr = view.getFloat32(pos + 4);
                pos += 2*4;
            }

            if (mask & INFOS_WHEELS) {
                const wl = view.getInt16(pos + 0);
                const wr = view.getInt16(pos + 2);
                const tl = view.getInt16(pos + 4);
                const tr = view.getInt16(pos + 6);
                pos += 4*2;
            }

            if (mask & INFOS_SPEED) {
                const w = view.getFloat32(pos + 0);
                const v = view.getFloat32(pos + 4);
                pos += 4*2;
            }

            if (mask & INFOS_RANGEFINDER) {
                this.capteur_distance = view.getUint16(pos + 0);
                this.distance_mur_moyenne = (this.distance_mur_moyenne*2 + this.capteur_distance)/3;
                pos += 2;
                if (DEBUG) {
                    console.log("Capteur distance : " + this.capteur_distance);
                    console.log("Distance moyenne : " + this.distance_mur_moyenne);
                }
            }
        });
    }

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

    getCapteurDistance() {
        return this.capteur_distance;
    }

    isMurDevant() {
        return this.distance_mur_moyenne < MUR_THRESHOLD;
    }
}

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),
        });
        await sleep(MASTER_SLEEP_TIME);
    }

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

    

}

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;
        this.chemins = new Array();
    }

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

    getCheminsPossibles(){
        return this.chemins;
    }

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

    toStringWithVoisins(){
        console.log("node : (" + this.getX + "; " + this.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;

    isFirstNode = true;
    
    async run(){
        led.blue();
        let iter = 0;
        while(!this.isObjectifTrouve()){
            if (iter >= 50)
                break;
            console.log("Iteration " + iter++);
            console.log(this.cheminsPossibles);
            let currentNode = this.getCaseAnalysee();
            currentNode = await this.analyseEnvironnement(currentNode); // met à jour ses connaissances (à dessiner au fur et à mesure ?)
            await this.deplacer(currentNode); // récupère un chemin non encore parcourus en fonction de ses connaissances
        }
        led.off();
    }

    isObjectifTrouve(){
        return false;
    }

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

    async isObstacleDevant () {
        //await turtle.avancer(20);
        await sleep(2000);
        const mur = position.isMurDevant();
        //await turtle.avancer(-20);
        return mur;
    }

    async analyseEnvironnement(currentNode){
        // SI case déjà visitée, alors on connait ses obstacles   
        if (currentNode == null) {
            console.log(">>>>>>>>> iteration ")
            currentNode = new Noeud(0, 0);

            // pour les 4 directions possibles
            for (let i = 0; i < 4; i++) {
                if (!await this.isObstacleDevant()){
                    this.addCheminPossible(currentNode);
                    console.log("chemins possibles : ");
                    console.log(this.cheminsPossibles);
                }
                await this.tourne(); // changement de l'angle de 90 degrés
                if (i == 1 && !this.isFirstNode) {
                    this.tourne(); // On scan pas notre pt d'entrée
                    this.isFirstNode = false;
                    i++;
                }
            }
            this.noeudsVisites.push(currentNode);
            console.log("noeuds visités : " + this.noeudsVisites)
        }   
        return currentNode;
    }

    async tourne(){
        await turtle.tourner(90);
        this.rotation--;
        if (this.rotation < 0) {
            this.rotation = 3;
        }
    }

    addCheminPossible(){
        const chemin = new Array();
        console.log("rotation : " + this.rotation);
        if(this.rotation == 0){
            console.log("rotation 0 ");
            chemin.push(new Noeud(this.posX, this.posY+1));
        } else if(this.rotation == 2){
            console.log("rotation 2 ");
            chemin.push(new Noeud(this.posX, this.posY-1));
        }else if(this.rotation == 3){
            console.log("rotation 3 ");
            chemin.push(new Noeud(this.posX-1, this.posY));
        }else if(this.rotation == 1){
            console.log("rotation 1");
            chemin.push(new Noeud(this.posX+1, this.posY));
        }
        console.log(chemin);
        this.cheminsPossibles.push(chemin);
        console.log(this.cheminsPossibles);
    }

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

    async 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
        await this.move();
    }

    // parcours en profondeur : il s'agit de LIFOs
    getNextNode(){
        if (this.cheminsPossibles.length > 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){
        // on ajoute le noeud visité à toutes les listes sauf la dernière
        for (let i=0; i < this.cheminsPossibles.length -1; i++){
            const cheminPossible = this.cheminsPossibles[i];

            if (this.getCaseAnalyseeWithCoord(nextNode.getX, nextNode.getY == 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 = 3
            this.posY--;
        } else if (currentNode.getX < nextNode.getX){
            this.rotation = 1
            this.pos++;
        } else if (currentNode.getY > nextNode.getY){
            this.rotation = 2
            this.posX--;
        } else {
            this.rotation = 0
            this.posX++;
        }
        console.log("new coordonnees : (" + this.posX + "; " + this.posY + ")");
    }

    async 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;
        }
        if (aRotater > 0) {
            aRotater = 4 - aRotater;
        }
        aRotater *= 90;
        turtle.tourner(aRotater);
        await turtle.avancer(190);
    }

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

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

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

let doRun = false;


async function isMurDevant() {
    turtle.avancer(20);
    await sleep(2000);
    const mur = position.isMurDevant();
    turtle.avancer(-20);

    return mur;
}

async function algo() {
    doRun = true;
    while(doRun) {
        
        await turtle.avancer(1500);
        await turtle.avancer(-20);
        await turtle.tourner(30);
        /*

        let mur = isMurDevant();
        if (!mur) {
            await turtle.avancer(75);
        }
        await turtle.tourner(90);
        mur = isMurDevant();
        if (mur) {
            await turtle.tourner(-90);
            await turtle.tourner(-90);
            await turtle.tourner(-90);
        }
            */
    }
}

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

    algo();



/*
    led.red();
    await sleep(400);
    
    led.green();
    await sleep(400);
    
    led.blue();
    await sleep(400);
    */
    
}