#include "walkerbot.h"
#include <stdlib.h>

/*
 legs = number of legs
 dof = joints per leg
*/
void setParamiters( walker_t *this, int legs, int dof) {
  this->legs   = legs;
  this->legdof = dof;
}

int footIndex( walker_t *this, int side, int leg) {
  return leg+(side*this->legs);
}

int axisIndex(walker_t *this, int side, int leg, int axis) {
  return axis+(leg*this->legdof)+(side*this->legdof*this->legs);
}

void walkerInit( walker_t *this ) {
  if (this->footPosition != NULL) {
    free(this->footPosition);
  }
  debug("Allocating for %d feet\n", this->legs);
  this->footPosition = malloc (sizeof(point3d_t)* this->legs);
  
  if (this->jointAngle != NULL) {
    free(this->jointAngle);
  }
  debug("Allocating for %d joints\n", this->legs*this->legdof);
  this->jointAngle = malloc (sizeof(point3d_t)* this->legs*this->legdof);
}

void walkerFree( walker_t *this) {
  free( this->footPosition);
  free (this->jointAngle);

}

void accel2velocity( walker_t *this){
  this->velocity.x += this->acceleration.x * dt;
  this->velocity.y += this->acceleration.y * dt;
  this->velocity.z += this->acceleration.z * dt;
}

void velocity2position( walker_t *this){
  this->position.x += this->velocity.x * dt;
  this->position.y += this->velocity.y * dt;
  this->position.z += this->velocity.z * dt;
}

void velocity2orientation( walker_t *this) {
  this->orientation.z = dir(this->velocity.x, this->velocity.y);
}