/* -------------------------------------------------------------------------- */
/* -         Protocol for Paramount MKS3 command set telescope control      - */
/* -------------------------------------------------------------------------- */
/*                                                                            */
/* Copyright 2010-2011 John Kielkopf                                          */
/*                                                                            */
/* Distributed under the terms of the General Public License (see LICENSE)    */
/*                                                                            */
/* John Kielkopf (kielkopf@louisville.edu)                                    */
/*                                                                            */
/* Date: July 22, 2012                                                        */
/* Version: 2.3                                                               */
/*                                                                            */
/* History:                                                                   */
/*                                                                            */
/* September 4, 2010                                                          */
/*   Version 1.0                                                              */
/*                                                                            */
/* November 17, 2010                                                          */
/*   Version 1.1                                                              */
/*   Added fan, rotator, thermal control                                      */
/*                                                                            */
/* October 10, 2011                                                           */
/*   Version 2.3                                                              */
/*   Version changed to match package                                         */
/*   Added CenterGuide                                                        */



/* This program requires an mks3 object module and header file                */
/*   developed by Petr Kubanek (petr@kubanek.net) for RTS2.                   */ 
/*   More information is in the xmtel/bisque/mks/README .                     */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <math.h>
#include "protocol.h"
#include "mks3.h"

#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

/* System variables and prototypes */

/* Telescope and mounting commands that may be called externally */

/* Interface control */

void ConnectTel(void);
int  SetTelEncoders(double homeha, double homedec);
void DisconnectTel(void);
int  CheckConnectTel(void);

/* Slew and track control */

void SetRate(int newrate);
void StartSlew(int direction);
void StopSlew(int direction);
void StartTrack(void);
void CenterGuide(double centerra, double centerdec, 
  int raflag, int decflag, int pmodel);
void StopTrack(void);
void FullStop(void);

/* Coordinates and time */

void GetTel(double *telra, double *teldec, int pmodel);
void GetNewPos(double *newposra, double *newposdec, 
  double newra0, double newdec0);
int  GoToCoords(double newRA, double newDec, int pmodel);
int  CheckGoTo(double desRA, double desDec, int pmodel);

/* Slew limits */

int  GetSlewStatus(void);
int  SetLimits(int limits);
int  GetLimits(int *limits);

/* Synchronizing */

void SyncTelOffsets(double newoffsetha, double newoffsetdec);

/* Instrumentation */

void Heater(int heatercmd);
void Fan(int fancmd);
void Focus(int focuscmd, int focusspd);
void Rotate(int rotatecmd, int rotatespd);
void GetFocus(double *telfocus);
void GetRotate(double *telrotate);
void GetTemperature(double *teltemperature);

/* External variables and shared code */

extern double LSTNow(void);
extern double UTNow(void);
extern double Map24(double hour);
extern double Map12(double hour);
extern double Map360(double degree);
extern double Map180(double degree);
extern double offsetra, offsetdec;
extern double SiteLatitude, SiteLongitude;
extern int    telmount;
extern int    homenow;
extern double homeha;            /* Home ha */
extern double homera;            /* Home ra derived from ha */
extern double homedec;           /* Home dec */ 
extern char   telserial[32];     /* Serial port */
  
extern void PointingFromTel (double *telra1, double *teldec1, 
  double telra0, double teldec0, int pmodel);

extern void PointingToTel (double *telra0, double *teldec0, 
  double telra1, double teldec1, int pmodel);
  
extern void EquatorialToHorizontal(double ha, double dec, double *az, double *alt);
extern void HorizontalToEquatorial(double az, double alt, double *ha, double *dec);  

/* Files */

FILE *fp_focus;
char *focusfile;

FILE *fp_temperature;
char *temperaturefile;

FILE *fp_rotate;
char *rotatefile;




/* Local data */

static int slewphase = 0;             /* Slew sequence counter */

/* Axis calibration and jog increments */

/* Position or encoder counts per degree */

static double altcountperdeg = ALTCOUNTPERDEG;
static double azcountperdeg  = AZCOUNTPERDEG;

