/*
 * control_packet_coder.java
 *
 * Created on April 6, 2004, 9:39 AM
 */

/**
 *
 * @author Jeff Craighead
 */
public class control_packet_coder {
    
    private byte send_packet[];
    private byte receive_packet[];
    
    //bit format: bit7....bit0
    static final byte autofocusOn = (byte)1; //bit0 
    static final byte laserOn = (byte)2;     //bit1
    static final byte focusOut = (byte)4;    //bit2
    static final byte focusIn = (byte)8;     //bit3
    static final byte raiseDown = (byte)16;  //bit4
    static final byte raiseUp = (byte)32;    //bit5
    static final byte tiltDown = (byte)64;   //bit6
    static final byte tiltUp = (byte)-128;   //bit7
    
    static final byte warning = (byte)1;
    static final byte zoomIn = (byte)2;
    static final byte zoomOut = (byte)4;
    static final byte zoomSpeed = (byte)8;

    
    
    private byte startByte;  // Start byte (byte 0) is either 0x52 or 0x72 for normal or advanced packet respectively
    
    /** Start Control Packet Variables **/
    
    private byte leftTrack;     // byte 1 (control packet)
    private byte rightTrack;    // byte 2 (control packet)
    private byte light;         // byte 3 (control packet)
    private byte tetherFlags;   // byte 4 (control packet) bit0:autofocus bit1:laser bit2:focus-out bit3:focus-in bit4:raise-down bit5:raise-up bit6:tilt-down bit7:tilt-up
    private byte pcFlags;       // byte 5 (control packet) bit0:warning-light bit1:zoom-in bit2:zoon-out bit3:zoom-speed bit4-bit7:not-used
    
    /** Start Response Packet Variables **/
    
    private byte battery;       // byte 1 (response packet)
    private byte temperature;   // byte 2 (response packet)
    private byte pitchHigh;     // byte 3 (response packet)
    private byte pitchLow;      // byte 4 (response packet)
    private byte raise;         // byte 5 (response packet)
    private byte joystickY;     // byte 6 (response packet)
    private byte joystickX;     // byte 7 (response packet)
    private byte cTetherFlags;  // byte 8 (response packet)
    private byte lightKnob;     // byte 9 (response packet)
    
    private byte tiltCurrent;   // byte 10 (adv. response packet)
    private byte raiseCurrent;  // byte 11 (adv. response packet)
    private byte lightCurrent;  // byte 12 (adv. response packet)
    private byte leftTrackCurrent;  // byte 13 (adv. response packet)
    private byte rightTrackCurrent; // byte 14 (adv. response packet)
    private byte focusCurrent;  // byte 15 (adv. response packet)    
    private byte cPcFlags;      // byte 16 (adv. response packet)   
    
    /** The checksum byte is the mathematical sum of bytes 0-10 in all but the advanced return packet where it is the sum of 0-16
     It is byte 11 in all but the advanced response packet where it is byte 17. **/

    private byte checksum;  
    
    /** Creates a new instance of control_packet_coder */
    public control_packet_coder() {
        send_packet = new byte[12];
        receive_packet = new byte[18];
        startByte = 0x72;
        leftTrack = 0;
        rightTrack = 0;
        light = 0;
        tetherFlags = 0;
        pcFlags = 0;
        battery = 0;
        temperature = 0;
        pitchHigh = 0;
        pitchLow = 0;
        raise = 0;
        joystickY = 0;
        joystickX = 0;
        cTetherFlags = 0;
        lightKnob = 0;
        tiltCurrent = 0;
        raiseCurrent = 0;
        lightCurrent = 0;
        leftTrackCurrent = 0;
        rightTrackCurrent = 0;
        focusCurrent = 0;
        cPcFlags = 0;    
        createControlPacket();
    }
    
    
    /**
     * Sets the speed and direction for the left track on the robot.
     * @param speed Signed Byte that indicates speed and direction of the motor.  Negative numbers move the moter in reverse.
     */
    
    public void setLeftTrack(byte speed) { // a two's compliment signed number setting the speed and direction of the left track
        leftTrack=speed;
    }
    
    /**
     * Sets the speed and direction for the left track on the robot.
     * @param speed Signed Byte that indicates speed and direction of the motor.  Negative numbers move the moter in reverse.
     */    
    public void setRightTrack(byte speed) { // a two's compliment signed number setting the speed and direction of the right track
        rightTrack=speed;
    }
    
    /**
     * Sets the intensity of the robot head lights.
     * @param intensity Unsigned Byte indicating how bright the robot head lights are. 0 is off 255 (or -1) is Max.
     */    
    public void setLight(byte intensity) { // a number setting the intensity of the headlights
        light=intensity;
    }
    
