#include <XBee.h>
#include <NewSoftSerial.h>
#include <Wire.h>
#include <ADXL345.h>
#include "Walker.h"

Walker w = Walker();
ADXL345* accelADXL;

void setup(){
  accelADXL = (ADXL345*) malloc( sizeof( ADXL345 ) );
  accelADXL->init();
  //              accelADXL = (ADXL345*) new ADXL345();
  accelADXL->powerOn();
  accelADXL->setJustifyBit( false );
  accelADXL->setFullResBit( true );
  accelADXL->setRangeSetting( 16 ); // 2: 2g, 4: 4g, 8: 8g, 16: 16g

  w.setup('D');
  pinMode(7,INPUT);
}

void dataFromInt14b(uint8_t * data, uint16_t  output, int offset ){
    data[offset]   = byte(output/128); // encode data to twice 7 bit, data should not be longer then 14 bit
    data[offset+1] = byte(output%128);
}

int accx, accy, accz;
uint8_t * buf;

uint16_t count;

void loop()
{
	w.loopStep();  

	if(digitalRead(7) == LOW) {
		count++;
		if(count == 0x01ff) {
			 count = 0;
			 accelADXL->readAccel( &accx, &accy, &accz );
			 dataFromInt14b(buf, 0, 0);         //ADDR 0 
			 buf[2] = 42;			// skip one byte for rssi...
			 dataFromInt14b(buf, accx, 3); 
			 dataFromInt14b(buf, accy, 5);
			 dataFromInt14b(buf, accz, 7);
			 w.sendDToHost(buf,9);
		}
	}
}