/* Sign of encoder counts for home position to increase HA and Dec */

static double altsign = ALTSIGN;
static double azsign = AZSIGN;
double azjog = 0.;
double altjog = 0.;

/* Communications */

static int TelConnectFlag = FALSE;

/* MKS3 variables */

MKS3Id axis0;  /* RA  axis */
MKS3Id axis1;  /* Dec axis */
short status0, status1;
short motorstate0, motorstate1;
long pos0, pos1;
long dpos0, dpos1;
long enc0, enc1;
double refra, refha;  /* RA  axis reference coordinates in hours   */
double refdec;        /* Dec axis reference coordinates in degrees */

 
/* End of prototype and variable definitions */

 
/* Report on telescope connection status */

int CheckConnectTel(void)
{
  if (TelConnectFlag == TRUE)
  {
    return TRUE;
  }
  else
  {
    return FALSE;
  }
}


/* Connect to the MKS3 serial interface */
/* Return without action if TelConnectFlag is TRUE */
/* Set TelConnectFlag TRUE on success */
/* Uses serial port telserial */

void ConnectTel(void)
{
  
  int ret, ret0, ret1;
  int i;
  short major, minor, build;
  int flag;
  int limits;
    
  if(TelConnectFlag != FALSE)
  {
    return;
  }
  
  /* Initialize mk3 and make the serial connection */  
  /* MKS3Init ("/dev/ttyS0"); */ 
  
  ret = MKS3Init (telserial);
  
  if (ret)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr,
      "The mount controller at  %s is not available ... \n", telserial);
    return;    
  }
  
  /* Set the controller and motor ids */

  /* RA axis */
  
  axis0.unitId = 0x64;
  axis0.axisId = 0;
  
  /* Dec axis */
  
  axis1.unitId = 0x64;
  axis1.axisId = 1;
  
  /* The following startup is from RTS2 paramount.cpp */
  
  /* Begin by exercising the motors on and off */
  
  /* Turn on the motors */
  
  ret0 = MKS3MotorOn (axis0);
  
  if (ret0 != MKS_OK)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr,
      "MKS3 RA start error %d \n", ret0);
    return; 
  }      
  
  ret1 = MKS3MotorOn (axis1);
  if (ret1 != MKS_OK)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr,
      "MKS3 Dec start error %d \n", ret0);
  }    
  usleep(100000);
  
  /* Turn off the motors */
  
  ret0 = MKS3PosAbort (axis0);
  if (ret0 != MKS_OK)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr,
      "MKS3 RA stop error %d \n", ret0);
  }  
  
  ret1 = MKS3PosAbort (axis1);
  if (ret1 != MKS_OK)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr,
      "MKS3 Dec stop error %d \n", ret1);
  }   
  
  usleep(100000);
  
  /* Wait until they stop */
  
  for (i = 0; i < 10; i++)
  {

    ret0 = MKS3MotorStatusGet (axis0, &motorstate0);
    ret1 = MKS3MotorStatusGet (axis1, &motorstate1);
       
    if ( (ret0 != MKS_OK) || (ret1 != MKS_OK) )
    {
      TelConnectFlag = FALSE;
      fprintf(stderr,
        "MKS3 get motor status errors RA: %d   Dec: %d\n", ret0, ret1);            
      return;
    }
    
    /* Break from loop if both motors stopped */
    
    if (!((motorstate0 & MOTOR_SLEWING) || (motorstate1 & MOTOR_SLEWING)))
    {
      break;
    }
    
    /* Check RA or Dec limits */
    
    if ( (motorstate0 & MOTOR_POSERRORLIM)
            || (motorstate1 & MOTOR_POSERRORLIM) )
    {
      ret0 = MKS3ConstsMaxPosErrorSet (axis0, 16000);
      ret1 = MKS3ConstsMaxPosErrorSet (axis1, 16000);
      
      /* Check if motor states are OK */
      
      if ( (ret0 != MKS_OK) || (ret1 != MKS_OK) )
      {
        TelConnectFlag = FALSE;
        fprintf(stderr,
        "MKS3 motor limit errors  RA: %d   Dec: %d\n", ret0, ret1);     
        return;
      }
      
      /* If motors are ok and in limit state then set counter and break */
      
      i = 10;
      break;
    }
    
    usleep (100000);
  }
  
  /* Set target to current position if count exceeded */

  if (i == 10)
  {
    ret0 = MKS3PosCurGet (axis0, &pos0);
    ret1 = MKS3PosCurGet (axis1, &pos1);
    if ( (ret0 != MKS_OK) || (ret1 != MKS_OK) )
    {
      TelConnectFlag = FALSE;
      fprintf(stderr, "MKS3 get position errors  RA: %d   Dec: %d\n", ret0, ret1);
      return;
    }
    ret0 = MKS3PosTargetSet (axis0, pos0);
    ret1 = MKS3PosTargetSet (axis1, pos1);
    if ( (ret0 != MKS_OK) || (ret1 != MKS_OK) )
    {
      TelConnectFlag = FALSE;
      fprintf(stderr, "MKS3 set position errors  RA: %d   Dec: %d\n", ret0, ret1);     
      return;
    }
  } 

  /* Homing could go here if needed */
    
  /* MKS3Home (axis0, 0);  */
  /* MKS3Home (axis1, 0);  */


  /* Confirm motors off */
  
  ret0 = MKS3MotorOff (axis0);
  ret1 = MKS3MotorOff (axis1);
    

  /* Request version of MK3 to confirm communications */
  
  ret = MKS3VersionGet (axis0, &major, &minor, &build);
  if (ret)
  {
    TelConnectFlag = FALSE;
    fprintf(stderr, "MKS3 did not respond to request for current version ...\n");      
    return;
  }

  fprintf (stderr, "Paramount %i.%i.%i\n", major, minor, build);
  fprintf (stderr, "Using %lf HA counts per degree \n", azcountperdeg);
  fprintf (stderr, "Using %lf Dec counts per degree \n", altcountperdeg);
  
  /* MKS3 has been initialized, motors have be cycled, and counters set */
  
  /* Perform additional startup tests for xmtel */

  flag = GetLimits(&limits);
  usleep(500000);
  limits = FALSE;
  flag = SetLimits(limits);
  usleep(500000);  
  flag = GetLimits(&limits);
  
  /* These defaults for homeha and homedec are overridden by prefs */

  if ( homenow != TRUE )
  {
    if (SiteLatitude < 0.)
    {
      homedec = 0.;
      homeha = -2.;
    }
    else
    {  
      homedec = 0.;
      homeha = 2.;
    }
  } 
  
  /* In the southern hemisphere the count directions may change */

  if (SiteLatitude < 0.)
  {
    altsign =  -1.*altsign;
    azsign  =  -1.*azsign;
  }
  
  /* Set reference ra and dec based on initial home position */
  /* This call is made  immediately after mount is homed     */
 
  SetTelEncoders(homeha, homedec);
    
  fprintf(stderr, "Using initial HA: %lf\n",  refha);
  fprintf(stderr, "Using initial RA: %lf\n",  refra);
  fprintf(stderr, "Using initial Dec: %lf\n", refdec); 
  
  /* Start motors */

  ret0 = MKS3MotorOn (axis0);
  usleep(100000);  
  ret1 = MKS3MotorOn (axis1);
  usleep(100000);  
     
  /* Announce initial settings */
         
  fprintf(stderr, "Local latitude: %lf\n", SiteLatitude);
  fprintf(stderr, "Local longitude: %lf\n", SiteLongitude);
  fprintf(stderr, "Local sidereal time: %lf\n", LSTNow()); 
  fprintf(stderr, "Mount type: %d\n", telmount);
  fprintf(stderr, "Mount now reading RA: %lf\n", refra);
  fprintf(stderr, "Mount now reading Dec: %lf\n", refdec);
  
  fprintf(stderr, "The telescope is running ...\n\n");
  TelConnectFlag = TRUE;  
}

