/*
 * TurntableManual.java created on 24.01.2006
 * 
 * (c) alexander noehrer
 */
package fieldnose.hardware.turntable;

import java.util.Scanner;

import org.apache.log4j.Logger;
import org.apache.log4j.xml.DOMConfigurator;

import fieldnose.common.Constants;
import fieldnose.hardware.complete.UsbIO;
import fieldnose.hardware.exception.IOException;
import fieldnose.hardware.exception.IOExceptionType;

/**
 * @author alexander noehrer
 */
public class TurntableARCS implements Turntable {
  private static final Logger logger = Logger.getLogger(TurntableARCS.class);

  private static final boolean DEBUG_SENSOR = false;

  private static final int NUMBER_OF_STEPS = 570;
  private static final int ADJUSTMENT = 10;
  private static final int FORWARDS = 16;
  private static final int BACKWARDS = 18;
  private static final int SENSOR = 1;
  // private static final int HOME_SENSOR = 1;
  private static final int POS_SENSOR = 2;
  private static final int BOTH_SENSORS = 3;

  private UsbIO usbIO;
  private Position position;
  private int numberOfStepsFromXtoY;
  private int numberOfStepsFromYtoZ;
  private int numberOfStepsFromZtoX;

  /**
   * constructor
   */
  public TurntableARCS() {
    this.position = Position.UNKNOWN;
    this.numberOfStepsFromXtoY = NUMBER_OF_STEPS;
    this.numberOfStepsFromYtoZ = NUMBER_OF_STEPS;
    this.numberOfStepsFromZtoX = NUMBER_OF_STEPS * 2;
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#open(java.lang.String)
   */
  public void open(final String interfaceString) throws IOException {
    if (interfaceString.equalsIgnoreCase("auto")) {
      this.usbIO = UsbIO.getInstance();
    } else {
      this.usbIO = UsbIO.getInstance(Integer.parseInt(interfaceString));
    }
    this.usbIO.write(1, 0);
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#close()
   */
  public void close() {
    try {
      this.usbIO.write(0, 4);
      this.usbIO.close();
      this.usbIO = null;
    } catch (IOException e) {
      logger.error("", e);
    }
  }

  private void printSensorValue() throws IOException {
    if (DEBUG_SENSOR) {
      logger.debug("sensor: " + this.usbIO.read(SENSOR));
    }
  }

  private void turnHome() throws IOException {
    int direction = BACKWARDS;
    int count = this.numberOfStepsFromZtoX;
    if (this.position.equals(Position.UNKNOWN)) {
      this.printSensorValue();
      boolean atPosition = (this.usbIO.read(SENSOR) == BOTH_SENSORS);
      count = 0;
      while (!atPosition) {
        if (count > (this.numberOfStepsFromZtoX * 2)) {
          throw new IOException(IOExceptionType.TIME_OUT);
        }
        this.usbIO.write(0, direction);
        direction = direction ^ 1;
        if ((count % 10) == 0) {
          this.printSensorValue();
          atPosition = (this.usbIO.read(SENSOR) == BOTH_SENSORS);
        }
        count++;
      }
    } else {
      for (int i = 0; i < this.numberOfStepsFromZtoX; i++) {
        this.usbIO.write(0, direction);
        direction = direction ^ 1;
      }
      this.printSensorValue();
      while (this.usbIO.read(SENSOR) != BOTH_SENSORS) {
        if (count > (this.numberOfStepsFromZtoX * 2)) {
          throw new IOException(IOExceptionType.TIME_OUT);
        }
        this.usbIO.write(0, direction);
        count++;
        direction = direction ^ 1;
        this.printSensorValue();
      }
      this.printSensorValue();
      if (count > (NUMBER_OF_STEPS * 2)) {
        this.numberOfStepsFromZtoX = count - ADJUSTMENT;
      }
      logger.debug("count was " + count + ", adjusting numberOfStepsFromZtoX to " + this.numberOfStepsFromZtoX);
    }
  }

  private void turnNext(int direction) throws IOException {
    int numberOfSteps = this.numberOfStepsFromXtoY;
    if (this.position.equals(Position.Y)) {
      numberOfSteps = this.numberOfStepsFromYtoZ;
    }
    int count = numberOfSteps;
    for (int i = 0; i < numberOfSteps; i++) {
      this.usbIO.write(0, direction);
      direction = direction ^ 1;
    }
    this.printSensorValue();
    while (this.usbIO.read(SENSOR) != POS_SENSOR) {
      if (count > (numberOfSteps * 2)) {
        throw new IOException(IOExceptionType.TIME_OUT);
      }
      this.usbIO.write(0, direction);
      count++;
      direction = direction ^ 1;
      this.printSensorValue();
    }
    this.printSensorValue();
    if (this.position.equals(Position.X)) {
      this.numberOfStepsFromXtoY = count - ADJUSTMENT;
      logger.debug("count was " + count + ", adjusting numberOfStepsFromXtoY to " + this.numberOfStepsFromXtoY);
    } else {
      this.numberOfStepsFromYtoZ = count - ADJUSTMENT;
      logger.debug("count was " + count + ", adjusting numberOfStepsFromYtoZ to " + this.numberOfStepsFromYtoZ);
    }
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#turnToXPosition()
   */
  public void turnToXPosition() throws IOException {
    switch (this.position) {
      case X:
      case HOME:
        break;
      case Y:
        this.turnNext(BACKWARDS);
        break;
      case Z:
      case UNKNOWN:
        this.turnHome();
        break;
      default:
        throw new IllegalArgumentException();
    }
    this.position = Position.X;
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#turnToYPosition()
   */
  public void turnToYPosition() throws IOException {
    switch (this.position) {
      case X:
      case HOME:
        this.turnNext(FORWARDS);
        break;
      case Y:
        break;
      case Z:
        this.turnNext(BACKWARDS);
        break;
      case UNKNOWN:
        this.turnHome();
        this.turnNext(FORWARDS);
        break;
      default:
        throw new IllegalArgumentException();
    }
    this.position = Position.Y;
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#turnToZPosition()
   */
  public void turnToZPosition() throws IOException {
    switch (this.position) {
      case UNKNOWN:
        this.turnHome();
      case X:
      case HOME:
        this.turnNext(FORWARDS);
        this.turnNext(FORWARDS);
        break;
      case Y:
        this.turnNext(FORWARDS);
        break;
      case Z:
        break;
      default:
        throw new IllegalArgumentException();
    }
    this.position = Position.Z;
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#turnToNextPosition()
   */
  public void turnToNextPosition() throws IOException {
    switch (this.position) {
      case X:
        this.turnToYPosition();
        break;
      case Y:
        this.turnToZPosition();
        break;
      case Z:
      case UNKNOWN:
      case HOME:
        this.turnToXPosition();
        break;
      default:
        throw new IllegalArgumentException();
    }
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#turnTo(fieldnose.hardware.turntable.Position)
   */
  public void turnTo(final Position position) throws IOException {
    switch (position) {
      case X:
        this.turnToXPosition();
        break;
      case Y:
        this.turnToYPosition();
        break;
      case Z:
        this.turnToZPosition();
        break;
      case HOME:
        this.turnToXPosition();
        break;
      case UNKNOWN:
        // nothing to do
        break;
      default:
        throw new IllegalArgumentException();
    }
  }

  /*
   * (non-Javadoc)
   * @see fieldnose.hardware.turntable.Turntable#abort()
   */
  public void abort() {
    try {
      this.usbIO.write(0, 4);
    } catch (IOException e) {
      logger.error("", e);
    }
  }

  /**
   * @param args
   */
  public static void main(String[] args) {
    DOMConfigurator.configureAndWatch(Constants.LOGGING_CONFIG, Constants.LOGGING_CONFIG_CHECK_INTERVAL);
    boolean done = false;
    final Scanner scanner = new Scanner(System.in);
    final TurntableARCS turntable = new TurntableARCS();
    try {
      turntable.open("auto");
      while (!done) {
        String pos = scanner.next();
        if (pos.equalsIgnoreCase("x")) {
          turntable.turnToXPosition();
        } else if (pos.equalsIgnoreCase("y")) {
          turntable.turnToYPosition();
        } else if (pos.equalsIgnoreCase("z")) {
          turntable.turnToZPosition();
        } else if (pos.equalsIgnoreCase("r")) {
          turntable.turnToXPosition();
          turntable.turnToNextPosition();
          turntable.turnToNextPosition();
          turntable.turnToNextPosition();
        } else if (pos.equalsIgnoreCase("s")) {
          while (true) {
            turntable.printSensorValue();
          }
        } else {
          done = true;
        }
      }
    } catch (Exception e) {
      logger.error("", e);
    } finally {
      turntable.close();
    }
  }
}