/*
  =================================================
    LIPS slit stage control program              
                 slitcont.c 
      for Linux (Vine 2.1.5; Kernel 2.2.19)
          Windows/Dos
      Ver 1.0       2001. 1.19   H.Akitaya    
      Ver 1.1       2001. 1.23   H.Akitaya    
      Ver 1.2       2001. 1.24   H.Akitaya    
      Ver 1.3       2001. 2.07   H.Akitaya    
      Ver 1.40      2001. 6.17   H.Akitaya
          Serial <-> Parralel circuit test
      Ver 1.40      2001. 6.24   H.Akitaya
          Serial <-> Parralel circuit (16bit PIO)
      Ver 1.50      2001.10.19   H.Akitaya
          Controller Rack and PIO Board 
          Guide Mirror control
      Ver 1.51      2001.10.23   H.Akitaya
          Controller Rack and PIO Board 
      Ver 1.6       2001.10.29   H.Akitaya
      Ver 1.7       2001.12.26   H.Akitaya
          GT Turret controler
      Ver 1.71      2001.12.31   H.Akitaya
      Ver 1.72      2002. 3. 2   H.Akitaya
      Ver 1.8       2003. 3.27   K.Homma
          Slitviewer and Cross Disperser controler
      Ver 1.81      2003. 3.28   K.Matsuda
  =================================================
*/

/*
  to complile...
       Linux : gcc -DLINUX -O2 -lm slitcont.c lpio_lnx.c lipspio.c -o slitcont
               su;
               chown root slitcont; chmod +s slitcont
   Dos/Win   : compile slitcont.c and lpio_dos.c with "#DEFINE DOS"
*/

#include<stdio.h>
#include<string.h>
#include<math.h>

#include "lipspio.h"

#define TRUE 0
#define SW_ON 1
#define SW_OFF 0
#define MAXNDIAPH 40
#define DIAPHDEFFILE "diaphdef.dat"
#define GDMRDEFFILE "gdmrdef.dat"
#define TRRTDEFFILE "trrtdef.dat"
#define SLTVDEFFILE "sltvdef.dat"
#define CRDSDEFFILE "crdsdef.dat"
#define TRRT_BACKPULSE 150
#define SLTV_BACKPULSE 150
#define CRDS_BACKPULSE 150

struct name_coord {
  char name[30];
  long coord;
  char comment[80];
};

int port_initialize( void );
int inst_initialize( unsigned long *p_status , unsigned int p_width );
int set_slit_org( unsigned long status , unsigned int p_width );
int setslit( long coord_n, long *p_pos, unsigned long status,
	    unsigned int p_width);
int move_slit_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		 unsigned int p_width );
int move_slit_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		 unsigned int p_width );

int set_gdmr_org( unsigned long status , unsigned int  p_width );
int set_gdmr_out( unsigned long status , unsigned int p_width_fin );
int move_gdmr_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int move_gdmr_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int set_trrt_org( unsigned long status , unsigned int  p_width );
int set_sltv_org( unsigned long status , unsigned int  p_width );
int set_crds_org( unsigned long status , unsigned int  p_width );
int move_trrt_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int move_trrt_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int setsltv( long coord_n, long *p_pos, unsigned long status,
	    unsigned int p_width);
int move_sltv_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int move_sltv_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int move_crds_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );
int move_crds_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width );

void p_sensor( int on_off , unsigned long *p_status);

void showhelp( void );
void start_msg( void );
void read_diaphdef( int *ndiaph, struct name_coord *diaphdef, char filename[] );
long read_gdmrdef( void );
void show_diaphdef( int ndiaph, struct name_coord diaphdef[] );


/*              */
/* main routine */
/*              */

