/***************************************************************
 * PrEncoderPC7166.cpp
 *
 * This is an implementation of the encoder class for PC7166 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 "PrEncoderPC7166.h"
#include "PrPC7166Defn.h"
#include "PrRobotDefn.h"

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

// encoder 24bits
// constructor sets flag
// flag() should be called before using
PrEncoderPC7166::PrEncoderPC7166()
: PrEncoderIntf(PC7166_ENCODER_CHANNEL_COUNT,PC7166_ENCODER_BIT_COUNT)
{
    // meter/tick
    const Float encoderDistPerCountGripper[] = PR_GRIPPER_DIST_PER_COUNT;
    const int encoderDistCountZeroGripper[] = PR_GRIPPER_ENCODER_COUNT_ZERO;

    const Float encoderDistPerCountBase[] = PR_BASE_DIST_PER_COUNT;  
    const int encoderDistCountZeroBase[] = PR_BASE_ENCODER_COUNT_ZERO;  

    encoderDistPerCount_ = new Float[channelCount()];
    encoderDistCountZero_ = new int[channelCount()];

    for(int i=0;i<PR_GRIPPER_NOJ;i++)
      {
	  encoderDistPerCount_[i+PR_GRIPPER_ENCODER_CHANNEL_START] 
	    = encoderDistPerCountGripper[i];
	  encoderDistCountZero_[i+PR_GRIPPER_ENCODER_CHANNEL_START]  
	    = encoderDistCountZeroGripper[i];
      }

    for(int j=0;j<PR_BASE_NOJ;j++)
      {
	  encoderDistPerCount_[j+PR_BASE_ENCODER_CHANNEL_START] 
	    = encoderDistPerCountBase[j];
	  encoderDistCountZero_[j+PR_BASE_ENCODER_CHANNEL_START] 
	    = encoderDistCountZeroBase[j];
      }

    // Enable permissions for I/O access
    ThreadCtl( _NTO_TCTL_IO, 0 );
    mmap_device_io( PC7166_ADDR_SPACE_SIZE, PC7166_BASE_ADDR );

    flag(PR_DRIVER_OK);
}

PrEncoderPC7166::~PrEncoderPC7166()
{
    delete[] encoderDistPerCount_;
    delete[] encoderDistCountZero_;
}

int PrEncoderPC7166::zero()
{
  return valuesPreset(encoderDistCountZero_);
}

// preset encoder values with data
int PrEncoderPC7166::valuesPreset(const int *data)
{
    for(int i=0;i<channelCount();i++)
      {
	  int value = data[i];
	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_MASTER_RESET);
	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_INPUT_SETUP);
	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_QUAD_X4);
	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_CNTR_RESET);

	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_ADDR_RESET);

	  hwIO().output1B(PC7166_BASE_ADDR+2*i, value&0xFF);
	  hwIO().output1B(PC7166_BASE_ADDR+2*i, (value >> 8)&0xFF);
	  hwIO().output1B(PC7166_BASE_ADDR+2*i, (value >> 16)&0xFF);

	  hwIO().output1B(PC7166_BASE_ADDR+(2*i+1),PC7166_ENCODER_PRESET_CTR);
      }
    return PR_DRIVER_OK;
}

// read the channel value and return it(meter)
Float PrEncoderPC7166::channelValue(int channel)
{
    hwIO().output1B(PC7166_BASE_ADDR+(2*channel+1), PC7166_ENCODER_LATCH_CNTR);

    hwIO().output1B(PC7166_BASE_ADDR+(2*channel+1), PC7166_ENCODER_ADDR_RESET);
    unsigned int bits = hwIO().input1B(PC7166_BASE_ADDR+2*channel);
    bits += hwIO().input1B(PC7166_BASE_ADDR+2*channel) << 8;
    bits += hwIO().input1B(PC7166_BASE_ADDR+2*channel) << 16;

    int value = bits - encoderDistCountZero_[channel];

    return BitsToDist(channel,value);
}

// private methods

// convert encoder counts to distance(meter)
inline Float PrEncoderPC7166::BitsToDist(int channel,int bits)
{
    return (bits*encoderDistPerCount_[channel]); 
}

// convert distance(meter) to encoder counts
inline int PrEncoderPC7166::DistToBits(int channel,Float dist)
{
    return (int)(dist/encoderDistPerCount_[channel]); 
}
