const URL = "192.168.185.166";
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;


const connectSockets = () => {
    if ((socketMoteur != undefined) || (socketInfos != undefined)) {
        disconnectSocket();
    }
    socketMoteur = new WebSocket("ws://" + URL + "/motors.ws");
    socketInfos  = new WebSocket("ws://" + 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();
    };

    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://' + 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://" + URL + "/position/reset", {
            method: 'get',
            headers: { "Content-Type": "application/x-www-form-urlencoded" },
            mode: "no-cors"
        });
    }
}


async function run() {


    connectSockets();
    moteur = new Moteur();
    led = new Led();
    position = new Position(socketInfos);
    pid = new PID();
    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);
    });

    */
    
}