/* Assign and save jog increment for use in StartSlew  */
/* Small HA jogs may show mount controller latency     */
void SetRate(int newrate)
{
           
  if(newrate == SLEW) 
  {
    /* 1 degree */
    azjog =  1.;
    altjog =  1.;
  }
  else if(newrate == FIND) 
  {
    /* 10 arcminutes */
    azjog =  10./60.;
    altjog =  10./60.;
  }
  else if(newrate == CENTER) 
  {
    /* 2 arcminutes */
    azjog =  2./60.;
    altjog =  2./60.;
  }
  else if(newrate == GUIDE) 
  {
    /* 0.5 arcminutes */ 
    azjog =  0.5/60.;   
    altjog =  0.5/60.;
  }

}
 

/* Start an incremental jog in a chosen direction */

void StartSlew(int direction)
{

  /* Get the current positions */
  
  MKS3PosCurGet (axis0, &pos0);
  MKS3PosCurGet (axis1, &pos1);
  
  /* Set the jog increments */

  dpos0 = (long) (azjog * azcountperdeg * azsign );
  dpos1 = (long) (altjog * altcountperdeg * altsign );

  if(direction == NORTH)
  {
     pos1 = pos1 + dpos1;
  }
  else if(direction == SOUTH)
  {
     pos1 = pos1 - dpos1;
  }
  else if(direction == EAST)
  {
     pos0 = pos0 + dpos0;
  }
  else if(direction == WEST)
  {
     pos0 = pos0 - dpos0;
  }
  
  /* Motors on -- could add a test for state here */
  
  /* MKS3MotorOn (axis0);  */
  /* MKS3MotorOn (axis1);  */

  /* Set differential target and begin motion */
  
  MKS3PosTargetSet (axis0, pos0);
  usleep(100000);
  MKS3PosTargetSet (axis1, pos1); 
    
}