main()
{
  unsigned int o_data,slit_pwidth, gdmr_pwidth, trrt_pwidth, sltv_pwidth, crds_pwidth;
  unsigned long outdata, indata, status;
  long pos, pulse_n, coord_n, pos_gdmr, gdmr_inpos, pos_trrt, pos_sltv, pos_crds;
  int error, i, ndiaph, ntrrt, nsltv, ncrds;
  char com_line[255], buf[255];

  struct name_coord diaphdef[MAXNDIAPH];
  struct name_coord trrtdef[MAXNDIAPH];
  struct name_coord sltvdef[MAXNDIAPH];
  struct name_coord crdsdef[MAXNDIAPH];

    /*
     start message
     */

  start_msg();
  
  /*
    initialize
  */
  
  pos = 0;
  pos_gdmr = 0; 
  pos_trrt = 0;
  pos_sltv = 0;
  pos_crds = 0;
  slit_pwidth = SLIT_PULSEWIDTH;
  gdmr_pwidth = GDMR_PULSEWIDTH;
  trrt_pwidth = TRRT_PULSEWIDTH;
  sltv_pwidth = SLTV_PULSEWIDTH;
  crds_pwidth = CRDS_PULSEWIDTH;
  port_initialize();              /* port initialization */
  inst_initialize( &status , slit_pwidth );    /* stage intialization */
  read_diaphdef( &ndiaph, diaphdef , DIAPHDEFFILE);
             /* define diaphragm names and coordinates */
  gdmr_inpos = read_gdmrdef();
             /* define guide-mirror in coordinate */
  read_diaphdef( &ntrrt, trrtdef , TRRTDEFFILE);
             /* define GT turret componet names and coordinates */
  read_diaphdef( &nsltv, sltvdef , SLTVDEFFILE);
             /* define slitviewer componet names and coordinates */
  read_diaphdef( &ncrds, crdsdef , CRDSDEFFILE);
             /* define cross disperser componet names and coordinates */
  printf("Stage position : %i\n", pos );
  printf("Pulse width : %i micro sec.\n", slit_pwidth );
  
  /*
    main routine
  */
  
  while(1){
    printf( ">" );
    fgets( com_line, 255, stdin ); 
    
    if ( strncmp( com_line, "mv", 2) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 3] ) != '\0' ) i++;
      pulse_n = atol( buf );
      if( pulse_n > 0 ) move_slit_cw( pulse_n, &pos, status, slit_pwidth );
      if( pulse_n < 0 ) move_slit_ccw( labs(pulse_n), &pos, status, slit_pwidth );
      printf("Stage position : %i\n", pos );
    }
 
    if ( strncmp( com_line, "set", 3) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 4] ) != '\0' ) i++;
      coord_n = atol( buf );
      setslit(  coord_n, &pos, status, slit_pwidth );
      printf("Stage position : %i\n", pos );
    }
    if ( strncmp( com_line, "dset", 4) == TRUE ){
      if( ndiaph == 0 ){
	printf( "No diaphragm is defined !!\n" );
      }else{
	i=0;
	while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
	for( i=1; i <= ndiaph; i++){
	  if( strncmp( buf, diaphdef[i].name, 4 ) ==  TRUE ){
	    printf( "%s %li %s\n", diaphdef[i].name,
		    diaphdef[i].coord, diaphdef[i].comment );
	    setslit( diaphdef[i].coord, &pos, status, slit_pwidth );
	    break;
	  }
	}
      }
    }

    if ( strncmp( com_line, "tset", 4) == TRUE ){
      if( ntrrt == 0 ){
	printf( "No component is defined !!\n" );
      }else{
	i=0;
	while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
	for( i=1; i <= ntrrt; i++){
	  if( strncmp( buf, trrtdef[i].name, 2 ) ==  TRUE ){
	    printf( "%s %li %s\n", trrtdef[i].name,
		    trrtdef[i].coord, trrtdef[i].comment );
	    pos_trrt = 0;
	    error = set_trrt_org( status, trrt_pwidth );
	    printf("GT Turret position : %i\n", pos_trrt );
	    move_trrt_cw( trrtdef[i].coord, &pos_trrt, status, trrt_pwidth );
	    printf("GT Turret position : %i\n", pos_trrt );
	    break;
	  }
	}
      }
    }
    
    if ( strncmp( com_line, "sset", 4) == TRUE ){
      if( nsltv == 0 ){
	printf( "No component is defined !!\n" );
      }else{
	i=0;
	while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
	for( i=1; i <= nsltv; i++){
	  if( strncmp( buf, sltvdef[i].name, 3 ) ==  TRUE ){
	    printf( "%s %li %s\n", sltvdef[i].name,
		    sltvdef[i].coord, sltvdef[i].comment );
	    setsltv( sltvdef[i].coord, &pos_sltv, status, sltv_pwidth );
	    break;
	  }
	}
      }
    }
    
    if ( strncmp( com_line, "cset", 4) == TRUE ){
      if( ncrds == 0 ){
	printf( "No component is defined !!\n" );
      }else{
	i=0;
	while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
	for( i=1; i <= ncrds; i++){
	  if( strncmp( buf, crdsdef[i].name, 3 ) ==  TRUE ){
	    printf( "%s %li %s\n", crdsdef[i].name,
		    crdsdef[i].coord, crdsdef[i].comment );
	    pos_crds = 0;
	    error = set_crds_org( status, crds_pwidth );
	    printf("Cross Disperser position : %i\n", pos_crds );
	    move_crds_cw( crdsdef[i].coord, &pos_crds, status, crds_pwidth );
	    printf("Cross Disperser position : %i\n", pos_crds );
	    break;
	  }
	}
      }
    }
   
    if ( strncmp( com_line, "slpw", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      slit_pwidth = atol( buf );
      printf("Slit Stage Motor Pulse width (micro sec.) : %i\n", slit_pwidth );
    }
    if ( strncmp( com_line, "gdpw", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      gdmr_pwidth = atol( buf );
      printf("Guide Mirror Motor Pulse width (micro sec.) : %i\n", gdmr_pwidth );
    }
    if ( strncmp( com_line, "trpw", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      gdmr_pwidth = atol( buf );
      printf("GT Turret Motor Pulse width (micro sec.) : %i\n", trrt_pwidth );
    }
    if ( strncmp( com_line, "svpw", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      sltv_pwidth = atol( buf );
      printf("Slitviewer Motor Pulse width (micro sec.) : %i\n", sltv_pwidth );
    }
    if ( strncmp( com_line, "cdpw", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      crds_pwidth = atol( buf );
      printf("Cross Disperser Motor Pulse width (micro sec.) : %i\n", crds_pwidth );
    }
    if ( strncmp( com_line, "slorg", 5 ) == TRUE ){
      error = set_slit_org( status, slit_pwidth );
      pos = 0;
      printf("Stage position : %i\n", pos );
  
    }
    /*
      if ( strncmp( com_line, "full", 4 ) == TRUE ){
      status &= ~PO_SLIT_FULLHALF;
      pioout( status );
      printf( "motor speed : full step\n" );
      }
      if ( strncmp( com_line, "half", 4 ) == TRUE ){
      status |= PO_SLIT_FULLHALF;
      pioout( status );
      printf( "motor speed : half step\n" );
      }
    */
    if ( strncmp( com_line, "gdorg", 5) == TRUE ){
      pos_gdmr = 0;
      error = set_gdmr_org( status, gdmr_pwidth );
      printf("Guide-mirror position : %i\n", pos_gdmr );
    }
    if ( strncmp( com_line, "gdout", 5) == TRUE ){
      pos_gdmr = 0;
      error = set_gdmr_out( status, gdmr_pwidth );
      printf("Guide-mirror position : %i\n", pos_gdmr );
    }
    if ( strncmp( com_line, "gdin", 4) == TRUE ){
      pulse_n = gdmr_inpos - pos_gdmr ;
      move_gdmr_ccw( (-1.0)*pulse_n, &pos_gdmr, status, gdmr_pwidth );
      printf("Guide-mirror position : %i\n", pos_gdmr );
    }
    if ( strncmp( com_line, "trorg", 5) == TRUE ){
      pos_trrt = 0;
      error = set_trrt_org( status, trrt_pwidth );
      printf("GT Turret position : %i\n", pos_trrt );
    }
    if ( strncmp( com_line, "trmv", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      pulse_n = atol( buf );
      if( pulse_n > 0 ) move_trrt_cw( pulse_n, &pos_trrt, status, trrt_pwidth );
      if( pulse_n < 0 ) move_trrt_ccw( labs(pulse_n), &pos_trrt, status, trrt_pwidth );
      printf("GT Turret position : %i\n", pos_trrt );
    }
    if ( strncmp( com_line, "svorg", 5) == TRUE ){
      pos_sltv = 0;
      error = set_sltv_org( status, sltv_pwidth );
      printf("Slitviewer position : %i\n", pos_sltv );
    }
    if ( strncmp( com_line, "svmv", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      pulse_n = atol( buf );
      if( pulse_n > 0 ) move_sltv_cw( pulse_n, &pos_sltv, status, sltv_pwidth );
      if( pulse_n < 0 ) move_sltv_ccw( labs(pulse_n), &pos_sltv, status, sltv_pwidth );
      printf("Slitviewer position : %i\n", pos_sltv );
    }
    if ( strncmp( com_line, "cdorg", 5) == TRUE ){
      pos_crds = 0;
      error = set_crds_org( status, crds_pwidth );
      printf("Cross Disperser position : %i\n", pos_crds );
    }
    if ( strncmp( com_line, "cdmv", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      pulse_n = atol( buf );
      if( pulse_n > 0 ) move_crds_cw( pulse_n, &pos_crds, status, crds_pwidth );
      if( pulse_n < 0 ) move_crds_ccw( labs(pulse_n), &pos_crds, status, crds_pwidth );
      printf("Cross Disperser position : %i\n", pos_crds );
    }
    if ( strncmp( com_line, "gdmv", 4) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 5] ) != '\0' ) i++;
      pulse_n = atol( buf );
      if( pulse_n > 0 ) move_gdmr_cw( pulse_n, &pos_gdmr, status, trrt_pwidth );
      if( pulse_n < 0 ) move_gdmr_ccw( labs(pulse_n), &pos_gdmr, status, trrt_pwidth );
      printf("Guide Mirror position : %i\n", pos_gdmr );
    }

    if ( strncmp( com_line, "hcton", 5 ) == TRUE ){
      status |= PO_HCTON;
      pioout( status );
      printf( "HCT lamp is turned on\n" );
    }
    if ( strncmp( com_line, "hctoff", 6 ) == TRUE ){
      status &= ~PO_HCTON;
      pioout( status );
      printf( "HCT lamp is turned off\n" );
    }
    if ( strncmp( com_line, "slhoff", 6 ) == TRUE ){
      status &= ~PO_SLIT_HOFF;
      pioout( status );
      printf( "motor current : off\n" );
    }
    if ( strncmp( com_line, "slhon", 5 ) == TRUE ){
      status |= PO_SLIT_HOFF;
      pioout( status );
      printf( "motor current : on\n" );
    }
    if ( strncmp( com_line, "gdhoff", 6 ) == TRUE ){
      status &= ~PO_GDMR_HOFF;
      pioout( status );
      printf( "motor current : off\n" );
    }
    if ( strncmp( com_line, "gdhon", 5 ) == TRUE ){
      status |= PO_GDMR_HOFF;
      pioout( status );
      printf( "motor current : on\n" );
    }
    if ( strncmp( com_line, "trhoff", 6 ) == TRUE ){
      status &= ~PO_TRRT_HOFF;
      pioout( status );
      printf( "GT turret motor current : off\n" );
    }
    if ( strncmp( com_line, "trhon", 5 ) == TRUE ){
      status |= PO_TRRT_HOFF;
      pioout( status );
      printf( "GT turret motor current : on\n" );
    }
    if ( strncmp( com_line, "svhoff", 6 ) == TRUE ){
      status &= ~PO_SLTV_HOFF;
      pioout( status );
      printf( "Slitviewer motor current : off\n" );
    }
    if ( strncmp( com_line, "svhon", 5 ) == TRUE ){
      status |= PO_SLTV_HOFF;
      pioout( status );
      printf( "Slitviewer motor current : on\n" );
    }
    if ( strncmp( com_line, "cdhoff", 6 ) == TRUE ){
      status &= ~PO_CRDS_HOFF;
      pioout( status );
      printf( "Cross Disperser motor current : off\n" );
    }
    if ( strncmp( com_line, "cdhon", 6 ) == TRUE ){
      status |= PO_CRDS_HOFF;
      pioout( status );
      printf( "Cross Disperser motor current : on\n" );
    }
	if ( strncmp( com_line, "ddef", 4 ) == TRUE ){
      show_diaphdef( ndiaph, diaphdef );
    }
	if ( strncmp( com_line, "tdef", 4 ) == TRUE ){
      show_diaphdef( ntrrt, trrtdef );
    }
	if ( strncmp( com_line, "sdef", 4 ) == TRUE ){
      show_diaphdef( nsltv, sltvdef );
    }
	if ( strncmp( com_line, "cdef", 4 ) == TRUE ){
      show_diaphdef( ncrds, crdsdef );
    }

    if ( strncmp( com_line, "help", 4 ) == TRUE ||
	 strncmp( com_line, "?", 1 ) == TRUE  ){
      showhelp();
    }
    
    if ( strncmp( com_line, "quit", 4 ) == TRUE || 
	 strncmp( com_line, "exit", 4 ) == TRUE ||
	 strncmp( com_line, "q", 1 ) == TRUE ){
      printf("Good bye!!\n");
      exit(0);
    }
    if ( strncmp( com_line, "serout", 6) == TRUE ){
      i=0;
      while( ( buf[i] = com_line[i + 7] ) != '\0' ) i++;
       outdata = atoi( buf );
      pioout(  outdata );
      printf("Parallel Output : %x\n", outdata );

    }
    if ( strncmp( com_line, "serin", 5) == TRUE ){
      indata = pioin();
      printf("Parallel Input : %x\n", indata );
    }
    if ( strncmp( com_line, "out", 3 ) == TRUE ){
      printf("Diaphragm -> Slit");
      setslit( diaphdef[1].coord, &pos, status, slit_pwidth );
      printf("Guide-mirror -> Out");
      pos_gdmr = 0;
      error = set_gdmr_out( status, gdmr_pwidth );
      printf("Guide-mirror position : %i\n", pos_gdmr );
      printf("Completed !!");
    }
    if ( strncmp( com_line, "in", 2 ) == TRUE ){
      printf("Guide-mirror -> In");
      pulse_n = gdmr_inpos - pos_gdmr ;
      move_gdmr_ccw( (-1.0)*pulse_n, &pos_gdmr, status, gdmr_pwidth );
      printf("Guide-mirror position : %i\n", pos_gdmr );
      printf("Diaphragm -> Hole");
      setslit( diaphdef[2].coord, &pos, status, slit_pwidth );
      printf("Completed !!");
    }
    if ( strncmp( com_line, "comp", 4 ) == TRUE ){
      status |= PO_HCTON;
      pioout( status );
      printf( "HCT lamp is turned on\n" );
      strcpy( buf, "hct" );
      for( i=1; i <= ntrrt; i++){
	if( strncmp( buf, trrtdef[i].name, 2 ) ==  TRUE ){
	  printf( "%s %li %s\n", trrtdef[i].name,
		  trrtdef[i].coord, trrtdef[i].comment );
	  pos_trrt = 0;
	  error = set_trrt_org( status, trrt_pwidth );
	  printf("GT Turret position : %i\n", pos_trrt );
	  move_trrt_cw( trrtdef[i].coord, &pos_trrt, status, trrt_pwidth );
	  printf("GT Turret position : %i\n", pos_trrt );
	  break;
	}
      }
    }
    if ( strncmp( com_line, "obs", 3 ) == TRUE ){
      status &= ~PO_HCTON;
      pioout( status );
      printf( "HCT lamp is turned off\n" );
      strcpy( buf, "hole" );
      for( i=1; i <= ntrrt; i++){
	if( strncmp( buf, trrtdef[i].name, 2 ) ==  TRUE ){
	  printf( "%s %li %s\n", trrtdef[i].name,
		  trrtdef[i].coord, trrtdef[i].comment );
	  pos_trrt = 0;
	  error = set_trrt_org( status, trrt_pwidth );
	  printf("GT Turret position : %i\n", pos_trrt );
	  move_trrt_cw( trrtdef[i].coord, &pos_trrt, status, trrt_pwidth );
	  printf("GT Turret position : %i\n", pos_trrt );
	  break;
	}
      }
    }

  }
}

int inst_initialize( unsigned long *p_status , unsigned int p_width )
{
  int c;
  unsigned long status, error;

  printf("Initializing stage ...   ");
  status = 0x0000 ;
  /* guide mirror */
  status |= PO_GDMR_CWP | PO_GDMR_CCWP | PO_GDMR_HOFF ; 
  /* slit stage */
  status |= PO_SLIT_CWP | PO_SLIT_CCWP | PO_SLIT_HOFF ; 
  /* GT turret */
  status |= PO_TRRT_CWP | PO_TRRT_CCWP | PO_TRRT_HOFF ; 
  /*    status |= PO_TRRT_HOFF ;*/
  /* Slitviewer */
  status |= PO_SLTV_CWP | PO_SLTV_CCWP | PO_SLTV_HOFF ; 
  /* Cross Disperser */
  status |= PO_CRDS_CWP | PO_CRDS_CCWP | PO_CRDS_HOFF ; 

  pioout( status );
  printf("OK status = 0x%x\n", status);
  
  printf("Set motors to the origin (y/n)?" );
  c = getchar();
  if ( c == 'y' ){
    printf("Setting slit stage to the origin ...\n" );
    error = set_slit_org( status , p_width );
    if( error == -1 ){
      printf("ERROR!\n");
    }
    printf("OK\n");
    
    printf("Seting Gdmr position to the origin ...\n" );
    error = set_gdmr_org( status , p_width );
    if( error == -1 ){
      printf("ERROR!\n");
    }
    printf("OK\n");

    printf("Seting  GT turret position to the origin ...\n" );
    error = set_trrt_org( status , p_width );
    if( error == -1 ){
      printf("ERROR!\n");
    }
    printf("OK\n");

    printf("Seting  Slitviewer position to the origin ...\n" );
    error = set_sltv_org( status , p_width );
    if( error == -1 ){
      printf("ERROR!\n");
    }
    printf("OK\n");
  }else{
    printf("Skiped ....\n");
  }
  
  *p_status = status;
  return( 1 );
}

/*
  set stage to org. position
*/

int set_slit_org( unsigned long status , unsigned int  p_width )
{
  unsigned int n_limit;
  
  /* find CCW-LIMIT (FULL STEP)*/
  printf("Finding the CCW Limit  ...   ");
  n_limit = 30000;     /* timeout pulse */
  while( ( pioin() & PI_SLIT_CCWLS ) == PI_SLIT_CCWLS ){
    pioout( status & ~PO_SLIT_CCWP );
    p_wait( p_width );
    pioout( status | PO_SLIT_CCWP );
    p_wait( p_width );
    if ( --n_limit == 0) return(-1); 
  }
  printf("Found!!\n");
  p_wait( p_width * 100 );
  /* find ORG (HALF STEP) */
  printf("Finding the origin  ...   ");
  status |= PO_SLIT_FULLHALF; /* half step */
  n_limit = 10000;     /* timeout pulse */
    while( ( pioin() & PI_SLIT_ORG ) == PI_SLIT_ORG ){
      pioout( status & ~PO_SLIT_CWP);
      p_wait( p_width );
      pioout( status | PO_SLIT_CWP );
      p_wait( p_width );
      if ( --n_limit == 0) return(-1); 
    }
    printf("Found!!\n");
    return(0);
}
/*
  set slit to absolute coordinate (coord_n)
*/
int setslit( long coord_n, long *p_pos, unsigned long status,
	    unsigned int p_width)
{
  long pulse_n;
  pulse_n = coord_n - *p_pos;
  if( pulse_n > 0 ) move_slit_cw( pulse_n, p_pos, status, p_width );
  if( pulse_n < 0 ) move_slit_ccw( labs(pulse_n), p_pos, status, p_width );
  return(1);
}  

/*
  move slit pulse_n steps in CW direction
*/
int move_slit_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		 unsigned int p_width )
{

  unsigned long i;
  unsigned long signal, speed;
  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  if( (status & PO_SLIT_FULLHALF) == PO_SLIT_FULLHALF ){
    speed = 1; /* half step */
  }else{
    speed = 2; /* full step */
  }

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_SLIT_CWLS ) != PI_SLIT_CWLS ){
      printf("CW Limit !! +%i steps moved\n", (i * speed) );
      *p_pos += ( i * speed );
      return(0);
    }
    pioout( status & ~PO_SLIT_CWP);
    p_wait( p_width );
    pioout( status | PO_SLIT_CWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. +%i steps moved\n", (i * speed ) );
  *p_pos += (i * speed) ;
  return(0);
}
/*
  move slit pulse_n steps in CCW direction
*/
int move_slit_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{
  unsigned long i;
  unsigned long signal, speed;
  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  if( (status & PO_SLIT_FULLHALF) == PO_SLIT_FULLHALF ){
    speed = 1; /* half step */
  }else{
    speed = 2; /* full step */
  }

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( ( signal & PI_SLIT_CCWLS ) != PI_SLIT_CCWLS ){
      printf("CCW Limit !! -%i steps moved\n", ( i * speed ) );
      *p_pos -= i * speed;
      return(0);
    }
    pioout( status & ~PO_SLIT_CCWP);
    p_wait( p_width );
    pioout( status | PO_SLIT_CCWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. -%i steps moved\n", ( i * speed )  );
  *p_pos -= ( i * speed );
  return(0);
}

/*--------------------
  GUIDE MIRROR
  --------------------*/

/*
  set Guide Mirror to org. position
*/

int set_gdmr_org( unsigned long status , unsigned int  p_width )
{
  unsigned int n_limit;

  p_sensor( SW_ON, &status );

  /* find ORG */
  printf("Finding the CW Limit  (Origin) ...   ");
  n_limit = 30000;     /* timeout pulse */
  while( ( pioin() & PI_GDMR_ORG ) == PI_GDMR_ORG ){
    pioout( status & ~PO_GDMR_CWP );
    p_wait( p_width );
    pioout( status | PO_GDMR_CWP );
    p_wait( p_width );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );

  return(0);
}

/*
  set Guide Mirror OUT-position
*/

int set_gdmr_out( unsigned long status , unsigned int p_width_fin )
{
  unsigned int n_limit, p_width;

  p_width = 10000;

  p_sensor( SW_ON , &status );

  /* find ORG */
  printf("Finding the CW Limit  (Origin) ...   ");
  n_limit = 30000;     /* timeout pulse */
  
  while( ( pioin() & PI_GDMR_ORG ) == PI_GDMR_ORG ){
    if ( p_width >= p_width_fin ) p_width -= GDMR_ACC;
    pioout( status & ~PO_GDMR_CWP );
    p_wait( p_width );
    pioout( status | PO_GDMR_CWP );
    p_wait( p_width );
    if ( --n_limit == 0){
      status &= ~PO_PHSENSOR;
      pioout( status );
      return(-1);
    } 
  }
  printf("Found!!\n");

  p_sensor( SW_OFF, &status );

  return(0);
}

/*
  move guide-mirror pulse_n steps in CW direction
*/

int move_gdmr_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;
  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 1000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_GDMR_ORG) !=  PI_GDMR_ORG ){
      printf("CW Limit (Origin) !! +%i steps moved\n", i );
      *p_pos += i;
      pioout( status & ~PO_PHSENSOR);
      return(0);
    }
    pioout( status & ~PO_GDMR_CWP);
    p_wait( p_width );
    pioout( status | PO_GDMR_CWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. +%i steps moved\n", i);
  *p_pos += i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/*
  move guide-mirror pulse_n steps in CCW direction
*/

int move_gdmr_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 10000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_GDMR_LIMIT) != PI_GDMR_LIMIT ){
      printf("CCW Limit !! %i steps moved\n", i );
      *p_pos -= i;
      pioout( status & ~PO_PHSENSOR);
      return(0);
      }
    pioout( status & ~PO_GDMR_CCWP);
    p_wait( p_width );
    pioout( status | PO_GDMR_CCWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. -%i steps moved\n", i);
  *p_pos -= i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/* -------------------
       GT Turret
   ------------------- */

/*
  set GT Turret to org. position
*/

int set_trrt_org( unsigned long status , unsigned int  p_width )
{
  unsigned int n_limit;
  long pos;
  unsigned long speed;

  speed = 20;
  pos = 0;

  /* find ORG (1st)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin ...   ");
  n_limit = 5000;     /* timeout pulse */
  while( ( pioin() & PI_TRRT_ORG ) == PI_TRRT_ORG ){
    pioout( status & ~PO_TRRT_CWP );
    p_wait( p_width );
    pioout( status | PO_TRRT_CWP );
    p_wait( p_width );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );
  p_wait( 500000 );

  /* back */
  move_trrt_ccw( TRRT_BACKPULSE, &pos, status, p_width * speed);
  p_wait( 500000 );

  /* find ORG (2nd)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin again...   ");
  n_limit = 5000;     /* timeout pulse */
  while( ( pioin() & PI_TRRT_ORG ) == PI_TRRT_ORG ){
    pioout( status & ~PO_TRRT_CWP);
    p_wait( p_width * speed );
    pioout( status | PO_TRRT_CWP);
    p_wait( p_width * speed );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );

  return(0);
}

/*
  move GT turret pulse_n steps in CW direction
*/

int move_trrt_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 1000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_TRRT_ORG) !=  PI_TRRT_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
    }
    pioout( status & ~PO_TRRT_CWP);
    p_wait( p_width );
    pioout( status | PO_TRRT_CWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. +%i steps moved\n", i);
  *p_pos += i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/*
  move GT turret pulse_n steps in CCW direction
*/

int move_trrt_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 10000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_TRRT_ORG) != PI_TRRT_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
      }
    pioout( status & ~PO_TRRT_CCWP);
    p_wait( p_width );
    pioout( status | PO_TRRT_CCWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. -%i steps moved\n", i);
  *p_pos -= i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/* -------------------
       Slitviewer
   ------------------- */

int setsltv( long coord_n, long *p_pos, unsigned long status,
	    unsigned int p_width)
{
  long pulse_n;
  pulse_n = coord_n - *p_pos;
  if( pulse_n > 0 ) move_sltv_cw( pulse_n, p_pos, status, p_width );
  if( pulse_n < 0 ) move_sltv_ccw( labs(pulse_n), p_pos, status, p_width );
  return(1);
}  

/*
  set Slitviewer to org. position
*/

int set_sltv_org( unsigned long status , unsigned int  p_width )
{
  unsigned int n_limit;
  long pos;
  unsigned long speed;

  speed = 20;
  pos = 0;

  /* find ORG (1st)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin ...   ");
  n_limit = 100000;     /* timeout pulse */
  while( ( pioin() & PI_SLTV_ORG ) == PI_SLTV_ORG ){
    pioout( status & ~PO_SLTV_CWP );
    p_wait( p_width );
    pioout( status | PO_SLTV_CWP );
    p_wait( p_width );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );
  p_wait( 500000 );

  /* back */
  move_sltv_ccw( SLTV_BACKPULSE, &pos, status, p_width * speed);
  p_wait( 500000 );

  /* find ORG (2nd)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin again...   ");
  n_limit = 100000;     /* timeout pulse */
  while( ( pioin() & PI_SLTV_ORG ) == PI_SLTV_ORG ){
    pioout( status & ~PO_SLTV_CWP);
    p_wait( p_width * speed );
    pioout( status | PO_SLTV_CWP);
    p_wait( p_width * speed );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );

  return(0);
}

/*
  move Slirviewer pulse_n steps in CW direction
*/

int move_sltv_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 1000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_SLTV_ORG) !=  PI_SLTV_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
    }
    pioout( status & ~PO_SLTV_CWP);
    p_wait( p_width );
    pioout( status | PO_SLTV_CWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. +%i steps moved\n", i);
  *p_pos += i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/*
  move Slitviewer pulse_n steps in CCW direction
*/

int move_sltv_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 10000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_SLTV_ORG) != PI_SLTV_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
      }
    pioout( status & ~PO_SLTV_CCWP);
    p_wait( p_width );
    pioout( status | PO_SLTV_CCWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. -%i steps moved\n", i);
  *p_pos -= i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/* -------------------
     Cross Disperser
   ------------------- */

/*
  set Cross Disperser to org. position
*/

int set_crds_org( unsigned long status , unsigned int  p_width )
{
  unsigned int n_limit;
  long pos;
  unsigned long speed;

  speed = 20;
  pos = 0;

  /* find ORG (1st)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin ...   ");
  n_limit = 100000;     /* timeout pulse */
  while( ( pioin() & PI_CRDS_ORG ) == PI_CRDS_ORG ){
    pioout( status & ~PO_CRDS_CWP );
    p_wait( p_width );
    pioout( status | PO_CRDS_CWP );
    p_wait( p_width );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );
  p_wait( 500000 );

  /* back */
  move_crds_ccw( CRDS_BACKPULSE, &pos, status, p_width * speed);
  p_wait( 500000 );

  /* find ORG (2nd)*/

  p_sensor( SW_ON, &status );

  printf("Finding Origin again...   ");
  n_limit = 100000;     /* timeout pulse */
  while( ( pioin() & PI_CRDS_ORG ) == PI_CRDS_ORG ){
    pioout( status & ~PO_CRDS_CWP);
    p_wait( p_width * speed );
    pioout( status | PO_CRDS_CWP);
    p_wait( p_width * speed );
    if ( --n_limit == 0){
      p_sensor( SW_OFF, &status );
      printf("Error !!\n");
      return(-1);
    } 
  }
  printf("Found!!\n");
  p_sensor( SW_OFF, &status );

  return(0);
}

/*
  move Cross Disperser pulse_n steps in CW direction
*/

int move_crds_cw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 1000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_CRDS_ORG) !=  PI_CRDS_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
    }
    pioout( status & ~PO_CRDS_CWP);
    p_wait( p_width );
    pioout( status | PO_CRDS_CWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. +%i steps moved\n", i);
  *p_pos += i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}

