/*
 * RoverState.java
 *
 * Created on June 30, 2003, 10:48 AM
 */

package PER.rover.control;

/**
 *
 * @author  Eric Porter
 */

public class RoverState {
    private int status;
    private int range;
    private int voltage;
    private int pan;
    private int tilt;
    private int dist;
    
    public final static int FULL_PACKET_LENGTH = 16;
    
    public final static int SUCCESS                    = 0;
    public final static int HL_CONTINUE                = 5;
    //errors detected locally
    public final static int INVALID_PACKET_LENGTH      = 10;
    public final static int UNKNOWN_PACKET_TYPE        = 11;
    public final static int COMM_DEAD                  = 12;
    //nonfatal errors detected on the Stayton
    public final static int STAYTON_INVALID_LENGTH     = 30;
    public final static int STAYTON_UNKNOWN_TYPE       = 31;
    public final static int BAD_INPUT                  = 32;
    //simple errors detected on Stayton that are fatal to a high level command
    public final static int CEREB_TIMEOUT              = 50;
    public final static int CAMERA_TIMEOUT             = 51;
    public final static int STAYTON_IO_ERROR           = 60;
    public final static int BATTERY_LOW                = 70;
    //fatal errors from the Stayton for high level commands only
    public final static int KILLED                     = 100;
    public final static int CANT_START                 = 101;
    public final static int STUCK                      = 102;
    public final static int OBSTACLE_DETECTED          = 103;
    public final static int HIT_WALL                   = 104;
    public final static int HL_USING_RESOURCE          = 105;
    public final static int TRACK_USING_RESOURCE       = 106;
    public final static int LOST_LANDMARK              = 111;
    public final static int TRACKING_HIGH              = 112;
    
    /** Creates new RoverState */
    public RoverState() {
    }
    
    public boolean parsePacket(byte [] packet) {
        if (packet == null || packet.length == 0) {
            status = COMM_DEAD;
            PER.rover.Log.println("No response, expected "+FULL_PACKET_LENGTH+" bytes!");
            return false;
        }
        if (packet.length != FULL_PACKET_LENGTH) {
            status = INVALID_PACKET_LENGTH;
            PER.rover.Log.println("Invalid packet; got "+packet.length+" bytes, expected "+FULL_PACKET_LENGTH+"!");
            return false;
        }
        
        status = ByteUtil.unsign(packet[0]);
        range = ByteUtil.unsign(packet[1]);
        pan = ByteUtil.networkShortToInt(packet, 2);
        tilt = ByteUtil.networkShortToInt(packet, 4);
        dist = ByteUtil.networkShortToInt(packet, 6);
        voltage = ByteUtil.unsign(packet[8]);
        //System.out.println("raw voltage: "+voltage+" read voltage: "+getRealVoltage());
        return status == SUCCESS;
    }
    
    // Accessors
    public int Status()			{ return status; }
    public int getStatus() { return status; }
    public void setStatus(int status) { this.status = status; }
    public int getRange() { return range; }
    public int Pan()			{ return pan; }
    public int getPan() { return pan; }
    public int Tilt()			{ return tilt; }
    public int getTilt()	{ return tilt; }
    
    /**
     * Gets the raw voltage reading from the cerebellum.
     * @return the raw voltage reading, between 0 and 255.
     */
    public int getRawVoltage() {
       return voltage;
    }
    
    /**
     * Gets a translated voltage reading on the robot.  
     * @return the voltage of the batteries in volts
     */
    public double getRealVoltage() {
       return .204*voltage;
    }
    
    /** Getter for property dist.
     * @return the absolute value of how far the rover has traveled
     *  (cm for goTo or degrees for turnTo)
     */
    public int getDist() {
        return dist;
    }
    
    /**
     * Function to help with high level commands.
     * @return true if the status returned caused the high level command to terminate.
     */
    public boolean isTerminationCondition() {
       return status == SUCCESS || status >= CEREB_TIMEOUT;
    }
}
