export class ServoMotor {
    constructor(arg1, arg2, arg3, arg4) {
        if (Array.isArray(arg1)) {
            // Full payload constructor
            const payload = arg1;

            this.CHANNEL = payload[0];
            this.ID = payload[1];
            this.MODEL = getModelType(payload[3], payload[2]);

            this.MIN_ANGLE_LIMIT = (payload[4] << 8) | payload[5];
            this.MAX_ANGLE_LIMIT = (payload[6] << 8) | payload[7];
            this.POSITION = (payload[8] << 8) | payload[9];
            this.CW_DEAD_ZONE = payload[10];
            this.CCW_DEAD_ZONE = payload[11];
            this.OFFSET = (payload[12] << 8) | payload[13];
            this.MODE = payload[14];
            this.TORQUE_ENABLE = payload[15];
            this.ACCELERATION = payload[16];
            this.GOAL_POSITION = (payload[17] << 8) | payload[18];
            this.GOAL_TIME = (payload[19] << 8) | payload[20];
            this.GOAL_SPEED = (payload[21] << 8) | payload[22];
            this.LOCK = payload[23];

            const rawSpeed = (payload[24] << 8) | payload[25];
            this.CURRENT_SPEED = rawSpeed > 0x7FFF ? rawSpeed - 0x10000 : rawSpeed;

            this.CURRENT_LOAD = (payload[26] << 8) | payload[27];
            this.TEMPERATURE = payload[28];
            this.MOVING = payload[29];
            this.CURRENT_CURRENT = (payload[30] << 8) | payload[31];
            this.VOLTAGE = payload[32];
        } else {
            // Simplified constructor used by robot config loader
            this.CHANNEL = arg1;
            this.ID = arg2;

            this.MODEL = arg3 || 'Unknown Model';

            this.NAME = arg4 || "UNKNOWN";
        }
    }
}


// Takes Motor object and updates simulated value, returns packet for real device
export function writeData(targetID, motor, key) {
    const entry = DataMap[key];
    if (!entry) {
        throw new Error(`Invalid data key: ${key}`);
    }

    let { address, length } = entry;
    const value = motor[key];

    if (value === undefined) {
        throw new Error(`Motor does not contain value for key: ${key}`);
    }



    // HANDLE DIFFERENT ADDRESS FOR LOCK ON SMS_STS    
    if (key == 'LOCK') {
        if (motor.MODEL && (motor.MODEL.includes("SMS") || motor.MODEL.includes("STS"))) {
            address = DataMap.LOCK_SMS_STS.address;
        } else {
            address = DataMap.LOCK.address;
        }
    }

    const packet = [motor.CHANNEL, targetID, address, length]; // channel, id, address, value

    if (length === 2) {
        packet.push(value & 0xFF);
        packet.push((value >> 8) & 0xFF);
    } else if (length === 1) {
        packet.push(value);
    } else {
        throw new Error(`Unsupported byte length: ${length}`);
    }

    return new Uint8Array(packet);
}


const DataMap = Object.freeze({
    MODEL: { address: 0x03, length: 2 },
    ID: { address: 0x05, length: 1 },
    BAUD_RATE: { address: 0x06, length: 1 },
    MIN_ANGLE_LIMIT: { address: 0x09, length: 2 },
    MAX_ANGLE_LIMIT: { address: 0x0B, length: 2 },
    CW_DEAD_ZONE: { address: 0x1A, length: 1 },
    CCW_DEAD_ZONE: { address: 0x1B, length: 1 },
    OFFSET: { address: 0x1F, length: 2 },
    MODE: { address: 0x21, length: 1 },
    TORQUE_ENABLE: { address: 0x28, length: 1 },
    ACCELERATION: { address: 0x29, length: 1 },
    GOAL_POSITION: { address: 0x2A, length: 2 },
    GOAL_TIME: { address: 0x2C, length: 2 },
    GOAL_SPEED: { address: 0x2E, length: 2 },
    LOCK: { address: 0x30, length: 1 }, //SCS
    LOCK_SMS_STS: { address: 0x37, length: 1 },//SMS_STS
    POSITION: { address: 0x38, length: 2 },
    CURRENT_SPEED: { address: 0x3A, length: 2 },
    CURRENT_LOAD: { address: 0x3C, length: 2 },
    VOLTAGE: { address: 0x3E, length: 1 },
    TEMPERATURE: { address: 0x3F, length: 1 },
    MOVING: { address: 0x42, length: 1 },
    CURRENT_CURRENT: { address: 0x45, length: 2 }
});



