/**
   @file

   Implementation of an example interface to a simple robot with two
   independent axies. The robot has four inputs whish are connected to
   the lower nible on IO port "interface_ioport". The bits are
   connected in this way (the signals a high-active):

   Bit 0: Drive Up
     wires: switch to GND: yellow-green;    +24V: gray-black

   Bit 1: Dirve down
     wires: switch to GND: red-green;       +24V: orange-black

   Bit 2: Dirve right
     wires: switch to GND: green-red;       +24V: yellow-black

   Bit 3: Dirve left
     wires: switch to GND: white-red;       +24V: red-blue

     power wires:     GND: blue;            +24V: red

   This interface assumes a linear dependency betwenn the coverence of
   distance and moving time. The 0,0 coordinates is left,bottom. 

   User functions are:

   interface_init() - Initialze the interface and drive the robot to
   the start position (X=undefined; Y=0).

   interface_driveto(int x, int y) - Drive the robot to the absolute
   coordinates x,y.
*/

/*
  Copyright (c) Andreas Hofmeier
  (www.an-h.de, www.an-h.de.vu, www.lgut.uni-bremen.de/an-h/)
  
  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/


#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <asm/io.h>
#include <time.h>
#include <sys/wait.h>


/** Time it rakes to drive to robot form y_min to y_max */
#define interface_ymax_time 20000 // at 6 bar
/** Time it rakes to drive to robot form x_min to x_max */
#define interface_xmax_time 20000 // 34670
/** Maximal Y value (cosidered as 100 percent) */
#define interface_ymax 100
/** Maximal X value (cosidered as 100 percent) */
#define interface_xmax 100
/** IO Port to which lowest nible to robot is connected */
#define interface_ioport 0x378

/** current X possition of robot (global) */
int interface_x;
/** current X possition of robot (global) */
int interface_y;

/** mode of interface:
    0: normal,
    1: simulation (do all except to drive the robot),
    2: simulation with position-output
    3: Blocked: Do nothing */
int interface_mode;


/**
   Read a byte from an IO port
   @param addr (int): address of port to read
   @return (unsigned char) read byte
*/
unsigned char input(int addr) {
  if (interface_mode == 0) {
    return inb(addr);
  }
  return 255;
}


/**
   Write a byte to an IO port
   @param addr (int): address of port to write onto
   @param out (unsigned char): byte to write
   @return (unsigned char) written byte
*/
unsigned char output(int addr, unsigned char out) {
  if (interface_mode == 0) {
    outb(out, addr);
    return out;
  }
  return out;
}


/**
   Wait a specified number of milliseconds
   @param msec (int): milliseconds to wait
*/
void msleep(int msec) {
  int sec = msec / 1000;
  msec = msec - sec * 1000;
  sleep(sec);
  usleep(msec * 1000);
}


/**
   Get to highest number out of two input numbers
   @param a (int) first input number
   @param b (int) second input number
   @return (int) the highest of the input numbers
 */
int getmax(int a, int b) {
  if (a > b) {
    return a;
  } else {
    return b;
  }
}


/**
   Get to lowest number out of two input numbers
   @param a (int) first input number
   @param b (int) second input number
   @return (int) the lowest of the input numbers
 */
int getmin(int a, int b) {
  if (a < b) {
    return a;
  } else {
    return b;
  }
}


/**
   Drive the robot in a specified direction. Any axies can be zero,
   greater than zero or less than zero, in this cases the robot will
   not driven, driven to increase to position (up[v,y] or right[h,x])
   and decrease the position (down[-v,-y] or left[-h,-x]).

   @param h (int) horizontal or X axias.
   @param v (int) vertical or Y axias.
*/
void interface_drive(int h, int v) {
  unsigned char buf;

  buf = 0;
  /* Y Axies */
  if (v > 0) {
    buf = buf | 1;
  }
  if (v < 0) {
    buf = buf | 2;
  }
  /* X Axies */
  if (h > 0) {
    buf = buf | 4;
  }
  if (h < 0) {
    buf = buf | 8;
  }

  /* Apply */
  buf = buf | (input(interface_ioport) & 240);
  output(interface_ioport, buf);
}