/*
  move Cross Disperser pulse_n steps in CCW direction
*/

int move_crds_ccw( unsigned long pulse_n, long *p_pos, unsigned long status,
		  unsigned int p_width )
{

  unsigned long i;
  unsigned long signal;

  if( pulse_n <= 0){
    printf("Can't move !\n");
    return(-1);
  }
  
  status |= PO_PHSENSOR;
  pioout( status );
  p_wait( 10000 );

  printf("Moving ...\n");
  i = 0;
  while( i < pulse_n ){
    signal = pioin();
    if( (signal & PI_CRDS_ORG) != PI_CRDS_ORG ){
      printf("Origin !! +%i steps moved. Continue... \n", i );
      }
    pioout( status & ~PO_CRDS_CCWP);
    p_wait( p_width );
    pioout( status | PO_CRDS_CCWP );
    p_wait( p_width );
    i++;
  }
  printf("Completed. -%i steps moved\n", i);
  *p_pos -= i ;

  status &= ~PO_PHSENSOR;
  pioout( status );

  return(0);
}


/*
  Photomicro sensor ON/OFF
*/


void p_sensor( int on_off , unsigned long *p_status)
{
  if ( on_off == SW_ON ){
    *p_status |= PO_PHSENSOR;
  }
  else if ( on_off == SW_OFF ){
    *p_status &= ~PO_PHSENSOR;
  }
  pioout( *p_status );
  p_wait( 1000 );
  return;
}

