void Accelerometer::setRate(float rate){
  byte _b,_s;
  int v = (int) (rate / 6.25);
  int r = 0;
  while (v >>= 1)
  {
    r++;
  }
  if (r <= 9) { 
    readFrom(DEVICE, ADXL345_BW_RATE, 1, &_b);
    _s = (byte) (r + 6) | (_b & B11110000);
    writeTo(DEVICE, ADXL345_BW_RATE, _s);
  }
}

bool ObbFile::removeFrom(int fd)
{
    if (fd < 0) {
        return false;
    }

    if (!readFrom(fd)) {
        return false;
    }

    ftruncate(fd, mFooterStart);

    return true;
}

void Visitor<Reader<DataStream>>::readFrom(
        const Space::ViewportModel& vp)
{
    readFrom(static_cast<const IdentifiedObject<Space::ViewportModel>&>(vp));
    m_stream
            << vp.name()
            << vp.transform()
            << vp.xDim()
            << vp.yDim()
            << vp.m_defaultValuesMap
            << vp.renderPrecision();

    insertDelimiter();
}

ArrayGraph::ArrayGraph(const GraphAttributes& GA, const EdgeArray<float>& edgeLength, const NodeArray<float>& nodeSize) :
    m_numNodes(0),
    m_numEdges(0),
    m_nodeXPos(0),
    m_nodeYPos(0),
    m_nodeSize(0),
    m_nodeMoveRadius(0),
    m_desiredEdgeLength(0),
    m_nodeAdj(0),
    m_edgeAdj(0)
{
    allocate(GA.constGraph().numberOfNodes(), GA.constGraph().numberOfEdges());
    readFrom(GA, edgeLength, nodeSize);
};

ISCORE_PLUGIN_SCENARIO_EXPORT void Visitor<Reader<DataStream>>::readFrom(const Scenario::StateModel& s)
{
    readFrom(static_cast<const IdentifiedObject<Scenario::StateModel>&>(s));

    // Common metadata
    readFrom(s.metadata);

    m_stream << s.m_eventId
             << s.m_previousConstraint
             << s.m_nextConstraint
             << s.m_heightPercentage;

    // Message tree
    m_stream << s.m_messageItemModel->rootNode();

    // Processes plugins
    m_stream << (int32_t) s.stateProcesses.size();
    for(const auto& process : s.stateProcesses)
    {
        readFrom(process);
    }

    insertDelimiter();
}

void IMU3000::setSampleRate(byte divider, byte lowpass)
{
	//writing divider
	writeTo(IMU3000_REG_SMPLRT_DIV,divider);

	//writing lowpass rate freq
	byte lpf;
	readFrom(IMU3000_REG_DLPF, 1, &lpf);
	
	lpf &= ~IMU3000_DLPF_CFG_MASK;
	lpf |= lowpass;

	writeTo(IMU3000_REG_DLPF, lpf);  

}

SpriteAnimationCollection * SpriteAnimationCollection::readFrom(const char * spriteAnimationDefinitionFileName) {
	if(spriteAnimationDefinitionFileName == NULL || Utilities::stringLength(spriteAnimationDefinitionFileName) == 0) { return NULL; }
	
	SpriteAnimationCollection * newSpriteAnimationCollection = NULL;

	FileReader input(spriteAnimationDefinitionFileName);
	if(!input.open()) {
		return NULL;
	}
	
	newSpriteAnimationCollection = readFrom(input);

	input.close();
	
	return newSpriteAnimationCollection;
}

void setRegisterBit(uint8 regAdress, uint8 bitPos, uint8 state)
{
    uint8 _b;

    readFrom(regAdress, 1, &_b);

    if (state)
    {
        _b |= (1 << bitPos);                        // forces nth bit of _b to be 1.  all other bits left alone.
    }
    else
    {
        _b &= ~(1 << bitPos);                       // forces nth bit of _b to be 0.  all other bits left alone.
    }

    writeTo(regAdress, _b);
}

void Visitor<Reader<DataStream>>::readFrom(const CurveSegmentModel& segmt)
{
    // To allow recration using createProcess
    m_stream << segmt.name();

    // Save the parent class
    readFrom(static_cast<const IdentifiedObject<CurveSegmentModel>&>(segmt));

    // Save this class (this will be loaded by writeTo(*this) in CurveSegmentModel ctor
    m_stream << segmt.previous() << segmt.following()
             << segmt.start() << segmt.end();

    // Save the subclass
    segmt.serialize(toVariant());

    insertDelimiter();
}

bool ObbFile::readFrom(const char* filename)
{
    int fd;
    bool success = false;

    fd = ::open(filename, O_RDONLY);
    if (fd < 0) {
        ALOGW("couldn't open file %s: %s", filename, strerror(errno));
        goto out;
    }
    success = readFrom(fd);
    close(fd);

    if (!success) {
        ALOGW("failed to read from %s (fd=%d)\n", filename, fd);
    }

out:
    return success;
}

