package PER.rover.control;

/*
 * TrikebotCommand.java
 *
 * Created on June 30, 2003, 9:33 AM
 */

public class RoverCommand {
   
   //simple stuff
   private final static byte INIT_ROVER                 = (byte) 'i';
   private final static byte HEAD_MOVE                  = (byte) 'h';
   private final static byte SET_ALL                    = (byte) 'l';
   private final static byte SPIN                       = (byte) 'p';
   private final static byte CRAB                       = (byte) 'c';
   private final static byte QUAD_TURN                  = (byte) 'q';
   private final static byte SCAN                       = (byte) 'n';
   private final static byte SET_LIGHT                  = (byte) 'x';
   //cmaera commands
   private final static byte TAKE_PICTURE               = (byte) 'a';
   private final static byte ADJUST_BRIGHTNESS          = (byte) 'b';
   private final static byte START_TRACK                = (byte) 'r';
   private final static byte STOP_TRACK                 = (byte) 'o';
   private final static byte DO_SPECIAL                 = (byte) 10;
   
   //advanced commands
   private final static byte GOTO                       = (byte) 'g';
   private final static byte TURNTO                     = (byte) 't';
   private final static byte STOP_AT_MARK               = (byte) 's';
   //private final static byte ALIGN_TO_MARK              = (byte) 'a';
   private final static byte GET_UPDATE                 = (byte) 'u';
   private final static byte KILL                       = (byte) 'k';
   private final static byte SET_CALIBRATION            = (byte) 'y';
   private final static byte GET_CALIBRATION            = (byte) 'z';
   private final static byte SET_SCAN_LIST              = (byte) 11;
   private final static byte GET_SCAN_LIST              = (byte) 12;
   
   private byte [] buf; //the internal buffer for the packet
   private int BUFFER_LENGTH = 1000;
   
   private int commandLength;
   
   public RoverCommand() {
      buf = new byte[BUFFER_LENGTH];
   }
   
   public byte [] getData() {
      return buf;
   }
   
   public int getLength() {
      return commandLength;
   }
   
   private void setType(byte type) {
      buf[0] = type;
   }
   
   public RoverCommand initRover() {
      commandLength = 1;
      setType(INIT_ROVER);
      return this;
   }
   
   public RoverCommand takePicture(int pan, int tilt, int width, int height, boolean lightUV) {
      commandLength = 20;
      setType(TAKE_PICTURE);
      buf[1] = 0;
      buf[2] = (byte) (lightUV ? 1 : 0);
      ByteUtil.intToNetworkLong(buf, pan, 4);
      ByteUtil.intToNetworkLong(buf, tilt, 8);
      ByteUtil.intToNetworkLong(buf, width, 12);
      ByteUtil.intToNetworkLong(buf, height, 16);
      return this;
   }
   
   public RoverCommand takeRecentPicture() {
      commandLength = 20;
      setType(TAKE_PICTURE);
      buf[1] = 1;
      buf[2] = 0;
      return this;
   }
   
   public RoverCommand takeRawPicture(int pan, int tilt, int width, int height, boolean lightUV) {
      commandLength = 20;
      setType(TAKE_PICTURE);
      buf[1] = 2;
      buf[2] = (byte) (lightUV ? 1 : 0);
      ByteUtil.intToNetworkLong(buf, pan, 4);
      ByteUtil.intToNetworkLong(buf, tilt, 8);
      ByteUtil.intToNetworkLong(buf, width, 12);
      ByteUtil.intToNetworkLong(buf, height, 16);
      return this;
   }
   
   public RoverCommand scan(int tilt, int minPan, int maxPan, int step) {
      commandLength = 20;
      setType(SCAN);
      ByteUtil.intToNetworkLong(buf, tilt, 4);
      ByteUtil.intToNetworkLong(buf, minPan, 8);
      ByteUtil.intToNetworkLong(buf, maxPan, 12);
      ByteUtil.intToNetworkLong(buf, step, 16);
      return this;
   }
   
   public RoverCommand spin(int speed) {
      commandLength = 8;
      setType(SPIN);
      ByteUtil.intToNetworkLong(buf, speed, 4);
      return this;
   }
   
   public RoverCommand crab(int speed, int angle) {
      commandLength = 12;
      setType(CRAB);
      ByteUtil.intToNetworkLong(buf, speed, 4);
      ByteUtil.intToNetworkLong(buf, angle, 8);
      return this;
   }
   
   public RoverCommand quadTurn(int speed, int radius) {
      commandLength = 12;
      setType(QUAD_TURN);
      ByteUtil.intToNetworkLong(buf, speed, 4);
      ByteUtil.intToNetworkLong(buf, radius, 8);
      return this;
   }
   