/* Stop the jog in chosen direction */
/* This function is handled automatically by the MKS3 driver */

void StopSlew(int direction)
{
    
  if(direction == NORTH)
  {

  }
  else if(direction == SOUTH)
  {

  }
  else if(direction == EAST)
  {

  }
  else if(direction == WEST)
  {

  }

}

/* Close the connection to the telescope controller */

void DisconnectTel(void)
{
  if(TelConnectFlag == TRUE)
  {

  }
  TelConnectFlag = FALSE;
}

   
/* Set the reference position to current ha and dec          */
/* Returns +1 (TRUE) on success and 0 (FALSE) otherwise      */

int SetTelEncoders(double setha, double setdec)
{
      
  double aznow, altnow;
  
  /* Test ha and dec for valid range */
  
  if ( setha > 6. || setha < -6. )
  {
    fprintf(stderr,"Telescope may be pointed outside valid HA range\n");
    fprintf(stderr,"It cannot be started safely using last known position\n");
    return(0);
  }
  
  if ( setdec > 90. || setdec < -90. )
  {
    fprintf(stderr,"Telescope may be pointed outside valid Dec range\n");
    fprintf(stderr,"It cannot be started safely using last known position\n");
    return(0);
  }
  
  /* Convert HA and Dec to Alt and Az */
  
  EquatorialToHorizontal(setha, setdec, &aznow, &altnow);
  
  if (altnow < 0.)
  {
    fprintf(stderr,"Telescope may be pointed below the horizon\n");
    fprintf(stderr,"It cannot be started safely using last known position\n");
    return(0);
  }

  refha  = setha;
  refra  = Map24(LSTNow() - refha);
  refdec = setdec;
                  
  return(1);
}

/* Read the mount position                                            */
/* Save readings in global variables                                  */
/* Use an NTP synchronized clock to find lst                          */
/* Convert the readings to mounting ha, ra and dec                    */
/* Correct for the pointing model to find the true direction vector   */
/* Report the ra and dec at which the telescope is pointed            */
/* Parmount/MKS reports a position not a raw encoder count            */
/* Positions are linearly  mapped to rotation in ra and dec           */
/* However pos0 gives ra rather than ha                               */