void acc_adxl345_read_xyz(int16 *x, int16 *y, int16 *z)
{
    uint8 _buff[6];
    readFrom(ADXL345_DATAX0, ADXL345_TO_READ, _buff); //read the acceleration data from the ADXL345


    //pc.printf("%d, %d\t%d, %d\t%d, %d\r\n", _buff[0], _buff[1], _buff[2], _buff[3], _buff[4], _buff[5]);


    // each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
    // thus we are converting both bytes in to one int

    *x = (int16)((((uint16)_buff[1]) << 8) | _buff[0]);
    *y = (int16)((((uint16)_buff[3]) << 8) | _buff[2]);
    *z = (int16)((((uint16)_buff[5]) << 8) | _buff[4]);

    //pc.printf("%d, %d, %d\r\n", *x, *y, *z);


}

int	IMU3000::getRange()
{
	byte rng;
	readFrom(IMU3000_REG_DLPF, 1, &rng);
	switch(rng)
	{
		case RANGE_250:
			return 250;
			break;
		case RANGE_500:
			return 500;
			break;
		case RANGE_1000:
			return 1000;
			break;
		case RANGE_2000:
			return 2000;
			break;
	}
	return 0;
}

/* =================================================
FUNCTION: getAccelerometerData

CREATED: 16-05-2014

DESCRIPTION: This function gathers raw sensor data.

PARAMETERS: result

GLOBAL VARIABLES: None.

RETURNS: result

AUTHOR: P. Kantue

================================================== */
void getAccelerometerData(int * result) {
  int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
  int buff[A_TO_READ];
  
  readFrom(ACC, regAddress, A_TO_READ, buff); //read the acceleration data from the ADXL345
  
  //each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significant Byte first!!
  //thus we are converting both bytes in to one int
  
  // when the sensor is attached upright on board
  result[2] = (((int)buff[1]) << 8) | buff[0];   
  result[0] = (((int)buff[3])<< 8) | buff[2];
  result[1] = (((int)buff[5]) << 8) | buff[4];
  
  // correct for sign change so that NED axis system is retained
  result[0] = -result[0];

  
  // when the sensor is attached flat on board
  //result[0] = (((int)buff[1]) << 8) | buff[0];   
  //result[1] = (((int)buff[3])<< 8) | buff[2];
  //result[2] = (((int)buff[5]) << 8) | buff[4];
}

int main (int argc, char *argv[])
{
    int fd;
    ssize_t  numRead, numWrite;
    int openFlags = O_CREAT | O_WRONLY | O_APPEND;
    mode_t filePerms = S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP |
                      S_IROTH |S_IWOTH;
    char *buf;
    buf = malloc(MAX_READ);
    if (buf == NULL) {
        printf("%s\n", "mallocate failed");
        return 0;
    }
    if (argc == 1) {
       readFrom(fd, buf, MAX_READ);
    }
    else if (argc == 2) {
       fd = open(argv[1], openFlags, filePerms);
       if (fd == -1) {
           printf("%s\n", "open failed");
           return 0;
       }
       while (1) {
            numRead = read(STDIN_FILENO, buf, MAX_READ);
            if (numRead == -1) {
                printf("%s\n", "read failed");
                return 0;
            }
            numWrite = write(fd, buf, numRead);
            if ( numWrite == -1) {
                printf("%s\n", "open failed");
                return 0;
            }
       }
    }
    return 0;
}

// Sets the range setting, possible values are: 2, 4, 8, 16
void Accelerometer::setRangeSetting(int val) {
  byte _s;
  byte _b;

  switch (val) {
  case 2:  
    _s = B00000000; 
    break;
  case 4:  
    _s = B00000001; 
    break;
  case 8:  
    _s = B00000010; 
    break;
  case 16: 
    _s = B00000011; 
    break;
  default: 
    _s = B00000000;
  }
  readFrom(DEVICE, ADXL345_DATA_FORMAT, 1, &_b);
  _s |= (_b & B11101100);
  writeTo(DEVICE, ADXL345_DATA_FORMAT, _s);
}

// Sets the range setting, possible values are: 2, 4, 8, 16
void CAdxl345::setRangeSetting(int val) {
	byte _s;
	byte _b;

	switch (val) {
	case 2:
		_s = 0x00;
		break;
	case 4:
		_s = 0x01;
		break;
	case 8:
		_s = 0x02;
		break;
	case 16:
		_s = 0x03;
		break;
	default:
		_s = 0x00;
	}
	readFrom(DEVICE, ADXL345_DATA_FORMAT, 1, &_b);
	_s |= (_b & 0x00EC);
	writeTo(DEVICE, ADXL345_DATA_FORMAT, _s);
}

void ITG3200::setZgyroStandby(bool _Status) {
  int a=0;
  a=readFrom( _dev_address,PWR_MGM, 1, &_buff[0]);
  a=writeTo( _dev_address,PWR_MGM, ((_buff[0] & PWRMGM_STBY_ZG) | _Status << 3));
}

byte ITG3200::getClockSource() {  
  int a=0;
  a=readFrom( _dev_address,PWR_MGM, 1, &_buff[0]);
  return (_buff[0] & PWRMGM_CLK_SEL);
}