/***************************************************************
 * PrEncoderTRC.cpp
 *
 * This is an implementation of the encoder class for TRC board.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 10/26/97: K.C. Chang: created.
 * 12/18/97: K.C. Chang: got rid of globals.
 * 02/22/03: Weyn Ong  : Version for QNX 6.  I/O permissions must be enabled
 *                       for port access.
 */

#include "PrEncoderTRC.h"
#include "PrRobotDefn.h"
#include "PrTRCDefn.h"

#include <sys/mman.h>
#include <sys/neutrino.h>

// encoder 16bits
// constructor sets flag
// flag() should be called before using
PrEncoderTRC::PrEncoderTRC()
: PrEncoderIntf(TRC_ENCODER_CHANNEL_COUNT,TRC_ENCODER_BIT_COUNT)
{
    const int encoderCountPerRev[] = PR_ARM_ENCODER_COUNT_PER_REV;
    const int encoderCountZero[] = PR_ARM_ENCODER_COUNT_ZERO;

    encoderCountPerRev_ = new int[channelCount()];
    encoderCountZero_ = new int[channelCount()];

    for(int i=0;i<channelCount();i++)
      {
	  encoderCountPerRev_[i] = encoderCountPerRev[i];
	  encoderCountZero_[i] = encoderCountZero[i];
      }


    unsigned int bits;
    
//     bits = hwIO().input2B(TRC_BASE_ADDR + TRC_ENCODER_COUNT_BASE 
// 			  + 2*TRC_ENCODER_LAST_CHANNEL) & bitMask();
//     if (swCounterPreset(TRC_ENCODER_LAST_CHANNEL,bits) != bits)
//       flag(PR_DRIVER_ERROR);

//     bits = hwIO().input2B(TRC_BASE_ADDR + TRC_ENCODER_COUNT_BASE 
// 			       + 2*(TRC_ENCODER_LAST_CHANNEL - 2)) & bitMask();
//     if (swCounterPreset(TRC_ENCODER_LAST_CHANNEL-2,bits) != bits)
    //      flag(PR_DRIVER_ERROR);

    for(int j=0;j<channelCount();j++)
      {

	bits = hwIO().input2B(TRC_BASE_ADDR + TRC_ENCODER_COUNT_BASE 
			      + 2*j) & bitMask();
	if (swCounterPreset(j,bits) != bits)
	  flag(PR_DRIVER_ERROR);
      }

    // Enable permissions for I/O access
    ThreadCtl( _NTO_TCTL_IO, 0 );
    mmap_device_io( TRC_ADDR_SPACE_SIZE, TRC_BASE_ADDR );

    if (flag() != PR_DRIVER_ERROR)
      flag(PR_DRIVER_OK);
}

PrEncoderTRC::~PrEncoderTRC()
{
    delete[] encoderCountPerRev_;
    delete[] encoderCountZero_;
}

int PrEncoderTRC::zero()
{
  return valuesPreset(encoderCountZero_);
}

// preset encoder values with data
int PrEncoderTRC::valuesPreset(const int *data)
{
    for(int i=0;i<channelCount();i++)
      {
	  int value;
// 	  if (i==TRC_ENCODER_LAST_CHANNEL)
// 	    value = swCounterPreset(i,data[i]);
// 	  else if (i==TRC_ENCODER_LAST_CHANNEL-2)
// 	    value = swCounterPreset(i,data[i]);
// 	  else
// 	    value = data[i];

	  value = swCounterPreset(i,data[i]);
	  if (hwIO().output2B(TRC_BASE_ADDR+TRC_ENCODER_LOAD_BASE+2*i,
			      value & bitMask()) != value)
	    return PR_DRIVER_ERROR;
      }
    return PR_DRIVER_OK;
}

// read the channel value and return it(rad)
Float PrEncoderTRC::channelValue(int channel)
{
    unsigned int bits = hwIO().input2B(TRC_BASE_ADDR + TRC_ENCODER_COUNT_BASE 
				       + 2*channel) & bitMask();
    int value = -encoderCountZero_[channel];
//     if (channel==TRC_ENCODER_LAST_CHANNEL)
//       value += swCounterMerge(channel,bits);
//     else if (channel==TRC_ENCODER_LAST_CHANNEL-2)
//       value += swCounterMerge(channel,bits);
//     else
//       value += bits;
    value += swCounterMerge(channel,bits);

    return BitsToRad(channel,value);
}

// private methods

// convert encoder counts to angle(rad)
inline Float PrEncoderTRC::BitsToRad(int channel,int bits)
{
    return (2*M_PI*bits/(Float)encoderCountPerRev_[channel]); 
}

// convert angle(rad) to encoder counts
inline int PrEncoderTRC::RadToBits(int channel,Float rad)
{
    return (int)((rad/(2*M_PI))*(Float)encoderCountPerRev_[channel]); 
}

