import lejos.nxt.*;
import lejos.nxt.addon.*;


/** 
 * Class for controlling PF Motors with MindSensors NRLink-Nx 
 * Extended to handle PWM control of motors
 * 
 * @author Mark Crosbie  <mark@mastincrosbie.com>
 * @author Alexander Vegh <alex@vegh.ch> 
 */ 
class PFLinkPWM extends PFLink { 

	private static final byte NR_TRANSMIT = 0x40;
    private static final byte NR_TXDATA = 0x42;
 
    public static final byte PWM_FORWARD_1 = 1;
    public static final byte PWM_FORWARD_2 = 2;
    public static final byte PWM_FORWARD_3 = 3;
    public static final byte PWM_FORWARD_4 = 4;
    public static final byte PWM_FORWARD_5 = 5;
    public static final byte PWM_FORWARD_6 = 6;
    public static final byte PWM_FORWARD_7 = 7;
    
    public static final byte PWM_FLOAT = 0;
    public static final byte PWM_BRAKE = 8;

    public static final byte PWM_REVERSE_1 = 15;
    public static final byte PWM_REVERSE_2 = 14;
    public static final byte PWM_REVERSE_3 = 13;
    public static final byte PWM_REVERSE_4 = 12;
    public static final byte PWM_REVERSE_5 = 11;
    public static final byte PWM_REVERSE_6 = 10;
    public static final byte PWM_REVERSE_7 = 9;
    
    public static final byte OUTPUT_A = 0;
    public static final byte OUTPUT_B = 1;
    
    public static final byte CHANNEL_1 = 0;
    public static final byte CHANNEL_2 = 1;
    public static final byte CHANNEL_3 = 2;
    public static final byte CHANNEL_4 = 3;
    
 
    public PFLinkPWM(I2CPort _Port) { 
        super(_Port); 
    } 

     /** 
     * Constructs and send a Power Function IR message in raw
     * format to the IR receiver. Sends the message to the IR
     * receiver. 
     * 
     * @param nibble1 - first nibble of command
     * @param nibble2 - second nibble
     * @param nibble3 - third nibble
     * @return Returns non-zero on error, zero on success
     */ 
    private int sendIRMessage(int nibble1, int nibble2, int nibble3) { 
        byte[] buf = new byte[2]; 

        // Store the two byte command in the transmit buffer by writing to the
        // transmit pseudo-register
        buf[0] = (byte) ((byte)nibble1 << 4 | (byte)(nibble2)); 
        buf[1] = (byte) (nibble3 << 4);
        buf[1] += (0xf ^ (byte)nibble1 ^ (byte)nibble2 ^ (byte)nibble3);
        
        sendData(NR_TXDATA, buf, 2);
        
        Sound.pause(10);
        // Now send the bytes by writing to the pseudo-register
        // telling it how many bytes to send
        buf[0] = 2;
        return sendData(NR_TRANSMIT, buf, 1);
    } 
    
    /**
     * Combo PWM mode - set both motors to a fixed speed
     * 
     * @param channel - channel IR receiver is set to. Range 0 to 3 corresponding to Channels 1 to 4
     * @param motorASpeed - speed and direction to set motorA
     * @param motorBSpeed - speed and direction to set motorB
     * @return Non-zero for error, zero for success 
     */
    public int comboPWM(int channel, byte motorASpeed, byte motorBSpeed) {
    	
    	byte nibble1 = (byte) (0x4 | (byte)channel);
    	byte nibble2 = motorBSpeed;
    	byte nibble3 = motorASpeed;
    	
    	return sendIRMessage(nibble1, nibble2, nibble3);
    }

    /**
     * Single PWM mode - set a single motor to a fixed speed
     * 
     * @param channel - channel IR receiver is set to. Range 0 to 3 corresponding to Channels 1 to 4
     * @param output - 0 for Channel A, 1 for Channel B
     * @param motorSpeed - speed and direction to set motorB
     * @return Non-zero for error, zero for success 
     */
    public int singlePWM(int channel, int output, byte motorSpeed) {
    	
    	byte nibble1 = (byte) (channel);
    	byte nibble2 = (byte) ((byte) 0x4 | (byte)output);
    	byte nibble3 = motorSpeed;
    	   	
    	return sendIRMessage(nibble1, nibble2, nibble3);
    }

}

public class PowerFuncPWM {

	public static final SensorPort NRlinkPort = SensorPort.S1;
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		
		  LCD.clearDisplay();
	 
		PFLinkPWM link = new PFLinkPWM(NRlinkPort); 
		link.initialize(PFLinkPWM.NR_RANGE_LONG); 
        LCD.drawString(link.getVersion(), 0, 0); 
        LCD.drawString(link.getProductID(), 0, 1); 
        LCD.drawString(link.getSensorType(), 0, 2); 
 
        for(byte i=0; i < 16; i++) {
        	//link.comboPWM(PFLinkPWM.CHANNEL_1, i, i);
        	link.singlePWM(PFLinkPWM.CHANNEL_1, PFLinkPWM.OUTPUT_B, i);
        	Sound.pause(1000);
        }
        
		Sound.pause(5000);
		LCD.drawString("Done", 0, 4);
	}
}