/*
   slit position definition
*/
void read_diaphdef( int *ndiaph, struct name_coord *diaphdef, char filename[] )
{
  FILE *diaph_f;
  int n;
  if( (diaph_f = fopen( filename, "r")) == NULL ){
    printf( "command file "DIAPHDEFFILE" is not found!!!\n" );
    *ndiaph = 0;
    return;
  }
  n=0;
  while( fscanf( diaph_f, "%s %li %s", diaphdef[n+1].name,
		&diaphdef[n+1].coord, diaphdef[n+1].comment
		) != EOF )    n++;
  *ndiaph = n;
  return;
}

/*
   guide mirror position definition
*/
long read_gdmrdef( void )
{
  long gdmr_inpos;
  FILE *gdmr_f;
  if( (gdmr_f = fopen( GDMRDEFFILE, "r")) == NULL ){
    printf( "command file "GDMRDEFFILE" is not found!!!\n" );
    return;
  }
  fscanf( gdmr_f, "%i", &gdmr_inpos );
  printf( "Guide Mirror IN: position = %i\n", gdmr_inpos );
  return( gdmr_inpos) ;
}

/*
   Show diaph definition
  */
void show_diaphdef( int ndiaph, struct name_coord diaphdef[] )
{
  int i;
  if( ndiaph == 0 ){
    printf( "No component is defined !!\n" );
    return;
  }
  printf( "Name              Position   Comment\n" );
  for(i = 1; i <= ndiaph; i++)
    printf( "%-16s%10li   %-s\n", diaphdef[i].name, diaphdef[i].coord,
	   diaphdef[i].comment );
  return;
}