void GetTel(double *telra, double *teldec, int pmodel)
{  
   
  double telha0 = 0.;
  double teldec0 = 0.;
  double telra0 = 0.;
  double telra1 = 0.;
  double teldec1 = 0.;
  double posra = 0.;
  double posdec = 0.;
  double dra = 0.;
  double ddec = 0.;
    
  /* Query controller for delta posra from reference */
  
  MKS3PosCurGet (axis0, &pos0);
  
  /* Query controller for delta posdec from reference */
  
  MKS3PosCurGet (axis1, &pos1);
 
  /* Convert long count to double */
  
  posra = (double) pos0;
  posdec = (double) pos1;
    
  /* Convert delta counts to delta degrees for both axes */
  
  dra = posra * azsign / azcountperdeg;
  ddec = posdec * altsign / altcountperdeg;
    
  /* Change delta ra from degrees to hours */
  
  dra = dra / 15.;
    
  /* Find telescope ra and dec from reference and save */
  
  telra0 = Map24(refra + dra);
  teldec0 = refdec + ddec;
  
  /* Find telescope ha */
  
  telha0 = Map12(LSTNow() - telra0); 
      
  /* Handle special cases of teldec0 beyond the pole */
  
  if (teldec0 > 90.)
  {
    teldec0 = 180. - teldec0;
    telra0 = Map24(telra0 + 12.);
  }
  else if (teldec0 < -90.)
  {
    teldec0 = -180. - teldec0;
    telra0 = Map24(telra0 + 12.);
  }
    
  /* Apply pointing model to the coordinates that are reported by the telescope */
  
  PointingFromTel(&telra1, &teldec1, telra0, teldec0, pmodel);
      
  /* Return corrected values */

  *telra=telra1;
  *teldec=teldec1;
    
  return;

}


/* Go to new celestial coordinates                                    */
/* Evaluate if target coordinates are valid                           */
/* Test slew limits                                                   */
/* Query if target is above the horizon                               */
/* Return without action for invalid requests                         */
/* Update current pointing                                            */
/* Set slewphase equal to number of slew segments needed              */
/* Begin next segment needed to reach target from current pointing    */
/* Repeated calls are required when more than one segment is needed   */
/* Return 1 if underway                                               */
/* Return 0 if done or not permitted                                  */

int GoToCoords(double newra, double newdec, int pmodel)
{
  long target_pos0;
  long target_pos1;
  double newha, newalt, newaz;
  double newra0, newdec0;
  double newra1, newdec1;
  double newposra, newposdec;
  

  /* Find target ha and use it to test horizon and mount flip */

  newha = LSTNow() - newra;
  newha = Map12(newha);
        
  /* Convert HA and Dec to Alt and Az */
  /* Test for visibility              */
  
  EquatorialToHorizontal(newha, newdec, &newaz, &newalt);
  
  /* Check altitude limit */
  
  if (newalt < MINTARGETALT)
  {
    fprintf(stderr,"Target is below the telescope horizon ...\n");
    slewphase = 0;
    return(0);
  }
  
  /* Target request is valid */
  /* Find mount coordinates for the target */
  
  newra1 = newra;
  newdec1 = newdec;
  PointingToTel(&newra0,&newdec0,newra1,newdec1,pmodel);  
  
  /* Find mount positions for these coordinates */
  
  GetNewPos(&newposra, &newposdec, newra0, newdec0);
  
  target_pos0 = (long) newposra;
  target_pos1 = (long) newposdec;
  
  /* Send target positions to initiate slew */
  
  slewphase = 1;
  MKS3PosTargetSet (axis0, target_pos0);
  MKS3PosTargetSet (axis1, target_pos1);
            
  /* A slew is in progress */

  return(1);
}


/* Find new mount target positions                             */
/* Input target mount ra and dec                               */
/* Read current mount positions and calculate mount ra and dec */
/* Return positions that would point mount to target           */

