package PER.rover.control;

import java.awt.image.BufferedImage;
import javax.imageio.ImageIO;
import java.io.ByteArrayInputStream;

/*
 * TrikebotController.java
 *
 * Created on April 12, 2002, 11:15 AM
 */

public class RoverController {
   public RoverState state, highLevelState;
   public ReceiveThread receive = null;
   
   private RoverCommand command;
   public Reliagram reliagram = null;
   
   // Socket read timeout
   public final static int READ_TIMEOUT = 5000;
   
   /** Creates new TrikebotController */
   public RoverController() {
      command = new RoverCommand();
      state = new RoverState();
      highLevelState = new RoverState();
   }
   
   //this command should always work
   public boolean initComm(String ipaddr) {
      if(reliagram != null)
         closeComm();
      reliagram = new Reliagram();
      if(reliagram.connect(ipaddr, 1701, READ_TIMEOUT)) {
         receive = new ReceiveThread(reliagram);
         return true;
      }else {
         reliagram.quit();
         reliagram = null;
         return false;
      }
   }
   
   public boolean closeComm() {
      if(reliagram != null) {
         reliagram.quit();
         receive.quit();
         reliagram = null;
         return true;
      }
      return false;
   }
   
   public boolean isConnected() {
      return reliagram != null;
   }
   
