/* encoder.c */

/* Copyright(c) 1991-2002 by Motion Engineering, Inc.  All rights reserved.
 *
 * This software  contains proprietary and  confidential information  of
 * Motion Engineering Inc., and its suppliers.  Except as may be set forth
 * in the license agreement under which  this software is supplied, use,
 * disclosure, or  reproduction is prohibited without the prior express
 * written consent of Motion Engineering, Inc.
 */

#if defined(MEI_RCS)
static const char MEIAppRCS[] =
    "$Header: /MainTree/XMPLib/XMP/app/encoder.c 8     7/23/01 2:36p Kevinh $";
#endif

/*

:Continuous display of motor actual position while switching the
 encoder polarity when a key is pressed.

 Warning!  This is a sample program to assist in the integration of the
 XMP motion controller with your application.  It may not contain all
 of the logic and safety features that your application requires.

*/

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

#include "stdmpi.h"
#include "stdmei.h"

#include "apputil.h"

#if defined(ARG_MAIN_RENAME)
#define main    encoderMain

argMainRENAME(main, encoder)
#endif

/* Command line arguments and defaults */
long    axisNumber  = -1;
long    motorNumber =  0;

Arg argList[] = {
    {   "-axis",    ArgTypeLONG,    &axisNumber,    },
    {   "-motor",   ArgTypeLONG,    &motorNumber,   },

    {   NULL,       ArgTypeINVALID, NULL,   }
};

typedef struct _Context *Context;

typedef struct _Context {
    MPIControl  control;
    MPIAxis     axis;
    MPIMotor    motor;

    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    MPIMotorConfig  motorConfig;

    long    axisNumber;
    long    motorNumber;

    long    encoderPhase;
} _Context;

long    contextCreate(Context   contextData,
                      long      argc,
                      char      *argv[]);
long    contextDelete(Context   contextData);

void    positionDisplay(double  position);
double  positionGet(Context contextData);

Static _Context ContextObject;

int
    main(int    argc,
         char   *argv[])
{
    Context context;

    long    returnValue;

    context = &ContextObject;

    returnValue =
        contextCreate(context,
                      argc,
                      argv);

    /* Disable amplifier */
    if (returnValue == MPIMessageOK) {
        returnValue =
            mpiMotorAmpEnableSet(context->motor,
                                 FALSE);
    }

    while (returnValue == MPIMessageOK) {
        long    key;

        printf("\nMove the motor and note the direction; it should be %s.\n",
                (context->motorConfig.encoderPhase == 0)
                                ? "normal"
                                : "reversed");

        printf("Press a key to change the encoder phasing, ESC to quit.\n");

        while ((key = meiPlatformKey(MPIWaitPOLL)) <= 0) {
            positionDisplay(positionGet(context));
        }

        if (key == 0x1b) {
            break;
        }

        context->motorConfig.encoderPhase = (context->motorConfig.encoderPhase == 0);

        returnValue =
            mpiMotorConfigSet(context->motor,
                              &context->motorConfig,
                              NULL);
    }

    returnValue = contextDelete(context);
    meiASSERT(returnValue == MPIMessageOK);

    return ((int)returnValue);
}

long
    contextCreate(Context   context,
                  long      argc,
                  char      *argv[])
{
    long    returnValue;

    long    argIndex;

    memset(context, 0, sizeof(*context));

    /* Parse command line for Control type and address */
    argIndex =
        argControl(argc,
                   argv,
                   &context->controlType,
                   &context->controlAddress);

    /* Parse command line for application-specific arguments */
    while (argIndex < argc) {
        long    argIndexNew;

        argIndexNew = argSet(argList, argIndex, argc, argv);

        if (argIndexNew <= argIndex) {
            argIndex = argIndexNew;
            break;
        }
        else {
            argIndex = argIndexNew;
        }
    }

    /* Check for unknown/invalid command line arguments */
    if ((argIndex < argc) ||
        (axisNumber  >= MEIXmpMAX_Axes) ||
        (motorNumber >= MEIXmpMAX_Motors)) {
        meiPlatformConsole("usage: %s %s\n"
                           "\t\t[-axis # (0 .. %d)]\n"
                           "\t\t[-motor # (0 .. %d)]\n",
                            argv[0],
                            ArgUSAGE,
                            MEIXmpMAX_Axes - 1,
                            MEIXmpMAX_Motors - 1);
        exit(MPIMessageARG_INVALID);
    }

    context->motorNumber = motorNumber;

    context->axisNumber =
        (axisNumber < 0)
            ? motorNumber
            : axisNumber;

    context->control =
        mpiControlCreate(context->controlType,
                         &context->controlAddress);
    msgCHECK(mpiControlValidate(context->control));

    /* Initialize motion controller */
    returnValue = mpiControlInit(context->control);
    msgCHECK(returnValue);

    /* Create axis object */
    context->axis =
        mpiAxisCreate(context->control,
                      context->axisNumber);
    msgCHECK(mpiAxisValidate(context->axis));

    /* Create motor object */
    context->motor =
        mpiMotorCreate(context->control,
                       context->motorNumber);
    msgCHECK(mpiMotorValidate(context->motor));

    returnValue =
        mpiMotorConfigGet(context->motor,
                          &context->motorConfig,
                          NULL);
    msgCHECK(returnValue);

    context->encoderPhase = context->motorConfig.encoderPhase;

    return (returnValue);
}

long
    contextDelete(Context   context)
{
    long    returnValue = MPIMessageOK;

    if (context->motor != MPIHandleVOID) {
        /* Set to original configuration */
        context->motorConfig.encoderPhase = context->encoderPhase;

        returnValue =
            mpiMotorConfigSet(context->motor,
                              &context->motorConfig,
                              NULL);

        if (returnValue == MPIMessageOK) {
            returnValue = mpiMotorDelete(context->motor);

            if (returnValue == MPIMessageOK) {
                context->motor = MPIHandleVOID;
            }
        }
    }

    if (returnValue == MPIMessageOK) {
        if (context->axis != MPIHandleVOID) {
            returnValue = mpiAxisDelete(context->axis);

            if (returnValue == MPIMessageOK) {
                context->axis = MPIHandleVOID;
            }
        }
    }

    if (returnValue == MPIMessageOK) {
        if (context->control != MPIHandleVOID) {
            returnValue = mpiControlDelete(context->control);

            if (returnValue == MPIMessageOK) {
                context->control = MPIHandleVOID;
            }
        }
    }

    return (returnValue);
}

void
    positionDisplay(double  position)
{
    printf("Actual Position: %8.0lf\r",
            position);
}

double
    positionGet(Context context)
{
    long    returnValue;

    double  actualPos;

    returnValue =
        mpiAxisActualPositionGet(context->axis,
                                 &actualPos);
    msgCHECK(returnValue);

    return (actualPos);
}