void GetNewPos(double *newposra, double *newposdec, 
  double targetra, double targetdec)
{  
   
  double telha = 0.;
  double targetha = 0.;
  double refha = 0.;
  double meridian = 0.;
  double telposra = 0.;
  double telposdec = 0.;
  double targetposra = 0.;
  double targetposdec = 0.;
  double targetdra = 0.;
  double targetddec = 0.;  
  double telra = 0.;
  double teldec = 0.;
  
  /* Query controller for posra relative to reference */
  
  MKS3PosCurGet (axis0, &pos0);
  
  /* Query controller for posdec relative to reference */
  
  MKS3PosCurGet (axis1, &pos1);
 
  /* Convert long count to double */
  
  telposra  = (double) pos0;
  telposdec = (double) pos1;
  
  /* Convert counts to degrees for both axes     */
  /* Northern hemisphere: azsign - and altsign - */
  /* Southern hemisphere: azsign - and altsign + */
  
  telra  = telposra  * azsign  / azcountperdeg;
  teldec = telposdec * altsign / altcountperdeg;
    
  /* Change ra from degrees to hours */
  
  telra = telra / 15.;
    
  /* Add reference to get absolute coordinates */
  
  telra  = Map24(refra + telra);
  teldec = refdec + teldec;
  
  /* Handle special cases of teldec beyond the poles */
  
  if (teldec > 90.)
  {
    
    /* Only accessible from the northern hemisphere */
    
    teldec = 180. - teldec;
    telra  = Map24(telra + 12.);
  }
  else if (teldec < -90.)
  {
    
    /* Only accessible from the southern hemisphere */
    
    teldec = -180. - teldec;
    telra  = Map24(telra + 12.);
  }
    
  /* Find current hour angles */

  meridian = LSTNow();
  telha    = Map12(meridian - telra); 
  targetha = Map12(meridian - targetra); 
  refha    = Map12(meridian - refra); 
  
  /* Northern hemisphere homeha = +2 hr */
  /* Southern hemisphere homeha = -2 hr */
  /* Both hemispheres    refha = elapsed lst since start + homeha */

  /* The telescope's current configuration is now known */
  
  /* Calculate target position */
  /* Use refra and refdec where pos0 and pos1 are zero */
  
  if (SiteLatitude < 0.)
  {
    /* Southern hemisphere homeha = -2 hr */
    
    if ((targetha >= -12.) && ( targetha < -6.))
    { 
      targetdra  = 15.*(Map12(targetra  - refra) - 12.);
      targetddec = (targetdec - refdec);
    }
    else if ((targetha >= -6.) && ( targetha <= 0.))
    { 
      targetdra  = 15.*Map12(targetra  - refra);
      targetddec = (targetdec - refdec);      
    }
    else if ((targetha > 0.) && ( targetha <= 6.))
    { 
      targetdra  = 15.*(Map12(targetra  - refra) - 12.);
      targetddec = 180. - (targetdec - refdec);
    }
    else if ((targetha > 6.) && ( targetha <= 12.))
    {  
      targetdra  = 15.*Map12(targetra  - refra);
      targetddec = 180. - (targetdec - refdec);
    }    
    else
    {
      fprintf(stderr,"MKS slew request to %lf hours HA error\n", targetha);     
    }
  }
  else
  {
    /* Northern hemisphere homeha = +2 hr */
    
    if ((targetha >= -12.) && ( targetha < -6.))
    { 
      targetdra  = 15.*Map12(targetra  - refra);
      targetddec = 180. - (targetdec - refdec);
    }
    else if ((targetha >= -6.) && ( targetha <= 0.))
    { 
      targetdra  = 15.*(Map12(targetra  - refra) - 12.);
      targetddec = 180. - (targetdec - refdec);
    }
    else if ((targetha > 0.) && ( targetha <= 6.))
    { 
      targetdra  = 15.*Map12(targetra  - refra);
      targetddec = (targetdec - refdec);
    }
    else if ((targetha > 6.) && ( targetha <= 12.))
    {  
      targetdra  = 15.*(Map12(targetra  - refra) - 12.);
      targetddec = (targetdec - refdec);
    }    
    else
    {
      fprintf(stderr,"MKS slew request to %lf hours HA error\n", targetha);     
    }
  }
  
  targetposra  = targetdra*azsign*azcountperdeg;
  targetposdec = targetddec*altsign*altcountperdeg;

  /* Consider exceptions here if any cause problems */


  /* Return new positions */

  *newposra  = targetposra;
  *newposdec = targetposdec;
  
    
  return;
}