   public RoverCommand headMove(boolean movePan, int panAngle, boolean moveTilt, int tiltAngle) {
      commandLength = 12;
      setType(HEAD_MOVE);
      buf[1] = (byte)(movePan ? 1 : 0);
      buf[2] = (byte)(moveTilt ? 1 : 0);
      ByteUtil.intToNetworkLong(buf, panAngle, 4);
      ByteUtil.intToNetworkLong(buf, tiltAngle, 8);
      return this;
   }
   
   public RoverCommand setAll(int mask, int motor0, int motor1, int servo0, int servo1, 
   int servo2, int servo3, int pan, int tilt) {
      commandLength = 40;
      setType(SET_ALL);
      
      ByteUtil.intToNetworkLong(buf, mask, 4);
      ByteUtil.intToNetworkLong(buf, motor0, 8);
      ByteUtil.intToNetworkLong(buf, motor1, 12);
      ByteUtil.intToNetworkLong(buf, servo0, 16);
      ByteUtil.intToNetworkLong(buf, servo1, 20);
      ByteUtil.intToNetworkLong(buf, servo2, 24);
      ByteUtil.intToNetworkLong(buf, servo3, 28);
      ByteUtil.intToNetworkLong(buf, pan, 32);
      ByteUtil.intToNetworkLong(buf, tilt, 36);
      return this;
   }
   
   public RoverCommand goTo(int dist, int angle, byte safetyLevel, boolean takePics) {
      commandLength = 12;
      setType(GOTO);
      buf[1] = safetyLevel;
      buf[2] = (byte)(takePics ? 0 : 1);
      ByteUtil.intToNetworkLong(buf, dist, 4);
      ByteUtil.intToNetworkLong(buf, angle, 8);
      return this;
   }
   
   public RoverCommand turnTo(int degrees) {
      commandLength = 8;
      setType(TURNTO);
      ByteUtil.intToNetworkLong(buf, degrees, 4);
      return this;
   }
   
   public RoverCommand killHL() {
      commandLength = 1;
      setType(KILL);
      return this;
   }
   
   public RoverCommand getUpdate() {
      commandLength = 1;
      setType(GET_UPDATE);
      return this;
   }
   
   public RoverCommand getCalibration() {
      commandLength = 1;
      setType(GET_CALIBRATION);
      return this;
   }
   
   public RoverCommand setCalibration(String cal) {
      commandLength = 1+cal.length();
      //make sure buffer is large enough
      if(cal.length() + 1 >= BUFFER_LENGTH) {
         BUFFER_LENGTH = cal.length() + 1;
         buf = new byte[BUFFER_LENGTH];
      }
      setType(SET_CALIBRATION);
      //copy into buffer
      byte [] bytes = cal.getBytes();
      for(int i=0; i<bytes.length; i++)
         buf[i+1] = bytes[i];
      return this;
   }
   
   public RoverCommand getScanList() {
      commandLength = 1;
      setType(GET_SCAN_LIST);
      return this;
   }
   
   public RoverCommand setScanList(String cal) {
      commandLength = 1+cal.length();
      //make sure buffer is large enough
      if(cal.length() + 1 >= BUFFER_LENGTH) {
         BUFFER_LENGTH = cal.length() + 1;
         buf = new byte[BUFFER_LENGTH];
      }
      setType(SET_SCAN_LIST);
      //copy into buffer
      byte [] bytes = cal.getBytes();
      for(int i=0; i<bytes.length; i++)
         buf[i+1] = bytes[i];
      return this;
   }
   
   public RoverCommand setLight(boolean on) {
      commandLength = 2;
      setType(SET_LIGHT);
      buf[1] = (byte) (on ? 1 : 0);
      return this;
   }
   
   public RoverCommand startTrack(int minY, int maxY, int minU, int maxU, 
      int minV, int maxV, int method, boolean movePan, boolean moveTilt) {
      commandLength = 9;
      setType(START_TRACK);
      buf[1] = (byte) minY;
      buf[2] = (byte) maxY;
      buf[3] = (byte) minU;
      buf[4] = (byte) maxU;
      buf[5] = (byte) minV;
      buf[6] = (byte) maxV;
      buf[7] = (byte) method;
      buf[8] = (byte) ((movePan ? 2 : 0) + (moveTilt ? 1 : 0));
      return this;
   }
   
   public RoverCommand stopTrack() {
      commandLength = 1;
      setType(STOP_TRACK);
      return this;
   }
   
   public RoverCommand doSpecial() {
      commandLength = 1;
      setType(DO_SPECIAL);
      return this;
   }
}