/**
   Initialze the interface and drive the robot to the start position.
 */
void interface_init(int mode) {
  if ((mode < 0) || (mode > 3)) {
    mode = 2;
  }
  interface_mode = mode;

  // enable access to IO ports (need root previleges)
  if (interface_mode == 0) {
    if (iopl(3) < 0) {
      perror("iopl()");
      //      exit(1);
      interface_mode = 2;
    }
  }

  switch (interface_mode) {
  case 0:
    fprintf(stderr, "Interface full active.\n");
    break;
  case 1:
    fprintf(stderr, "Interface disabled.\n");
    break;
  case 2:
    fprintf(stderr, "Interface disabled: position output\n");
    break;
  }

  fprintf(stderr, "Initialze interface (moving to x=50,y=0)...\n");

  // drive to the coordinates ?, 0
  // x will not be driven, because it is too noisy and slowly
  interface_drive(0, -1);
  // code for "normal" drive-time for 0,0
  /*  if (interface_xmax_time > interface_ymax_time) {
    msleep(interface_xmax_time + interface_xmax_time/5);
  } else {
    msleep(interface_ymax_time + interface_ymax_time/5);
    } */
  // code for Y-drive-time only
  msleep(interface_ymax_time + interface_ymax_time/5);
  interface_drive(0, 0);

  // set coordinates to ?, 0
  interface_x = 50;
  interface_y = 0;
}


/**
   Drive to robot to absolute coordinates
   @param x (int): absolute x coordinate
   @param y (int): absolute y coordinate
*/
void interface_driveto(int x, int y) {
  int i;

  // absulute coordinates are in range?
  if (x > interface_xmax) {
    x = interface_xmax;
  }
  if (x < 0) {
    x = 0;
  }
  if (y > interface_ymax) {
    y = interface_ymax;
  }
  if (y < 0) {
    y = 0;
  }

  // calculate relative coordinates
  x = (x - interface_x);
  y = (y - interface_y);

  // store new coordinates
  interface_x += x;
  interface_y += y;

  //  if (interface_mode == 2) {
    fprintf(stderr, "x = %03d ; y = %03d\n",
	    interface_x, interface_y);
    //  }

  // if any change has to be done...
  if ((x != 0) || (y != 0)) {
    // convert the relative distance into a time in which this
    // distance is covered
    x = (int) ((double)
	       (((double) x * (double) interface_xmax_time)
		/ (double) interface_xmax));
    y = (int) ((double)
	       (((double) y * (double) interface_ymax_time)
		/ (double) interface_ymax));


    // if the drive-time for both axies is not equal, drive the
    // greater time as long as the times are equal
    if ((x != 0) && (y != 0)) {
      interface_drive(x, y);
      msleep(getmin(abs(x), abs(y)));
      interface_drive(0, 0);

      i = getmin(abs(x), abs(y));
      x = x/abs(x) * (abs(x) - i);
      y = y/abs(y) * (abs(y) - i);
    }

    // if there is any time left, drive these
    if ((x != 0) || (y != 0)) {
      interface_drive(x, y);
      msleep(getmax(abs(x), abs(y)));
      interface_drive(0, 0);
    }
  }
}


/** Stop the interface, switch all off. */
void interface_stop() {
  fprintf(stderr, "interface_stop()\n");
  // block the interface
  interface_mode = 3;
  // switch all off.
  //  interface_drive(0, 0);

  //  interface_drive do not work, because "interface_mode = 3"
  //  switches to IO-access off. For this reason, the functions were
  //  directly used.
  outb((inb(interface_ioport) & 240), interface_ioport);
}