/* Low level check of slew status on both axes */
/* Advise using CheckGoTo in external applications */
/* Return a flag indicating whether a slew is now in progress */
/*   1 -- slew is in progress on either drive */
/*   0 -- slew not in progress for either drive */

int GetSlewStatus(void)
{

  /* Query azimuth drive first */
        
  if ( TRUE ) 
  {
     return(1);
  }
  
  /* Query altitude drive if azimuth drive is not slewing */

  if ( TRUE ) 
  {
     return(1);
  }

 return(0);
}




/* Test whether the destination was reached                  */
/* Initiate the next segment if slewphase is greater than 1  */
/* Reset slewphase when goto has finished a segment          */
/* Return value is                                           */
/*   0 -- goto in progress                                   */
/*   1 -- goto complete within tolerance                     */
/*   2 -- goto complete but outside tolerance                */

int CheckGoTo(double desRA, double desDec, int pmodel)
{
  double telra1, teldec1;
  double errorRA, errorDec, nowRA, nowDec;
  double tolra, toldec;

  /* Is the telescope slewing? */
    
  if ( GetSlewStatus() == 1 )
  {
    /* One or more axes remain in motion */
    /* Try again later */
    
    return(0);
  }
  
  /* Was this a two-phase slew? */
  
  if ( slewphase == 2 )
  {
        
    /* Reset the slew phase and continue to the destination */
    
    slewphase = 0;    
    
    /* Go to the original destination */
    /* GoToCoords will change slewphase to 1 */
        
    GoToCoords(desRA, desDec, pmodel);
        
    /* Return a flag indicating a new goto operation is in progress */
    
    return(0);
  }
  else if ( slewphase == 1 )
  {    
      
    /* No axes are moving. Insure that tracking is started again. */
    
    StartTrack();
    
    /* Where are we now? */
  
    GetTel(&nowRA, &nowDec, pmodel);

    /* Compare to destination with pre-defined tolerances */
     
    telra1 = desRA;
    teldec1 = desDec;
        
    /* RA slew tolerance in hours */
    
    tolra = SLEWTOLRA;
    
    /* Dec slew tolerance in degrees */
    
    toldec = SLEWTOLDEC;

    /* What is the absolute value of the pointing error? */
  
    /* Magnitude of RA pointing error in hours */
    
    errorRA = fabs(nowRA - telra1);
    
    /* Magnitude of Dec pointing error in degrees */
    
    errorDec = fabs(nowDec - teldec1);
  
    /* Compare and notify whether we are within tolerance */

    if( ( errorRA > tolra ) || ( errorDec > toldec ) )
    {
      /* Result of slew is outside acceptable tolerance */
      /* Signal the calling routine that another goto may be needed */
    
      slewphase = 0;
      return(2);
    }
  }    
  else
  {
    /* Unexpected slew phase */
    /* Reset and return success without a test */
    /* This should clear errors and enable another slew request from the UI */
    /* Better would be to flag an error but that might have unintended consequences */
  
    slewphase = 0;    
  } 
  return(1); 
}

/* Coordinates and time */

/* Synchronize remote telescope to this ra-dec pair */
/* In this inteface simply update global offsets */

void SyncTelOffsets(double newoffsetha, double newoffsetdec)
{
/*
  offsetha = newoffsetha;
  offsetdec = newoffsetdec;
*/
  return;
}


/* Start sidereal tracking */
/* Also enable motion in declination */

void StartTrack(void)
{    
  MKS3MotorOn (axis0);
  MKS3MotorOn (axis1);
} 

/* Use high resolution encoder to improve tracking */

void CenterGuide(double centerra, double centerdec, 
  int raflag, int decflag, int pmodel)
{
}


/* Stop azimuth motion only */
/* Telescope will not respond to joystick until StartTrack is executed */

void StopTrack(void)
{
  MKS3MotorOff (axis0);
}


/* Stop altitude and azimuth motion */

void FullStop(void)
{ 
   MKS3MotorOff (axis0);
   MKS3MotorOff (axis1);
}