export function getModelType(major, minor) {


    const id = combine(major, minor);
    return modelList.get(id) || "Unknown Model";
}

const modelList = new Map([
    [combine(5, 0), "SCSXX"],
    [combine(5, 4), "SCS009"],
    [combine(5, 8), "SCS2332"],
    [combine(5, 12), "SCS45"],
    [combine(5, 15), "SCS15"],
    [combine(5, 16), "SCS315"],
    [combine(5, 25), "SCS115"],
    [combine(5, 35), "SCS215"],
    [combine(5, 40), "SCS40"],
    [combine(5, 60), "SCS6560"],
    [combine(5, 240), "SCDZZ"],
    [combine(6, 0), "SMXX-360M"],
    [combine(6, 3), "SM30-360M"],
    [combine(6, 8), "SM60-360M"],
    [combine(6, 12), "SM80-360M"],
    [combine(6, 16), "SM100-360M"],
    [combine(6, 20), "SM150-360M"],
    [combine(6, 24), "SM85-360M"],
    [combine(6, 26), "SM60-360M"],
    [combine(8, 0), "SM30BL"],
    [combine(8, 1), "SM30BL"],
    [combine(8, 2), "SM30BL"],
    [combine(8, 3), "SM30BL"],
    [combine(8, 4), "SM30BL"],
    [combine(8, 5), "SM30BL"],
    [combine(8, 6), "SM30BL"],
    [combine(8, 7), "SM30BL"],
    [combine(8, 8), "SM30BL"],
    [combine(8, 9), "SM30BL"],
    [combine(8, 10), "SM30BL"],
    [combine(8, 11), "SM30BL"],
    [combine(8, 12), "SM30BL"],
    [combine(8, 13), "SM30BL"],
    [combine(8, 14), "SM30BL"],
    [combine(8, 15), "SM30BL"],
    [combine(8, 16), "SM30BL"],
    [combine(8, 17), "SM30BL"],
    [combine(8, 18), "SM30BL"],
    [combine(8, 19), "SM30BL"],
    [combine(8, 25), "SM29BL(LJ)"],
    [combine(8, 29), "SM29BL(FT)"],
    [combine(8, 30), "SM30BL(FT)"],
    [combine(8, 20), "SM30BL(LJ)"],
    [combine(8, 40), "SM40BLHV"],
    [combine(8, 42), "SM45BLHV"],
    [combine(8, 44), "SM85BLHV"],
    [combine(8, 120), "SM120BLHV"],
    [combine(8, 220), "SM200BLHV"],
    [combine(9, 0), "STSXX"],
    [combine(9, 2), "STS3032"],
    [combine(9, 3), "STS3215"],
    [combine(9, 4), "STS3040"],
    [combine(9, 5), "STS3020"],
    [combine(9, 6), "STS3046"],
    [combine(9, 20), "SCSXX-2"],
    [combine(9, 15), "SCS15-2"],
    [combine(9, 35), "SCS225"],
    [combine(9, 40), "SCS40-2"]
]);

export const reverseModelMap = new Map();
for (const [combined, model] of modelList.entries()) {
    const minor = combined >> 8;
    const major = combined & 0xFF;
    reverseModelMap.set(model, { major, minor });
}

function combine(major, minor) {
    return (minor << 8) | major;
}
