import { SerialManager } from './serial.js';
import { ServoMotor, getModelType, reverseModelMap, writeData } from './feetechDefinitions.js';
import { CurveEditor } from './curveEditor.js';
import { Robot } from './robot.js';
import { NodeEditor } from './nodeeditor/NodeEditor.js';




window.onload = () => {



    const serial = new SerialManager();
    const servoMotors = [[], []]; // index 0 = channel 0, index 1 = channel 1
    const statusText = document.getElementById('statusText');
    const disconnectBtn = document.getElementById('disconnect');
    const connectBtn = document.getElementById('connect');
    const syncCheckbox = document.getElementById("syncCheckbox");
    const feebackCheckbox = document.getElementById("feebackCheckbox");
    const clearBtn = document.getElementById("clearAnimation");

    // Limits rate of move commands sent while sliding timeslider
    let lastSyncTime = 0; // global or outer-scope variable
    const syncIntervalMs = 20; // e.g. 100ms = max 10 times per second

    let isInterpolating = false;
    let currentFrame = 0;
    let dialKeyframes = Array.from({ length: 5 }, () => ({}));

    let selectedDial = null;
    let draggingKeyframe = null; // { dialIndex, originalFrame }
    let isDragging = false;



    const dialColors = [
        '#F50057', // Raspberry
        '#6200EA', // Deep Violet
        '#FFB400', // Bright Amber
        '#2979FF', // Royal Blue
        '#FF5252', // Coral Red
        '#00C853', // Vivid Green
        '#FF80AB', // Bubblegum
        '#00B8D4', // Sky Cyan
        '#FF9100', // Neon Orange
        '#651FFF', // Electric Indigo
        '#FF4F81', // Vibrant Pink
        '#AEEA00', // Chartreuse
        '#FFAB40', // Sunset Orange
        '#00E5FF', // Aqua
        '#FF6F00', // Vivid Orange
        '#64DD17', // Leaf Green
        '#FFEA00', // Vivid Yellow
        '#C51162', // Deep Rose
        '#40C4FF', // Sky Blue
        '#D500F9', // Vivid Purple
        '#76FF03', // Lime Green
        '#FF4081', // Hot Pink
        '#00B0FF', // Electric Blue
        '#FF1744', // Crimson
        '#B388FF', // Soft Violet
        '#1DE9B6', // Minty Teal
        '#AA00FF', // Vivid Lavender
        '#FFB347', // Pastel Orange
        '#00C1D4', // Bright Cyan
        '#7C4DFF'  // Neon Purple
    ];





    let dials = [];
    const frameSlider = document.getElementById('frameSlider');
    const frameDisplay = document.getElementById('frameDisplay');




    const curveCanvas = document.getElementById('curveCanvas');
    const curveEditor = new CurveEditor(curveCanvas, 10, frameSlider);


    // Animation File List
    const fileListElement = document.getElementById('fileList');
    const playButton = document.getElementById('playFile');
    const loadButton = document.getElementById('loadFile');
    const deleteButton = document.getElementById('deleteFile');

    let selectedFile = null;


    let connectedRobot = null;//GenerateTestRobot();



    const nodeCanvas = document.getElementById("nodeeditor");
    let nodeEditor = null;


    //nodeEditor.addServoNode(400, 150, "Servo Output", 5 );
    // nodeEditor.addInputNode(100, 500, "Input Nod", { defaultValue: 3 });
    // nodeEditor.addNoiseNode(400, 450); // Adds a random generator node at (200, 150)
    // nodeEditor.addVariableNode(300, 250);

    // nodeEditor.addNode(100, 100, "Time", { fill: "#e0f7e9", stroke: "#2e7d32" }); // mint green
    // nodeEditor.addNode(300, 200, "Output"); // uses default pastel

    function onConnectRobot(robot) {
        connectedRobot = robot;
        console.log(connectedRobot);
        let motorIDList = []
        clearDials();
        for (const motor of connectedRobot.motors) {
            curveEditor.addChannel(motor.ID);
            motorIDList.push(motor.ID);
            addDial(motor.ID, motor.NAME);
        }
        if (connectedRobot.motors.length > 0) {
            setSelectedMotor(connectedRobot.motors[0].ID);
        }


        nodeEditor = new NodeEditor(nodeCanvas, {
            motorIds: motorIDList
        });

        nodeEditor.generateDefaultNodes(curveEditor.curveSets, motorIDList);
    }


    function setSelectedMotor(motorID) {
        //console.log(motorID);
        curveEditor.selectMotor(motorID);
        selectedDial = motorID;

        const dialElements = document.querySelectorAll('.dial');

        dialElements.forEach((el, index) => {
            el.classList.remove('selected');

            if (dials[index]?.motorID === motorID) {
                el.classList.add('selected');
            }
        });

        //console.log("Selected motor:", motorID);
        // Any other logic you want to run


    }
    window.setSelectedMotor = setSelectedMotor;


    // TODO: Info should all be loaded on connect from handshake packet
    function GenerateTestRobot() {
        const robot = new Robot('Atlas', 'FW-2.0.1');

        // Create motors manually
        let testMotors = [];
        let positions = ["eyelids", "headtilt", "neckrotate", "rightshoulder", "rightforearm"];
        for (let i = 0; i < 5; i++) {
            let motor = new ServoMotor(0, 10 + i, "SCS009")
            robot.assignMotor(positions[i], motor);

            addDial(motor.ID);
        }
        // addDial(123);
        // Retrieve motor by position
        //console.log(robot.getMotor('eyelids'));


        return robot;
    }

    function clearFileList() {
        fileListElement.innerHTML = '';
        selectedFile = null;
        loadButton.disabled = true;
        deleteButton.disabled = true;
    }


    function addFileToList(filename) {
        const li = document.createElement('li');
        li.textContent = filename;

        li.addEventListener('click', () => {
            // Deselect previous
            const previouslySelected = fileListElement.querySelector('.selected');
            if (previouslySelected) previouslySelected.classList.remove('selected');

            // Select new
            li.classList.add('selected');
            selectedFile = filename;
            loadButton.disabled = false;
            deleteButton.disabled = false;
        });

        fileListElement.appendChild(li);
    }

    playButton.addEventListener('click', () => {
        if (!selectedFile) return;

        const filename = selectedFile;
        console.log("Sanitized filename for delete:", filename);

        const filenameBytes = new TextEncoder().encode(filename);
        const filenameLength = filenameBytes.length;

        // Total size: 2 bytes for length + filename bytes + oneshot/loop tag + loopCount
        const buffer = new ArrayBuffer(2 + filenameLength + 2);
        const view = new DataView(buffer);
        let offset = 0;

        // 🔹 Filename block
        view.setUint16(offset, filenameLength, true); offset += 2;
        filenameBytes.forEach(byte => view.setUint8(offset++, byte));

        const ONESHOT = 0x01; // play once
        const LOOP = 0x02;    // loop endlessly
        const REPEAT = 0x03;  // followed by loop count

        const repeatCount = parseInt(document.getElementById("repeatCount").value, 10);

        let playTag = REPEAT;
        view.setUint8(offset++, playTag);
        view.setUint8(offset++, repeatCount);

        const payload = new Uint8Array(buffer);
        //serial.deleteFile(payload); // CMD_DELETE_FILE

        serial.requestPlayFile(payload);
    });

    loadButton.addEventListener('click', () => {
        if (!selectedFile || loadButton.disabled) return;

        const filename = "/" + selectedFile;
        console.log(`Loading file: ${filename}`);

        // 🧷 Lock buttons
        loadButton.disabled = true;
        deleteButton.disabled = true;



        serial.requestFile(filename);
    });


    deleteButton.addEventListener('click', () => {
        if (selectedFile) {
            console.log(`Deleting file: ${selectedFile}`);
            sendDeleteToESP32();

            const selectedLi = fileListElement.querySelector('.selected');
            if (selectedLi) selectedLi.remove();
            selectedFile = null;
            loadButton.disabled = true;
            deleteButton.disabled = true;
        }
    });








    // Timeline

    frameSlider.oninput = () => {
        currentFrame = parseInt(frameSlider.value);
        frameDisplay.textContent = currentFrame;
        //console.log(currentFrame);

        syncDialsWithCurveEditor();
        syncMotorsWithTimeline();


    };

    function clearDials() {
        const dialArea = document.getElementById('dialArea');
        dialArea.innerHTML = ''; // Remove all child elements
        dials = [];
    }

    function addDial(motorID, motorName) {

        const index = dials.length;

        // Create dial wrapper
        const dialWrapper = document.createElement('div');
        dialWrapper.className = 'dial';
        dialWrapper.dataset.index = index;

        // Create label
        const label = document.createElement('label');
        label.textContent = "MotorID " + motorID;

        const label2 = document.createElement('label2');
        label2.textContent = motorName;

        // Create dial container
        const dialDiv = document.createElement('div');
        dialDiv.id = `dial${index}`;

        // Create value display
        const valueSpan = document.createElement('span');
        valueSpan.id = `value${index}`;
        valueSpan.textContent = '2048';

        // Assemble and append
        dialWrapper.appendChild(label);
        dialWrapper.appendChild(label2);
        dialWrapper.appendChild(dialDiv);
        dialWrapper.appendChild(valueSpan);
        document.getElementById('dialArea').appendChild(dialWrapper);

        // Create Nexus dial
        const dial = new Nexus.Dial(`#dial${index}`, {
            size: [80, 80],
            min: 0,
            max: 4095,
            value: 4095 / 2
        });

        dial.motorID = motorID;
        dial.colorize("accent", dialColors[index]);

        dial.on('change', (v) => {
            if (isInterpolating) return;
            const val = Math.round(v);
            document.getElementById(`value${index}`).textContent = val;
            //dialKeyframes[index][currentFrame] = val;

            syncMotorsWithTimeline();
        });

        dials.push(dial);
    }

    function syncDialsWithCurveEditor() {
        for (let ch = 0; ch < dials.length; ch++) {
            dials[ch].value = curveEditor.getMotorPositionAtTime(dials[ch].motorID, currentFrame);

        }

    }



    function syncMotorsWithTimeline() {

        const now = Date.now();
        if (syncCheckbox.checked && now - lastSyncTime >= syncIntervalMs) {
            lastSyncTime = now;

            const motorPayloads = [];

            for (let ch = 0; ch < dials.length; ch++) {
                const value = dials[ch].value;
                motorPayloads.push({ motorId: dials[ch].motorID, position: value });
            }

            const buffer = new ArrayBuffer(motorPayloads.length * 3);
            const view = new DataView(buffer);

            motorPayloads.forEach(({ motorId, position }, i) => {
                const offset = i * 3;
                view.setUint8(offset, motorId);
                view.setUint16(offset + 1, position, true); // little-endian
            });

            const payload = new Uint8Array(buffer);
            console.log("SENDING POSITIONTS");
            console.log(payload);
            serial.sendSetPositions(payload);
        }
    }


    document.querySelectorAll('.dial').forEach(el => {
        el.onclick = () => {
            const selectedDial = parseInt(el.dataset.index);

            setSelectedMotor(dials[selectedDial].motorID);
        };
    });



    // Connect button
    document.getElementById('connect').addEventListener('click', async () => {
        try {
            await serial.connect();
            statusText.textContent = 'Connected ✅';
            disconnectBtn.hidden = false;
            connectBtn.hidden = true;
            console.log("Serial connected");
            let text = "";
            serial.startReading((command, payload) => {
                switch (command) {
                    case 0x01: // ID response
                        text = new TextDecoder().decode(new Uint8Array(payload));
                        onConnectRobot(Robot.fromBytes(new Uint8Array(payload)));
                        console.log(connectedRobot);
                        document.getElementById('log').value += `ID Response: ${text}\n`;
                        break;

                    case 0x02: // File response
                        text = new TextDecoder().decode(new Uint8Array(payload));
                        const files = text.trim().split('\n');
                        document.getElementById('log').value += `File list Response: ${files}\n`;
                        clearFileList();

                        files.forEach(filename => {
                            if (filename && filename.toLowerCase().endsWith(".anim")) {
                                addFileToList(filename);
                            }
                        });

                        break;
                    case 0x03:  // CMD_LOAD_FILE
                        const fileData = new Uint8Array(payload);
                        console.log(`Received file (${fileData.length} bytes)`);
                        document.getElementById('log').value += `Loaded file\n`;
                        // 🔹 Do something with the file
                        handleLoadedFile(fileData);
                        break;

                    case 0x04:  // CMD_DELETE_FILE
                        console.log(`File deleted`);
                        console.log(new Uint8Array(payload));
                        document.getElementById('log').value += `File deleted\n`;
                        // 🔹 Do something with the file
                        //handleLoadedFile(fileData);
                        serial.requestFileList();
                        break;
                    case 0x05:  // CMD_SAVE_FILE

                        console.log(`Saved file Response Recieved`);
                        console.log(new Uint8Array(payload));
                        document.getElementById('log').value += `Saved file Response Recieved\n`;
                        // 🔹 Do something with the file
                        //handleLoadedFile(fileData);
                        serial.requestFileList();
                        break;

                    case 0x06:  // CMD_MESSAGE

                        //console.log(`Message Recieved`);
                        const decoder = new TextDecoder();
                        //console.log(payload);
                        const stringPayload = decoder.decode(new Uint8Array(payload));
                        //console.log(stringPayload);
                        document.getElementById('log').value += "MSG: " + stringPayload + `\n`;
                        const logBox = document.getElementById('log');
                        logBox.value += "MSG: " + stringPayload + `\n`;
                        logBox.scrollTop = logBox.scrollHeight;
                        // 🔹 Do something with the file
                        //handleLoadedFile(fileData);
                        break;

                    case 0x07:  // CMD_SET_POSITION
                        console.log(`Positions set`);
                        console.log(new Uint8Array(payload));
                        document.getElementById('log').value += `Positions set\n`;
                        // 🔹 Do something with the file
                        //handleLoadedFile(fileData);
                        break;

                    case 0x08: // CMD_PLAY_FILE
                        console.log(`Anim file played`);
                        console.log(new Uint8Array(payload));
                        document.getElementById('log').value += `Anim file played\n`;
                        break;

                    case 0x09: // CMD_SCAN_CHANNEL
                        console.log(new Uint8Array(payload));
                        handleScanChannelResponse(new Uint8Array(payload))
                        break;

                    case 0x10: // CMD_SCAN_CHANNEL
                        console.log(new Uint8Array(payload));
                        document.getElementById('log').value += `Data updated\n`;
                        //handleScanChannelResponse(new Uint8Array(payload))
                        break;

                    case 0x15: // POSITION STREAM
                        //console.log(new Uint8Array(payload));
                        handlePositionStreamPacket(payload);
                        break;

                    // Add more cases as needed
                    default:
                        document.getElementById('log').value += `Unknown command ${command}\n`;
                        break;
                }
            });


            // 🔹 Send ID request (CMD_ID_REQUEST = 0x01)
            await serial.requestIDPacket();

            await serial.requestFileList(); // or use a constant if defined



        } catch (err) {
            statusText.textContent = 'Connection failed ❌';
            console.error("Connection error:", err);
        }
    });

    function handlePositionStreamPacket(data) {

        //console.log(data);
        const motorCount = Math.floor(data.length / 2); // Each motor uses 2 bytes
        let d = [];
        for (let i = 0; i < motorCount; i++) {
            const high = data[i * 2];       // High byte
            const low = data[i * 2 + 1];    // Low byte
            const value = (low << 8) | high; // Combine into uint16_t
            d.push(value);
            if (dials[i]) {
                dials[i].value = value;
            }
        }

    }

    function handleLoadedFile(data) {
        const raw = new Uint8Array(data);
        const view = new DataView(raw.buffer);
        let offset = 0;

        // 🔹 Parse header
        const magic = String.fromCharCode(
            view.getUint8(offset),
            view.getUint8(offset + 1),
            view.getUint8(offset + 2),
            view.getUint8(offset + 3)
        );
        offset += 4;

        const frameCount = view.getUint16(offset, true); offset += 2;
        const version = view.getUint8(offset++);
        const frameRate = view.getUint8(offset++);
        offset += 8; // reserved

        if (magic !== "ANIM") {
            console.error("Invalid file format");
            return;
        }

        console.log("🧩 Magic:", magic);
        console.log("🎞️ Frame Count:", frameCount);
        console.log("📦 Version:", version);
        console.log("⏱️ Frame Rate:", frameRate);

        // 🔹 Read curve segment count
        if (offset + 2 > view.byteLength) {
            console.error("File too short to contain curve count");
            return;
        }

        const curveCount = view.getUint16(offset, true);
        offset += 2;


        const SEGMENT_SIZE = 17;
        const curveSets = {};
        let latestEndTime = 1;
        console.log(raw);

        for (let i = 0; i < curveCount; i++) {
            const motorID = view.getUint8(offset++);
            const startTime = view.getUint16(offset, true); offset += 2;
            const endTime = view.getUint16(offset, true); offset += 2;
            const startPointY = view.getInt16(offset, true); offset += 2;
            const startHandleX = view.getUint16(offset, true); offset += 2;
            const startHandleY = view.getInt16(offset, true); offset += 2;
            const endHandleX = view.getUint16(offset, true); offset += 2;
            const endHandleY = view.getInt16(offset, true); offset += 2;
            const endPointY = view.getInt16(offset, true); offset += 2;

            //console.log("RECEIVED VALUES RAW:");
            //console.log(startTime, endTime, startPointY, endPointY);

            const toFloat = v => (v / 65535) * 2 - 1;

            const curve = {
                startPoint: {
                    x: startTime,
                    y: curveEditor.exportRangeToY(startPointY)
                },
                startPointHandle: {
                    x: startHandleX,
                    y: curveEditor.exportRangeToY(startHandleY)
                },
                endPointHandle: {
                    x: endHandleX,
                    y: curveEditor.exportRangeToY(endHandleY)
                },
                endPoint: {
                    x: endTime,
                    y: curveEditor.exportRangeToY(endPointY)
                }
            };

            if (endTime > latestEndTime) {
                latestEndTime = endTime;
            }

            if (!curveSets[motorID]) {
                curveSets[motorID] = [];
            }
            curveSets[motorID].push(curve);
            console.log(motorID);
        }

        console.log("🎯 Loaded Curves:", curveSets);


        curveEditor.loadCurveSets(curveSets);
        curveEditor.setLength(latestEndTime);
        if (offset < view.byteLength) {
            const nodeGraphData = raw.slice(offset);  // grab remaining bytes
            nodeEditor.loadFromBinary(nodeGraphData); // call your editor's loader
        }

        // 🔓 Unlock buttons
        loadButton.disabled = false;
        deleteButton.disabled = false;

        document.getElementById("filenameInput").value = selectedFile.replace(/\.anim$/i, "");


    }




    function sanitizeFilename(input) {
        // Remove non-alphanumeric characters
        const stripped = input.replace(/[^a-zA-Z0-9]/g, "");

        // If nothing remains, fallback to default
        const safeName = stripped || "default";

        // Add leading slash and .anim extension
        return "/" + safeName + ".anim";
    }

    async function sendAnimationToESP32() {
        const rawInput = document.getElementById("filenameInput").value;
        const filename = sanitizeFilename(rawInput);
        console.log("Sanitized filename:", filename);

        const frameCount = curveEditor.timelineLength * curveEditor.pixelsPerSecond; // or whatever your timeline length is
        console.log(frameCount);
        const frameRate = 48;
        const version = 1;

        const headerSize = 16;

        const filenameBytes = new TextEncoder().encode(filename);
        const filenameLength = filenameBytes.length;

        const totalSize = 1024;
        const buffer = new ArrayBuffer(totalSize);
        const view = new DataView(buffer);
        let offset = 0;

        // 🔹 Filename block
        view.setUint16(offset, filenameLength, true); offset += 2;
        filenameBytes.forEach(byte => view.setUint8(offset++, byte));

        // 🔹 Header
        view.setUint8(offset++, "A".charCodeAt(0));
        view.setUint8(offset++, "N".charCodeAt(0));
        view.setUint8(offset++, "I".charCodeAt(0));
        view.setUint8(offset++, "M".charCodeAt(0));
        view.setUint16(offset, frameCount, true); offset += 2;
        view.setUint8(offset++, version);
        view.setUint8(offset++, frameRate);
        offset += 8; // reserved


        let curvePacket = curveEditor.encodeCurves()

        let nodeGraphPacket = nodeEditor.encodeNodeGraph();

        // 🔹 Append nodeGraphPacket


        // 🔹 Append curvePacket
        curvePacket.forEach(byte => view.setUint8(offset++, byte));
        nodeGraphPacket.forEach(byte => view.setUint8(offset++, byte));

        // 🔹 Send to ESP32
        const payload = new Uint8Array(buffer.slice(0, offset));
        console.log()
        console.log(payload);
        serial.saveFile(payload); // CMD_SAVE_ANIMATION
    }


    async function sendDeleteToESP32() {
        if (!selectedFile) return;

        const filename = selectedFile;
        console.log("Sanitized filename for delete:", filename);

        const filenameBytes = new TextEncoder().encode(filename);
        const filenameLength = filenameBytes.length;

        // Total size: 2 bytes for length + filename bytes
        const buffer = new ArrayBuffer(2 + filenameLength);
        const view = new DataView(buffer);
        let offset = 0;

        // 🔹 Filename block
        view.setUint16(offset, filenameLength, true); offset += 2;
        filenameBytes.forEach(byte => view.setUint8(offset++, byte));

        const payload = new Uint8Array(buffer);
        serial.deleteFile(payload); // CMD_DELETE_FILE
    }


    // Disconnect button
    disconnectBtn.addEventListener('click', () => {
        serial.disconnect();
        statusText.textContent = 'Disconnected';
        disconnectBtn.hidden = true;
        connectBtn.hidden = false;
        console.log("Serial disconnected");
    });


    clearBtn.addEventListener('click', () => {
        currentFrame = 0;
        dialKeyframes = Array.from({ length: 5 }, () => ({}));
    });


    document.getElementById('send').onclick = async () => {
        const text = document.getElementById('input').value + '\n';
        const encoder = new TextEncoder();
        await writer.write(encoder.encode(text));
        document.getElementById('input').value = '';
    };


    document.addEventListener('click', (e) => {
        const ignoredTags = ['BUTTON', 'INPUT', 'TEXTAREA', 'CANVAS'];
        const clickedInsideDial = e.target.closest('.dial');
        const clickedControl = ignoredTags.includes(e.target.tagName);

        if (!clickedInsideDial && !clickedControl && selectedDial !== null) {
            selectedDial = null;
            document.querySelectorAll('.dial').forEach(el => el.classList.remove('selected'));

        }

    });




    document.getElementById('saveAnimation').onclick = async () => {
        await sendAnimationToESP32();

    };



    document.getElementById('btn_apply_config_channel_0').onclick = async () => {
        const table = document.querySelector('#channel0-motor-table tbody');
        const rows = table.querySelectorAll('tr');
        const motors = [];

        rows.forEach(row => {
            const cells = row.querySelectorAll('td');
            const motor = {
                MODEL: reverseModelMap.get(cells[0].textContent.trim()),
                ID: parseInt(cells[1].textContent.trim()),
                // MIN_ANGLE_LIMIT: parseInt(cells[2].textContent.trim()),
                // MAX_ANGLE_LIMIT: parseInt(cells[3].textContent.trim()),
                // POSITION: parseInt(cells[4].textContent.trim()),
                // CW_DEAD_ZONE: parseInt(cells[5].textContent.trim()),
                // CCW_DEAD_ZONE: parseInt(cells[6].textContent.trim()),
                // OFFSET: parseInt(cells[7].textContent.trim()),
                // MODE: parseInt(cells[8].textContent.trim()),
                // TORQUE_ENABLE: parseInt(cells[9].textContent.trim()),
                // ACCELERATION: parseInt(cells[10].textContent.trim()),
                // GOAL_POSITION: parseInt(cells[11].textContent.trim()),
                // GOAL_TIME: parseInt(cells[12].textContent.trim()),
                // GOAL_SPEED: parseInt(cells[13].textContent.trim()),
                // LOCK: parseInt(cells[14].textContent.trim()),
                // CURRENT_SPEED: parseInt(cells[15].textContent.trim()),
                // CURRENT_LOAD: parseInt(cells[16].textContent.trim()),
                // TEMPERATURE: parseInt(cells[17].textContent.trim()),
                // MOVING: parseInt(cells[18].textContent.trim()),
                // CURRENT_CURRENT: parseInt(cells[19].textContent.trim()),
                // VOLTAGE: parseInt(cells[20].textContent.trim()),
                NAME: cells[21].textContent.trim()
            };

            motors.push(motor);
        });

        console.log("Compiled motor list:", motors);

        await serial.sendConfigUpdate(encodeMotorConfig(motors));
        // You can now use this list for saving, sending, or applying config
    };

    function encodeMotorConfig(motors) {
        const robotName = "Mr Roboto";
        const firmwareVersion = 1;

        const bufferSize = 1024; // adjust as needed
        const buffer = new ArrayBuffer(bufferSize);
        const view = new DataView(buffer);
        let offset = 0;

        const encoder = new TextEncoder();
        const nameBytes = encoder.encode(robotName);
        const nameLength = Math.min(nameBytes.length, 255); // max 255 bytes

        // 🔹 Encode robotName (length + bytes)
        view.setUint8(offset++, nameLength);
        for (let i = 0; i < nameLength; i++) {
            view.setUint8(offset++, nameBytes[i]);
        }

        // 🔹 Encode firmwareVersion (2 bytes)
        view.setUint16(offset, firmwareVersion, true); offset += 2;

        // 🔹 Encode motor count (1 byte)
        view.setUint8(offset++, motors.length);

        // 🔹 Encode motor entries
        motors.forEach(motor => {
            const { major, minor } = motor.MODEL;
            const modelValue = (minor << 8) | major;

            view.setUint16(offset, modelValue, true); offset += 2; // MODEL
            view.setUint16(offset, motor.ID, true); offset += 2;   // ID

            const motorNameBytes = encoder.encode(motor.NAME);
            const motorNameLength = Math.min(motorNameBytes.length, 255);

            view.setUint8(offset++, motorNameLength);
            for (let i = 0; i < motorNameLength; i++) {
                view.setUint8(offset++, motorNameBytes[i]);
            }
        });

        return new Uint8Array(buffer.slice(0, offset));
    }




    // MOTOR CONTROL PANEL

    function insertTableRow(motor) {
        const tableId = motor.CHANNEL === 0 ? 'channel0-motor-table' :
            motor.CHANNEL === 1 ? 'channel1-motor-table' : null;

        if (!tableId) {
            console.error('Invalid channel number. Use 0 or 1.');
            return;
        }

        const tbody = document.querySelector(`#${tableId} tbody`);
        const newRow = document.createElement('tr');
        newRow.setAttribute('data-row-id', motor.ID);

        const cells = [
            motor.MODEL,
            motor.ID,
            motor.MIN_ANGLE_LIMIT,
            motor.MAX_ANGLE_LIMIT,
            motor.POSITION,
            motor.CW_DEAD_ZONE,
            motor.CCW_DEAD_ZONE,
            motor.OFFSET,
            motor.MODE,
            motor.TORQUE_ENABLE,
            motor.ACCELERATION,
            motor.GOAL_POSITION,
            motor.GOAL_TIME,
            motor.GOAL_SPEED,
            motor.LOCK,
            motor.CURRENT_SPEED,
            motor.CURRENT_LOAD,
            motor.TEMPERATURE,
            motor.MOVING,
            motor.CURRENT_CURRENT,
            motor.VOLTAGE,
            motor.NAME
        ];

        const modelType = motor.MODEL.startsWith('SCS') ? 'SCS' :
            motor.MODEL.startsWith('STS') ? 'STS' : null;

        const rangeMin = 0;
        const rangeMax = modelType === 'SCS' ? 1023 :
            modelType === 'STS' ? 4095 : 180;

        cells.forEach((value, index) => {
            const td = document.createElement('td');
            td.textContent = value;

            if (index === 0) {
                td.classList.add('non-editable');
            } else {
                td.classList.add('editable-cell');
                td.setAttribute('contenteditable', 'true');
                td.setAttribute('data-type', 'number');

                if (index === 21) {
                    td.setAttribute('data-type', 'text');
                } else {
                    td.setAttribute('data-type', 'number');
                    td.setAttribute('data-min', rangeMin.toString());
                    td.setAttribute('data-max', rangeMax.toString());

                    if (index === 1) {
                        td.setAttribute('data-min', '0');
                        td.setAttribute('data-max', '255');
                    } else if (index === 9) {
                        td.setAttribute('data-min', '0');
                        td.setAttribute('data-max', '1');
                    } else if (index === 14) {
                        td.setAttribute('data-min', '0');
                        td.setAttribute('data-max', '1');
                    }
                }

                td.addEventListener('input', function () {
                    const type = td.getAttribute('data-type');
                    const min = parseFloat(td.getAttribute('data-min'));
                    const max = parseFloat(td.getAttribute('data-max'));
                    let value = td.textContent.trim();

                    if (type === 'number') {
                        let num = parseFloat(value);

                        if (value === '' || isNaN(num)) {
                            num = min;
                            preserveCursor(td, num.toString(), true);
                        } else {
                            if (num < min) num = min;
                            if (num > max) num = max;
                            preserveCursor(td, num.toString());
                        }

                        td.classList.add('edited');
                        td.classList.add('bg-warning');
                        td.title = `Auto-corrected to ${num}`;
                    }

                    console.log("EDITED");
                });
            }

            newRow.appendChild(td);
        });

        tbody.appendChild(newRow);
    }



    function collectChangePackets(channel) {
        const tableId = channel === 0 ? 'channel0-motor-table' :
            channel === 1 ? 'channel1-motor-table' : null;

        const editedCells = document.querySelectorAll(`#${tableId} td.edited`);
        const packets = [];

        editedCells.forEach(cell => {
            const row = cell.closest('tr');
            const rowId = row.getAttribute('data-row-id');
            const title = getColumnTitle(cell);
            const value = cell.textContent.trim();

            packets.push({
                id: rowId,
                title,
                value
            });

            // Remove the classes
            cell.classList.remove('edited', 'bg-warning');
        });

        return packets;
    }


    function getColumnTitle(cell) {
        const table = cell.closest('table');
        const cellIndex = cell.cellIndex;

        const headerRow = table.querySelector('thead tr');
        const headerCell = headerRow.children[cellIndex];

        return headerCell.dataset.key || headerCell.textContent.trim();
    }






    function preserveCursor(td, newText, forceEnd = false) {
        const selection = window.getSelection();
        let cursorOffset = 0;

        if (!forceEnd && selection.rangeCount > 0 && selection.anchorNode === td.firstChild) {
            const range = selection.getRangeAt(0);
            cursorOffset = range.startOffset;
        }

        td.textContent = newText;

        const newRange = document.createRange();
        const offset = forceEnd ? newText.length : Math.min(cursorOffset, newText.length);
        newRange.setStart(td.firstChild, offset);
        newRange.collapse(true);

        selection.removeAllRanges();
        selection.addRange(newRange);
    }

    function getServoMotorByID(channel, id) {
        for (var i = 0; i < servoMotors[channel].length; i++) {
            console.log(servoMotors[channel][i].ID, id);
            if (servoMotors[channel][i].ID === id) {
                return servoMotors[channel][i];
            }
        }
        return null;
    }

    document.getElementById('btn_scan_channel_0').onclick = async () => {
        // Clear table
        document.querySelector("#channel0-motor-table tbody").innerHTML = "";
        servoMotors[0] = [];

        await serial.requestScan(0);
    };

    document.getElementById('btn_scan_channel_1').onclick = async () => {
        // Clear table

        servoMotors[1] = [];
        document.querySelector("#channel1-motor-table tbody").innerHTML = "";

        await serial.requestScan(1);
    };

    document.getElementById('btnSendChangesCh0').onclick = async () => {

        const packets = collectChangePackets(0); // or 2
        for (var i = 0; i < packets.length; i++) {
            let channel = 0
            let servoMotor = getServoMotorByID(channel, parseInt(packets[i].id, 10));
            let dataKey = packets[i].title;
            let value = packets[i].value;
            servoMotor[dataKey] = value;
            let dataPacket = writeData(packets[i].id, servoMotor, dataKey);
            //dataPacket[1] = packets[i].id;
            console.log(dataPacket);
            serial.requestWriteData(dataPacket);
        }
        console.log(servoMotors);
        console.log('Sending packets:', packets);
    };

    document.getElementById('btnSendChangesCh1').onclick = async () => {

        const packets = collectChangePackets(1); // or 2
        for (var i = 0; i < packets.length; i++) {
            let channel = 1
            let servoMotor = getServoMotorByID(channel, parseInt(packets[i].id, 10));
            let dataKey = packets[i].title;
            let value = packets[i].value;
            servoMotor[dataKey] = value;
            let dataPacket = writeData(servoMotor, dataKey);
            serial.requestWriteData(dataPacket);
        }
        console.log(servoMotors);
        console.log('Sending packets:', packets);
    };

    function handleScanChannelResponse(payload) {
        console.log("received motor info packet: " + payload);

        if (payload.length == 2) {
            if (payload[1] == 255) {
                console.log("SCAN COMPLETE");
                document.getElementById('log').value += `Scan Complete\n`;
                return;
            }
        } else if (payload.length != 33) {
            console.log("ERROR: INCORRECT PACKET SIZE: " + payload.length);
            return;
        }
        const motor = new ServoMotor(Array.from(payload));
        console.log(motor.MODEL, motor.POSITION, motor.CURRENT_SPEED);
        servoMotors[motor.CHANNEL].push(motor);

        // If motor is in robot config already, grab the name
        let configMotor = connectedRobot.getMotor(motor.ID);
        if (configMotor != null) {
            motor.NAME = configMotor.NAME;
        }

        insertTableRow(motor);

    }



    feebackCheckbox.addEventListener('change', async function () {
        if (feebackCheckbox.checked) {
            console.log("Checkbox is checked!");
            serial.requestPositionStreaming(true);
        } else {
            console.log("Checkbox is unchecked!");
            serial.requestPositionStreaming(false);
        }
    });


    function matchPositions() {
        // if (feebackCheckbox.checked) {
        //     console.log("Running every 100ms");
        // }
    }

    // Run 10 times per second
    const intervalId = setInterval(matchPositions, 100);
};