    /**
     * Sets the camera Autofocus on or off.
     * @param autofocusOn True for autofocus False for manual.
     */    
    public void setAutofocus(boolean autofocusOn) { // 0-manual focus, 1-autofocus
        if(autofocusOn) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.autofocusOn);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.autofocusOn));
        }
        
    }
    
    /**
     * Sets the laser on or off.
     * @param laserOn True to turn the laser on, False to turn the laser off.
     */    
    public void setLaser(boolean laserOn) { // sets the state of the laser lines
        if(laserOn) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.laserOn);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.laserOn));
        }
    }
    
    /**
     * Makes the camera focus out. If both focus in and out are set the robot will default to in.
     * @param focusOut True to focus out.
     */    
    public void setFocusOut(boolean focusOut) { // causes the field of focus to move out from the vehicle
        if(focusOut) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.focusOut);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.focusOut));
        }
    }
    
    /**
     * Makes the camera focus in. If both focus in and out are set the robot will default to in.
     * @param focusIn True to focus in.
     */    
    public void setFocusIn(boolean focusIn) { // causes the field of focus to move towards the vehicle. If both IN and OUT are set, IN is the default.
        if(focusIn) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.focusIn);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.focusIn));
        }
    }
    
    /**
     * Causes the robot to lower its configuration.
     * @param raiseDown True to lower the robot.
     */    
    public void setRaiseDown(boolean raiseDown) {  // causes the vehicle geometry to lower
        if(raiseDown) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.raiseDown);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.raiseDown));
        }
    }
    
    /**
     * Causes the robot to raise its configuration.
     * @param raiseUp True to raise the robot.
     */    
    public void setRaiseUp(boolean raiseUp) { // causes the vehicle geometry to raise
        if(raiseUp) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.raiseUp);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.raiseUp));
        }
    }
    
    /**
     * Causes the camera to tilt down.
     * @param tiltDown True to tilt down.
     */    
    public void setTiltDown(boolean tiltDown) { // causes the camera to tilt down
        if(tiltDown) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.tiltDown);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.tiltDown));
        }
    }
    
    /**
     * Causes the camera to tilt up.
     * @param tiltUp True to tilt up.
     */    
    public void setTiltUp(boolean tiltUp) { // causes the camera to tilt up
        if(tiltUp) {
            tetherFlags = (byte)(tetherFlags | control_packet_coder.tiltUp);
        }
        else {
            tetherFlags = (byte)(tetherFlags & (~control_packet_coder.tiltUp));
        }
    }
    
    /**
     * Sets the waring light on the OCU.
     * @param warning True to turn the light on, False to turn it off.
     */    
    public void setWarning(boolean warning) { // sets the warning light on the controller panel
        if(warning) {
            pcFlags = ((byte)(pcFlags | control_packet_coder.warning));
        }
        else {
            pcFlags = (byte)(pcFlags & (~control_packet_coder.warning));
        }
    }
    
    /**
     * Causes the camera to zoom in.
     * @param zoomIn True to zoom in.
     */    
    public void setZoomIn(boolean zoomIn) { // causes the camera to zoom in
        if(zoomIn) {
            pcFlags = ((byte)(pcFlags | control_packet_coder.zoomIn));
        }
        else {
            pcFlags = (byte)(pcFlags & (~control_packet_coder.zoomIn));
        }
    }
    
    /**
     * Causes the camera to zoom out.
     * @param zoomOut True to zoom out.
     */    
    public void setZoomOut(boolean zoomOut) { // causes the camera to zoom out
        if(zoomOut) {
            pcFlags = (byte)(pcFlags | control_packet_coder.zoomOut);
        }
        else {
            pcFlags = (byte)(pcFlags & (~control_packet_coder.zoomOut));
        }
    }
    
    /**
     * Sets the zoom speed of the camera.  There are two settings, fast and slow.
     * @param zoomSpeed True for fast, false for slow.
     */    
    public void setZoomSpeed(boolean zoomSpeed){ // 0-low, 1-high
        if(zoomSpeed) {
            pcFlags = (byte)(pcFlags | control_packet_coder.zoomSpeed);
        }
        else {
            pcFlags = (byte)(pcFlags & (~control_packet_coder.zoomSpeed));
        }
    }
    
    /**
     * This is used to update all of the object variables to the new values returned in the response packet.
     * It takes an array of bytes (a response packet) that you get from the SerialPortControl object.
     * @param rPacket This is the response packet array.  Get this from the SerialPortControl object.
     * @return Returns 0 if the packet was OK, returns -1 if there was a checksum error.
     */    
    public int decodeResponsePacket(byte rPacket[]){
        if(rPacket[17]==calculateChecksum(rPacket)){
            battery = rPacket[1];
            temperature = rPacket[2];
            pitchHigh = rPacket[3];
            pitchLow = rPacket[4];
            raise = rPacket[5];
            joystickY = rPacket[6];
            joystickX = rPacket[7];
            cTetherFlags = rPacket[8];
            lightKnob = rPacket[9];
            tiltCurrent = rPacket[10];
            raiseCurrent = rPacket[11];
            lightCurrent = rPacket[12];
            leftTrackCurrent = rPacket[13];
            rightTrackCurrent = rPacket[14];
            focusCurrent = rPacket[15];
            cPcFlags = rPacket[16];
            return 0;
        }
        else{
            System.out.println("Bad Checksum on Response Packet");
            return -1;
        }
        
    }
    
    /**
     * This creates a control packet that can be sent to the robot via the SerialPortControl object.
     * @return The returned byte array is the control packet.  Send it to the robot via the SerialPortControl object.
     */    
    public byte[] createControlPacket(){
        
        send_packet[0] = startByte;
        send_packet[1] = leftTrack;
        send_packet[2] = rightTrack;
        send_packet[3] = light;
        send_packet[4] = tetherFlags;
        send_packet[5] = pcFlags;
        send_packet[6] = 0;
        send_packet[7] = 0;
        send_packet[8] = 0;
        send_packet[9] = 0;
        send_packet[10] = 0;
        send_packet[11] = calculateChecksum(send_packet);
        return send_packet;
        
    }
    
    
    
    /**
     * Returns the left track vector.  Postitive values indicate forward, negative values indicate backwards.
     * @return The return byte indicates the speed and direction of the left track.
     */
    
    public byte getLeftTrack() {
        return leftTrack;
    }
    
    /**
     * Returns the right track vector.  Postitive values indicate forward, negative values indicate backwards.
     * @return The return byte indicates the speed and direction of the left track.
     */    
    public byte getRightTrack() {
        return rightTrack;
    }
    
    /**
     * Returns the light intensity setting.
     * @return This byte indicates how bright the light is set.
     * This should be interpreted as an unsigned byte and is on a scale where 0 is off and 255 is brightest.
     */    
    public byte getLight() {
        return light;
    }
    
    /**
     * Returns the battery voltage.
     * @return The battery voltage in volts.
     */    
    public double getBatteryVoltage() {
        return ((double)battery/3.1914);
    }
    
    /**
     * Returns the battery voltage as a raw value from the controller.  This is not the actual voltage.
     * @return A byte that returns the RAW battery voltage number from the controller.
     */    
    public byte getBatteryRaw() {
        return battery;
    }
    
    /**
     * Returns the external temperature at the robot.
     * @return Temperature at the robot in degrees Farenheight.
     */    
    public byte getTemperature() {
        return temperature;
    }
    
    /**
     * Returns the pitch of the robot.
     * @return This signed integer indicates pitch of the robot in degrees above or below horizontal.
     */    
    public int getPitch() {
        int pitch = pitchHigh << 8;
        int iPitchLow = pitchLow;
        pitch |= iPitchLow;
        return pitch;
    }
    
    /**
     * Returns the raise of the robot.
     * @return This byte indicates the raise of the robot from 0-90 degrees above horizontal.
     */    
    public byte getRaise() {
        return raise;
    }
    
    /**
     * Returns the Y postition of the OCU joystick.
     * @return This is a signed value indicating the Y position of the OCU joystick.
     * Returns 0 when the joystick is centered.
     */    
    public byte getJoyY() {
        return joystickY;
    }
    
    /**
     * Returns the X postion of the OCU joystick.
     * @return This is a signed value indicating the Y position of the OCU joystick.
     * Returns 0 when the joystick is centered.
     */    
    public byte getJoyX() {
        return joystickX;
    }
    
    /**
     * Returns the autofocus setting.
     * @return A boolean value that is true if autofocus is enabled and false if it is disabled.
     */    
    public boolean getAutofocus() {
        if((tetherFlags & control_packet_coder.autofocusOn)>0) return true;
        else return false;
    }
    
    /**
     * Returns the on/off state of the laser.
     * @return A boolean value that is true for on and false for off.
     */    
    public boolean getLaser() {
        if((tetherFlags & control_packet_coder.laserOn)>0) return true;
        else return false;
    }
    
    /**
     * Returns the focusOut status.
     * @return A boolean value that is true if focusOut is enabled and false if it is not.
     */    
    public boolean getFocusOut() {
        if((tetherFlags & control_packet_coder.focusOut)>0) return true;
        else return false;
    }
    
    /**
     * Returns the focusIn status.
     * @return A boolean value that is true if focusIn is enabled and false if it is not.
     */    
    public boolean getFocusIn() {
        if((tetherFlags & control_packet_coder.focusIn)>0) return true;
        else return false;
    }
    
    /**
     * Returns the RaiseDown status.
     * @return A boolean value that is true if RaiseDown is enabled, false if it is not.
     */    
    public boolean getRaiseDown() {
        if((tetherFlags & control_packet_coder.raiseDown)>0) return true;
        else return false;
    }
    
    /**
     * Returns the RaiseUp status.
     * @return A boolean value that is true if RaiseUp is enabled, false if it is not.
     */    
    public boolean getRaiseUp() {
        if((tetherFlags & control_packet_coder.raiseUp)>0) return true;
        else return false;
    }
    
    /**
     * Returns the TiltDown status.
     * @return A boolean value that is true if TiltDown is enabled, false if it is not.
     */    
    public boolean getTiltDown() {
        if((tetherFlags & control_packet_coder.tiltDown)>0) return true;
        else return false;
    }
    
    /**
     * Returns the TiltUp status.
     * @return A boolean value that is true if TiltUp is enabled, false if it is not.
     */    
    public boolean getTiltUp() {
        if((tetherFlags & control_packet_coder.tiltUp)>0) return true;
        else return false;
    }
    
    /**
     * Returns the OCU warning light status.
     * @return A boolean value that is true if the warning light is enabled, false if it is not.
     */    
    public boolean getWarning() {
        if((pcFlags & control_packet_coder.warning)>0) return true;
        else return false;
    }
    
    /**
     * Returns the ZoomIn status.
     * @return A boolean value that is true if ZoomIn is enabled, false if it is not.
     */    
    public boolean getZoomIn() {
        if((pcFlags & control_packet_coder.zoomIn)>0) return true;
        else return false;
    }
    
    /**
     * Returns the ZoomOut status.
     * @return A boolean value that is true if ZoomOut is enabled, false if it is not.
     */    
    public boolean getZoomOut() {
        if((pcFlags & control_packet_coder.zoomOut)>0) return true;
        else return false;
    }
    
    /**
     * Returns the ZoomSpeed status.
     * @return A boolean value that is true if ZoomSpeed is fast, false if it is slow.
     */    
    public boolean getZoomSpeed() {
        if((pcFlags & control_packet_coder.zoomSpeed)>0) return true;
        else return false;
    }
    
    /**
     * Returns the tilt current.
     * @return A byte that indicates the current draw of the tilt motor.
     */    
    public byte getTiltCurrent() {
        return tiltCurrent;
    }
    
    /**
     * Returns the raise current.
     * @return A byte that indicates the current draw of the raise motor.
     */    
    public byte getRaiseCurrent() {
        return raiseCurrent;
    }
    
    /**
     * Returns the light current.
     * @return A byte that indicates the current draw of the headlights.
     */    
    public byte getLightCurrent() {
        return lightCurrent;
    }
    
    /**
     * Returns the left track current.
     * @return A byte that indicates the current draw of the left track motor.
     */    
    public byte getLeftTrackCurrent() {
        return leftTrackCurrent;
    }
    
    /**
     * Returns the right track current.
     * @return A byte that indicates the current draw of the right track motor.
     */    
    public byte getRightTrackCurrent() {
        return rightTrackCurrent;
    }
    
    /**
     * Returns the focus current.
     * @return A byte that indicates the current draw of the focus motor.
     */    
    public byte getFocusCurrent() {
        return focusCurrent;
    }
    
    /**
     * Returns the position of the OCU light knob.
     * @return An unsigned byte that indicates the position of the OCU light knob.
     */    
    public byte getLightKnob() {
        return lightKnob;
    }
    
    /**
     * Retuns the controller Tether flag byte.  This is part of the return packet.
     * @return A byte that contains the raw Controller Tether Flags, these are accessable with the get functions in this object.
     */    
    public byte getControllerTetherFlags() {
        return cTetherFlags;
    }
    
    /**
     * Retuns the controller PC flag byte.  This is part of the return packet.
     * @return A byte that contains the raw Controller PC Flags, these are accessable with the get functions in this object.
     */    
    public byte getControllerPcFlags() {
        return cPcFlags;
    }
        
    
    /** Start Checksum Functions **/
    
    private byte calculateChecksum(byte[] packet){
        byte sum = 0;
        for(int i=0; i<packet.length-1; i++){
            sum += packet[i];
        }        
        return sum;
    }
    
}