/*
  Show command help
*/
void showhelp( void ){
  printf("============= available commands =============\n");
  printf("mv (pulse_n) : move stage (pulse_n) steps\n");
  printf(" set (coord) : set stage position to (coord)\n");
  printf("    gdmv (n) : move guide-mirror n step\n");
  printf("       gdorg : move guide-mirror to the origin\n");
  printf("    trmv (n) : move GT turret motor n step\n");
  printf("       trorg : move GT turret motor to the origin\n");
  printf("    svmv (n) : move Slirviewer motor n step\n");
  printf("       svorg : move Slitviewer motor to the origin\n");
  printf("    cdmv (n) : move Cross Disperser motor n step\n");
  printf("       cdorg : move Cross Disperser motor to the origin\n");
  printf("       slorg : set stage position to the origin\n");
  printf("       slhon : slit stage motor current on (default)\n");
  printf("      slhoff : slit stage motor current off\n");
  printf("       gdhon : guide mirror motor current on (default)\n");
  printf("      gdhoff : guide mirror motor current off\n");
  printf("       trhon : GT turret motor current on (default)\n");
  printf("      trhoff : GT turret motor current off\n");
  printf("       svhon : Slitviewer motor current on (default)\n");
  printf("      svhoff : Slitviewer motor current off\n");
  printf("       cdhon : Cross Disperser motor current on (default)\n");
  printf("      cdhoff : Cross Disperser motor current off\n");
  printf("       hcton : turn on HCT lamp\n");
  printf("      hctoff : turn off HCT lamp\n");
  printf("dset (diaph) : set stage to the diaphragm (dipaph)\n");
  printf("tset (comp)  : set GT turret to the component (comp)\n");
  printf("sset (filt)  : set SV turret to the component (filt)\n");
  printf("cset (RorB)  : set Cross Disperser to the component (RorB)\n");
  printf("        ddef : show diaphragm definitions\n");
  printf("        tdef : show turret componemt definitions\n");
  printf("        sdef : show SV turret componemt definitions\n");
  printf("        cdef : show Cross Disperser componemt definitions\n");
  printf("  slpw (p_w) : set slit stage motor pulse width (p_w) micro sec.\n");
  printf("  gdpw (p_w) : set guide mirror motor pulse width (p_w) micro sec.\n");
  printf("  trpw (p_w) : set GT turret motor pulse width (p_w) micro sec.\n");
  printf("  svpw (p_w) : set Slitviewer motor pulse width (p_w) micro sec.\n");
  printf("  cdpw (p_w) : set Cross Disperser motor pulse width (p_w) micro sec.\n");
  printf("     ?, help : show command help (this document)\n");
  printf("          in : Guide Mirror-> in, Diaphragm -> hole\n"); 
  printf("         out : Diaphragm -> slit, Guide Mirror -> out\n"); 
  printf("         obs : HCT lamp -> off, GT Turret -> hole\n"); 
  printf("        comp : HCT lamp -> on, GT Turret -> hct\n"); 
  printf("     ?, help : show command help (this document)\n");
  printf("     q, quit : quit program\n");
  printf("==============================================\n");
  return;
}

/*
  startup messages
*/
void start_msg( void ){
  printf("=================================================\n");
  printf("  LIPS motor control program               \n");

#ifdef LINUX
  printf("                             for Linux           \n");
#endif
  
#ifdef DOS
  printf("                             for Windows/Dos     \n");
#endif

  printf("         Ver 1.81      2003.3.28   K.Matsuda   \n");
  printf("=================================================\n");
  return;
}