// wedoExtension.js
// Shane M. Clements, January 2014
// LEGO WEDO Scratch Extension
//
// This is an extension for development and testing of the Scratch Javascript Extension API.

(function(ext) {
    var device = null;
    var rawData = null;

    // Motor states: power: 0 to 100, dir: -1 or 1
    var motors = [
        {power: 100, dir: 1, isOn: false},
        {power: 100, dir: 1, isOn: false}
    ];
    var motorOffTime = 0;

    // Sensor states:
    var id0 = 0;
    var id1 = 0;
    var weDoDistance = 0;
    var weDoTilt = 0;

    // Commands
    ext.motorOnFor = function(motor, time, callback) {
        //ext.allMotorsOn();
	ext.motorOn(motor);

        setTimeout(function() {
	    ext.motorOff(motor);
            //callback();
	    if (typeof callback=="function") callback();
        }, 1000 * time);
    };
    
    ext.motorOn = function(motor) {
	switch(motor) {
	    case "motor":
		ext.allMotorsOn('m');
		break;
	    case "motor A":
		setMotorOn('m', 0, true);
		break;
	    case "motor B":
		setMotorOn('m', 1, true);
		break;
	    case "lights":
		ext.allMotorsOn('l');
		break;
	    default:
		ext.allMotorsOn('a');
	}
    };

    ext.allMotorsOn = function(type) {
        setMotorOn(type, 0, true);
        setMotorOn(type, 1, true);
    };

    ext.motorOff = function(motor) {
	switch(motor) {
	    case "motor":
		ext.allMotorsOff('m');
		break;
	    case "motor A":
		setMotorOn('m', 0, false);
		break;
	    case "motor B":
		setMotorOn('m', 1, false);
		break;
	    case "lights":
		ext.allMotorsOff('l');
		break;
	    default:
		ext.allMotorsOff('a');
	}
    };

    ext.resetAll = function() {
	ext.allMotorsOff('a');
    };
    ext.allMotorsOff = function(type) {
        setMotorOn(type, 0, false);
        setMotorOn(type, 1, false);
    };

    ext.startMotorPower = function(motor, power) {
	switch(motor) {
	    case "motor":
		setMotorPower('m', 0, power);
		setMotorPower('m', 1, power);
		setMotorOn('m', 0, true);
		setMotorOn('m', 1, true);
		break;
	    case "motor A":
		setMotorPower('m', 0, power);
		setMotorOn('m', 0, true);
		break;
	    case "motor B":
		setMotorPower('m', 1, power);
		setMotorOn('m', 1, true);
		break;
	    case "lights":
		setMotorPower('l', 0, power);
		setMotorPower('l', 1, power);
		setMotorOn('l', 0, true);
		setMotorOn('l', 1, true);
		break;
	    default:
		setMotorPower('a', 0, power);
		setMotorPower('a', 1, power);
		setMotorOn('a', 0, true);
		setMotorOn('a', 1, true);
	}
    };

    ext.setMotorDirection = function(motor, s) {
        var dir;
        if('this way' == s) dir = 1;
        else if('that way' == s) dir = -1;
        else if('reverse' == s) dir = 0;
        else return;

	switch(motor) {
	    case "motor A":
		setMotorDirection(0, dir);
		break;
	    case "motor B":
		setMotorDirection(1, dir);
		break;
	    default:
		setMotorDirection(0, dir);
		setMotorDirection(1, dir);
	}
    };

    // Hat blocks
    ext.whenDistance = function(s, dist) { return device!=null && ('<' == s ? (getDistance() < dist) : (getDistance() > dist)); };
    ext.whenTilt = function(s, tilt) { return device!=null && ('=' == s ? (getTilt() == tilt) : (getTilt() != tilt)); };
    //ext.whenDistanceLessThan = function(dist) { return device!=null && getDistance() < dist; };
    //ext.whenTiltIs = function(tilt) { return device!=null && getTilt() == tilt; };

    // Reporters
    ext.getDistance = function() { return getDistance(); };
    ext.getTilt = function() { return getTilt(); };

    // Internal logic
    function setMotorDirection(motorID, dir) {
        // Dir: -1 - counter-clockwise, 1 - clockwise, 0 - reverse
        var motor = getMotor('m', motorID);
        if (!motor) return; // motorID must be 0 or 1
        if ((dir == -1) || (dir == 1)) motor.dir = dir;
        if (dir == 0) motor.dir = -motor.dir; // reverse
        if (motor.isOn) sendMotorState();
    };

    function setMotorOn(type, motorID, flag) {
        var motor = getMotor(type, motorID);
        if (!motor) return; // motorID must be 0 or 1
        var wasOn = motor.isOn && (motor.power > 0);
        motor.isOn = (flag == true);
        if (wasOn) checkForMotorsOff();
        sendMotorState();
    };

    function setMotorPower(type, motorID, pwr) {
        // Pwr: 0..100
        var motor = getMotor(type, motorID);
        if (!motor) return; // motorID must be 0 or 1
        var wasOn = motor.isOn && (motor.power > 0);
        motor.power = Math.max(0, Math.min(pwr, 100));
        if (motor.power > 0) motor.isOn = true;
        if (wasOn) checkForMotorsOff();
        sendMotorState();
    };

    var wedoCommand = new Uint8Array(9); wedoCommand[1] = 0x40;
    function sendMotorState() {
        if (device) {
            // Each motor is controlled by a signed byte whose sign determines the direction and absolute value the power
            wedoCommand[2] = motorValue(0);
            wedoCommand[3] = motorValue(1);
            device.write(wedoCommand.buffer);
        }
    }

    function motorValue(motorID) {
        // Return a two character hex string to control the given motor.
        var motor = motors[motorID];
        var byte = 0;
        if (motor.isOn && (motor.power > 0)) byte = (17 + Math.floor(1.1 * motor.power));
        if (motor.dir < 0) byte = (256 - byte) & 0xFF;
        return byte;
    }

    function getMotor(type, motorID) {
	if(rawData && okayToReadIDs()) {
	    var s = new Uint8Array(rawData);
	    id0 = s[3];
	    id1 = s[5];
	}
	//console.log(id0);
	//console.log(id0.toString(2));
	//console.log(id1);
	//console.log(id1.toString(2));
	//console.log();
        if ((motorID == 0) && isMotor(type, id0)) return motors[0];
        if ((motorID == 1) && isMotor(type, id1)) return motors[1];
        return null;
    }

    function isMotor(type, id) {
	switch (type) {
	    case 'm': // motor
		return (234 <= id) && (id <= 246);
	    case 'l': // light
		return (200 <= id) && (id <= 205);
	}
	return ((234 <= id) && (id <= 246)) || ((200 <= id) && (id <= 205));
    }

    function checkForMotorsOff() {
        // Called on motor transition from on to off or motor power goes from non-zero to zero.
        // If both motors are just become off (or zero power), set motorOffTime to the current time.
        if (motors[0].isOn && (motors[0].power > 0)) return; // motor 0 is still on
        if (motors[1].isOn && (motors[1].power > 0)) return; // motor 1 is still on
        motorOffTime = new Date().getTime();
    }

    function okayToReadIDs() {
        // The WeDo sensor ID data is garbled and meaningless while any motor is running.
        // In fact, the ID continues to be garbled for a short while after all motors have
        // been turned off because the motor "coasts" and generates a current which throws
        // off the analog-to-digital converter in the WeDo hub. Thus, we keep track when the last
        // motor was turned off and wait half a second before trying to read the sensor ID's
        // Cached values of the sensor ID's are used while motors are running. Thus, if a user
        // plugs a different sensor into the WeDo hub while the motors are running, the plugin
        // won't notice until all motors are stopped.
        if (motors[0].isOn || motors[1].isOn) return false;
        return (new Date().getTime() - motorOffTime) > 500;
    }

    function updateSensor(id, rawValue) {
        if ((170 <= id) && (id <= 190)) { // distance sensor
            weDoDistance = Math.round((100 * (rawValue - 70)) / 140);
            weDoDistance = Math.max(0, Math.min(weDoDistance, 100));
        }
        if ((28 <= id) && (id <= 47)) { // tilt sensor
            if (rawValue < 49) weDoTilt = 3;
            else if (rawValue < 100) weDoTilt = 2;
            else if (rawValue < 154) weDoTilt = 0;
            else if (rawValue < 205) weDoTilt = 1;
            else weDoTilt = 4;
        }
    }

    function getDistance() {
        if(rawData) processData();
        return weDoDistance;
    }

    function getTilt() {
        if(rawData) processData();
        return weDoTilt;
    }

    function processData() {
        var s = new Uint8Array(rawData);

        if (okayToReadIDs()) {
            id0 = s[3];
            id1 = s[5];
        }
        weDoDistance = weDoTilt = 0; // zero if no sensor plugged in
        updateSensor(id0, s[2]);
        updateSensor(id1, s[4]);

        rawData = null;
    }

    var poller = null;
    ext._deviceConnected = function(dev) {
        if(device) return;

        device = dev;
        device.open();
        poller = setInterval(function() {
            device.read(function(data) {
                rawData = data;
            });
        }, 20);
    };

    ext._deviceRemoved = function(dev) {
        if(device != dev) return;
        if(poller) poller = clearInterval(poller);
        device = null;
    };

    ext._shutdown = function() {
        setMotorOn('a', 0, false);
        setMotorOn('a', 1, false);

        if(poller) poller = clearInterval(poller);
        if(device) device.close();
        device = null;
    };

    ext._getStatus = function() {
        if(!device) return {status: 1, msg: 'LEGO WeDo disconnected'};
        return {status: 2, msg: ' LEGO WeDo connected'};
    }

    var descriptor = {
        blocks: [
            ['w', 'turn %m.motor on for %n secs',               'motorOnFor',       'motor',1],
            [' ', 'turn %m.motor on',                           'motorOn',          'motor'],
            [' ', 'turn %m.motor off',                          'motorOff',         'motor'],
            [' ', 'set %m.motor power to %n',                   'startMotorPower',  'motor',100],
            [' ', 'set %m.motor2 direction to %m.motorDirection','setMotorDirection','motor','this way'],
            ['h', 'when distance %m.lessMore %n',               'whenDistance',     '<',    20],
            ['h', 'when tilt %m.eNe %n',                        'whenTilt',         '=',    1],
            ['r', 'distance',                                   'getDistance'],
            ['r', 'tilt',                                       'getTilt']
        ],
        menus: {
            motor: ['motor', 'motor A', 'motor B', 'lights', 'everything'],
	    motor2: ['motor', 'motor A', 'motor B', 'all motors'],
            motorDirection: ['this way', 'that way', 'reverse'],
            lessMore: ['<', '>'],
            eNe: ['=','not =']
        },
	url: '/info/help/studio/tips/ext/LEGO WeDo/'
    };
    ScratchExtensions.register('LEGO WeDo', descriptor, ext, {type: 'hid', vendor:1684, product:3});
})({});