/* Set slew limits control off or on */

int SetLimits(int limits)
{
  
  if ( limits == TRUE )
  {
    fprintf(stderr,"Limits enabled\n"); 
  
  }
  else
  {
    limits = FALSE;
    fprintf(stderr,"Limits disabled\n");   
  }
       
  return (limits);
}


/* Get status of slew limits control */

int GetLimits(int *limits)
{
  int mks3limits;
  
  mks3limits = 0;
  
  if ( mks3limits == 1 )
  {
    *limits = 1;
  }
  else
  {
    *limits = 0;
  }

  return (mks3limits);
}
  

/* Set slew speed limited by MAXSLEWRATE */

int SetSlewRate(int slewRate)
{
  fprintf(stderr,"Setting fixed slew rate not enabled.\n");
  return 0;
}


/* Control the dew and drive heaters */

void Heater(int heatercmd)
{
  char cmdstr[256];
  sprintf(cmdstr,"setheater %d 1>/dev/null 2>/dev/null", heatercmd);
  system(cmdstr);  
}

/* Control the telescope fans */

void Fan(int fancmd)
{
  char cmdstr[256];
  sprintf(cmdstr,"setfan %d 1>/dev/null 2>/dev/null", fancmd);
  system(cmdstr);   
}

/* Adjust the focus using an external routine */
/* The routine will time out on its own and report through the status file */

void Focus(int focuscmd, int focusspd)
{
  char cmdstr[256];
  sprintf(cmdstr,"setfocus %d %d 1>/dev/null 2>/dev/null", focuscmd, focusspd);
  system(cmdstr);  
}


/* Report the current focus reading from the status file */

void GetFocus(double *telfocus)
{
  int nread;
  double current_focus;
  char cmdstr[256];
  sprintf(cmdstr,"getfocus 1>/dev/null 2>/dev/null");
  system(cmdstr);  
  
  focusfile = (char*) malloc (MAXPATHLEN);
  strcpy(focusfile,FOCUSFILE);
  fp_focus = fopen(focusfile, "r");
  if (fp_focus == NULL)
  {
    return;
  }
  
  nread = fscanf(fp_focus, "%lg", &current_focus);
  fclose(fp_focus);
  
  if (nread !=0)
  {
    *telfocus = current_focus;
  }
  
  return;  
}



/* Adjust the rotation */

void Rotate(int rotatecmd, int rotatespd)
{
  char cmdstr[256];
  sprintf(cmdstr,"setrotate %d %d  1>/dev/null 2>/dev/null", rotatecmd, rotatespd);
  system(cmdstr);
}

/* Report the rotation setting */

void GetRotate(double *telrotate)
{
  int nread;
  double current_rotate;
  char cmdstr[256];
  sprintf(cmdstr,"getrotate 1>/dev/null 2>/dev/null");
  system(cmdstr);  
  
  rotatefile = (char*) malloc (MAXPATHLEN);
  strcpy(rotatefile,ROTATEFILE);
  fp_rotate = fopen(rotatefile, "r");
  if (fp_rotate == NULL)
  {
    return;
  }
  
  nread = fscanf(fp_rotate, "%lg", &current_rotate);
  fclose(fp_rotate);
  
  if (nread !=0)
  {
    *telrotate = current_rotate;
  }
  
  return;  
}

/* Report the temperature */

void GetTemperature(double *teltemperature)
{
  int nread;
  double current_temperature;
  char cmdstr[256];
  
  sprintf(cmdstr,"gettemperature 1>/dev/null 2>/dev/null");
  system(cmdstr);  
  
  temperaturefile = (char*) malloc (MAXPATHLEN);
  strcpy(temperaturefile,TEMPERATUREFILE);
  fp_temperature = fopen(temperaturefile, "r");
  if (fp_temperature == NULL)
  {
    return;
  }
  
  nread = fscanf(fp_temperature, "%lf", &current_temperature);
  fclose(fp_temperature);
  
  if (nread !=0)
  {
    *teltemperature = current_temperature;
  }
  
  return;
}