   public boolean initRobot() {
      int seqNum;
      synchronized (command) {
         command.initRover();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean killRobot() {
      return initRobot();
   }
   
   /**
    * Tries to take a picture.  With the creative web cam, 320x240 is the best 
    * resolution to take pictures at.  Getting the image from the camera only 
    * takes about 300ms.  If you go to a higher resolution, it will take the 
    * middle pixels from a 640x480 image.  For example, at 352x288, the field of
    * view is smaller than at 320x240 because of the way images are taken.
    * Because of a bug in the driver on the Stayton, all images at 160x120 are corrupted. 
    * @param pan The pan value in degrees at which to take the picture.
    * @param tilt The tilt vale in degrees at which to take the picture.
    * @param width The width of the image in pixels.
    * @param height The hiehgt of the image in pixels.
    * @return A new BufferedImage upon success.
    *         Upon failure, null is returned and <code>state</code> is set.
    */
   public BufferedImage takePicture(int pan, int tilt, int width, int height, boolean lightUV) {
      int seqNum, extraTime;
      synchronized(command) {
         command.takePicture(pan, tilt, width, height, lightUV);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      if(width > 320 || height > 240)
         extraTime = 3000;
      else
         extraTime = 1500;
      return takePictureCommon(seqNum, extraTime);
   }
   
   /**
    * This function is the same as the other takePicture function, but has the 
    * UV light off.
    */
   public BufferedImage takePicture(int pan, int tilt, int width, int height) {
      return takePicture(pan, tilt, width, height, false);
   }
   
   /**
    * This function returns the raw YUV that the camera returns.
    */
   public byte [] takeRawPicture(int pan, int tilt, int width, int height) {
      int seqNum;
      synchronized(command) {
         command.takeRawPicture(pan, tilt, width, height, false);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      Datapack dpack = null;
      if(seqNum > 0)
         dpack = reliagram.receive(seqNum, READ_TIMEOUT + 200);
      if(dpack == null || seqNum <= 0) {
         state.parsePacket(null);
         return null;
      }
      if(dpack.getLength() == RoverState.FULL_PACKET_LENGTH) {
         state.parsePacket(dpack.getData());
         return null;
      }
      return dpack.getData();
   }
   
   /**
    * Gives you the most recent picture that was taken by the rover.  
    * DriveToAction and TurnToAction take pictures while they are doing the 
    * Aciton.
    * @return The most recent picture the rover has taken.  If it is in the 
    * process of taking a picture, that picture is returned.
    */
   public BufferedImage takeRecentPicture() {
      int seqNum;
      synchronized(command) {
         command.takeRecentPicture();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return takePictureCommon(seqNum, 200);
   }
   
   //helper for taking pictures, allow extra time for pictures because it takes longer
   private BufferedImage takePictureCommon(int seqNum, int extraTime) {
      Datapack dpack = null;
      if(seqNum > 0)
         dpack = reliagram.receive(seqNum, READ_TIMEOUT + extraTime);
      if(dpack == null || seqNum <= 0) {
         state.parsePacket(null);
         return null;
      }
      if(dpack.getLength() == RoverState.FULL_PACKET_LENGTH) {
         state.parsePacket(dpack.getData());
         return null;
      }
      try {
         ByteArrayInputStream bais = new ByteArrayInputStream(dpack.getData(), 0, dpack.getLength());
         BufferedImage image = ImageIO.read(bais);
         bais.close();
         return image;
      } catch(Exception e) {
         return null;
      }
   }
   
   /**
    * Scans the area around where the rover is.  The scan is done starting with
    * the minPan and increments to maxPan.  Scans are done at positions from
    * minPan to maxPan inclusive.
    * @param tilt The tilt at which the scan is done.
    * @param minPan The pan angle at which the scan starts
    * @param maxPan The pan angle at which the scan ends.
    * @param step The angle difference between successive scan points.
    * @return An array of the scan points in terms of the range sensing data.
    *   There will be <code>(maxPan-minPan+step)/step</code> points in the array.
    */
   public int [] scan(int tilt, int minPan, int maxPan, int step) {
      if(tilt > 90 || tilt < -90 || minPan < -180 || maxPan > 180 ||
      minPan > maxPan || step <= 0) {
         state.setStatus(RoverState.BAD_INPUT);
         return null;
      }
      int seqNum;
      synchronized(command){
         command.scan(tilt, minPan, maxPan, step);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      Datapack dpack = null;
      //give it extra time because scanning can take a while
      int extraTime = Math.max(2000, 1000+(maxPan-minPan)*50/step);
      if(seqNum > 0)
         dpack = reliagram.receive(seqNum, READ_TIMEOUT+extraTime);
      if(dpack == null || seqNum <= 0 || dpack.getLength() == 0) {
         state.parsePacket(null);
         return null;
      }
      if(dpack.getData()[0] != RoverState.SUCCESS) { //error
         state.parsePacket(dpack.getData());
         return null;
      }
      byte [] data = dpack.getData();
      int length = dpack.getLength();
      if(length != ((maxPan-minPan)/step+2)){
         if(length == RoverState.FULL_PACKET_LENGTH)
            state.parsePacket(dpack.getData());
         else
            state.setStatus(RoverState.INVALID_PACKET_LENGTH);
         return null;
      }
      int [] scanVals = new int [length-1];
      for(int i=1; i<length; i++)
         scanVals[i-1] = ByteUtil.unsign(data[i]);
      return scanVals;
   }
   
   //go a certain number of cm, with wheels pointed at angle
   public boolean goTo(int dist, int angle) {
      return goTo(dist, angle, PER.rover.DriveToAction.CYCLE_SAFETY, true);
   }
   
   public boolean goTo(int dist, int angle, byte safetyLevel, boolean takePics) {
      int seqNum;
      synchronized (command) {
         command.goTo(dist, angle, safetyLevel, takePics);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doHighLevelReceive(seqNum);
   }
   
   //turn to a certain number of degrees
   public boolean turnTo(int degrees) {
      int seqNum;
      synchronized (command) {
         command.turnTo(degrees);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doHighLevelReceive(seqNum);
   }
   
   public boolean killHighLevel() {
      int seqNum;
      synchronized (command) {
         command.killHL();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doHighLevelReceive(seqNum);
   }
   
   public boolean updateHighLevel() {
      int seqNum;
      synchronized (command) {
         command.getUpdate();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doHighLevelReceive(seqNum);
   }
   
   public String getCalibration() {
      int seqNum;
      synchronized(command) {
         command.getCalibration();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      Datapack dpack = null;
      if(seqNum > 0)
         dpack = reliagram.receive(seqNum, READ_TIMEOUT);
      if(dpack == null || seqNum <= 0) {
         state.parsePacket(null);
         return null;
      }
      if(dpack.getLength() == RoverState.FULL_PACKET_LENGTH) {
         state.parsePacket(dpack.getData());
         return null;
      }
      if(dpack.getLength() == 1) {
         state.setStatus(ByteUtil.unsign(dpack.getData()[0]));
         return null;
      }
      return new String(dpack.getData(), 1, dpack.getLength()-1);
   }
   
   public boolean setCalibration(String cal) {
      int seqNum;
      synchronized (command) {
         command.setCalibration(cal);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public String getScanList() {
      int seqNum;
      synchronized(command) {
         command.getScanList();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      Datapack dpack = null;
      if(seqNum > 0)
         dpack = reliagram.receive(seqNum, READ_TIMEOUT);
      if(dpack == null || seqNum <= 0) {
         state.parsePacket(null);
         return null;
      }
      if(dpack.getLength() == RoverState.FULL_PACKET_LENGTH) {
         state.parsePacket(dpack.getData());
         return null;
      }
      if(dpack.getLength() == 1) {
         state.setStatus(ByteUtil.unsign(dpack.getData()[0]));
         return null;
      }
      return new String(dpack.getData(), 1, dpack.getLength()-1);
   }
   
   public boolean setScanList(String cal) {
      int seqNum;
      synchronized (command) {
         command.setScanList(cal);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean look(int pan, int tilt) {
      return headMove(true, pan, true, tilt);
   }
   
   public boolean setPan(int pan) {
      return headMove(true, pan, false, 0);
   }
   
   public boolean setTilt(int tilt) {
      return headMove(false, 0, true, tilt);
   }
   
   public boolean refresh() {
      return headMove(false, 0, false, 0);
   }
   
   public boolean headMove(boolean doPan, int pan, boolean doTilt, int tilt) {
      int seqNum;
      synchronized (command) {
         command.headMove(doPan, pan, doTilt, tilt);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean SleepServos(boolean pan, boolean tilt) {
      // fixed by rgockley 1-6-03 (had been SetParam)
      return true;//DirectSetParameters(false, 0, false, 0, false, 0, pan, 0, tilt, 0);
   }
   
   public boolean MoveHeadTo(int pan, int tilt) {return look(pan, tilt);}
   public boolean PanHeadTo(int pan) {return setPan(pan);}
   public boolean TiltHeadTo(int tilt) {return setTilt(tilt);}
   public boolean RefreshState() {return refresh();}
   
   //sets stuff directly, these values are in servo values, not degrees
   public boolean setAll(int mask, int motor0, int motor1, int servo0, int servo1,
   int servo2, int servo3, int pan, int tilt) {
      int seqNum;
      synchronized (command) {
         command.setAll(mask, motor0, motor1, servo0, servo1,
      servo2, servo3, pan, tilt);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean spin(int speed) {
      int seqNum;
      synchronized (command) {
         command.spin(speed);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean crab(int speed, int angle) {
      int seqNum;
      synchronized (command) {
         command.crab(speed, angle);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean quadTurn(int speed, int radius) {
      int seqNum;
      synchronized (command) {
         command.quadTurn(speed, radius);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean setLight(boolean on) {
      int seqNum;
      synchronized (command) {
         command.setLight(on);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean startTrack(int minY, int maxY, int minU, int maxU, 
      int minV, int maxV) {
      return startTrack(minY, maxY, minU, maxU, minV, maxV, 0, true, true);
   }
   
   public boolean startTrack(int minY, int maxY, int minU, int maxU, 
      int minV, int maxV, int method, boolean movePan, boolean moveTilt) {
      int seqNum;
      synchronized (command) {
         command.startTrack(minY, maxY, minU, maxU, minV, maxV, method, movePan, moveTilt);
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean stopTrack() {
      int seqNum;
      synchronized (command) {
         command.stopTrack();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   public boolean doSpecial() {
      int seqNum;
      synchronized (command) {
         command.doSpecial();
         seqNum = reliagram.send(command.getData(), command.getLength());
      }
      return doReceive(seqNum);
   }
   
   private boolean doReceive(int seqNum) {
      if(seqNum < 0) {
         state.parsePacket(null);
         return false;
      }
      Datapack dpack = reliagram.receive(seqNum);
      if(dpack == null)
         return state.parsePacket(null);
      else
         return state.parsePacket(dpack.getData());
   }
   
   private boolean doHighLevelReceive(int seqNum) {
      if(seqNum < 0) {
         highLevelState.parsePacket(null);
         //System.out.println("seq num < 0");
         return false;
      }
      Datapack dpack = reliagram.receive(seqNum);
      if(dpack == null)
         return highLevelState.parsePacket(null);
      else
         return highLevelState.parsePacket(dpack.getData());
   }
}