/***************************************************************
 * PrForceSensorJr3.cpp
 *
 * Implementation of JR3 force sensor functionality that is common
 * across both the PCI and ISA versions. 
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 02/23/03: Weyn Ong : Created; based on code by Jaeheung Park.
 */

#include "PrJr3Defn.h"
#include "PrForceSensorJr3.h"
#include <stdio.h>

// Protected constructor.  This object can only be instantiated by
// a derived class.
PrForceSensorJr3::PrForceSensorJr3()
    : PrForceSensorIntf( PR_JR3_CHANNEL_COUNT, PR_JR3_BIT_COUNT ) {
    // Doesn't need to do anything.
}

Float 
PrForceSensorJr3::forceMax() {
    return -1.0;    // NOT IMPLEMENTED
}

Float 
PrForceSensorJr3::torqueMax() {
    return -1.0;    // NOT IMPLEMENTED
}

int 
PrForceSensorJr3::values( Float * data ) {

    ForceArray forceArray = getForces( 0 );
    memcpy( data, &forceArray, sizeof(Float) * PR_JR3_CHANNEL_COUNT );

    return ( PR_DRIVER_OK );
}

int 
PrForceSensorJr3::value(int channel,Float &data) {
    /* is the channel 0-5 */ 
    if ( ( channel < 0 ) || ( channel > PR_JR3_CHANNEL_COUNT-1 ) )
        return ( PR_DRIVER_ERROR );

    int forceValue = readJr3( PR_JR3_FILTER0_ADDR + channel );
    forceValue = *(signed short *)(&forceValue);
    data = fullScale_.axis[channel] * ((float) forceValue / 16384);

    return ( PR_DRIVER_OK );
}

PrForceSensorJr3::ForceArray
PrForceSensorJr3::getForces( int filterNum ) {
    int axload;
    ForceArray filtered_forces;
    int force_address;
    int i;

    force_address = PR_JR3_FILTER0_ADDR + 0x0008*filterNum;

    for (i = 0; i <= 5; i++) { 
        axload = readJr3( force_address + i );
        axload = *(signed short *)(&axload);
        filtered_forces.axis[i] = fullScale_.axis[i] * ((float)axload/16384); 
    }

    return filtered_forces;
}

PrForceSensorJr3::ForceArray
PrForceSensorJr3::getDerivativeForces( int filterNum ) {
    int axrate;
    ForceArray filtered_rate;
    int rate_address;
    int i;

    rate_address = PR_JR3_FILTER0_ADDR + 0x0008*filterNum ;
    writeJr3( PR_JR3_FADDR_FOR_RATE, rate_address );

    for (i = 0; i <= 5; i++) {                                         
        axrate = readJr3( PR_JR3_RATE_ADDR + i );
        axrate = *(signed short *)(&axrate);
        filtered_rate.axis[i] = fullScale_.axis[i] * ((float)axrate/16384);  
    }

    return filtered_rate;
}

unsigned int
PrForceSensorJr3::getRateDivisor() {
    return readJr3( PR_JR3_RATE_DIVISOR );
}

void
PrForceSensorJr3::setRateDivisor( unsigned int divisor ) {
    writeJr3( PR_JR3_RATE_DIVISOR, divisor );
}

unsigned int
PrForceSensorJr3::getErrorCount() {
    return readJr3( PR_JR3_ERROR_COUNT_ADDR );
}

unsigned int
PrForceSensorJr3::getSaturationWarning() {
    return readJr3( PR_JR3_WARN_BITS_ADDR );
}

unsigned int
PrForceSensorJr3::getErrorMessages() {
    return readJr3( PR_JR3_ERROR_BITS_ADDR );
}

int
PrForceSensorJr3::zero() {
    writeJr3( PR_JR3_COMM0_ADDR, PR_JR3_ZERO_OFFSETS );
    return ( PR_DRIVER_OK );
}

void
PrForceSensorJr3::initialize() {
    zero();
    getFullScales();
}

void
PrForceSensorJr3::getFullScales() {
    for ( int i = 0; i < 6; i++ ) {
        int scale = readJr3( PR_JR3_FULL_SCALE_ADDR + i );
        scale = *(signed short *)(&scale);
        fullScale_.axis[i] = (float) scale;
    }
}
