#define BR_IRGAIN 10
#define fwSpeed 50

int Connections_A[9] = {5, 1, 2, 5, -15, -6, -2, 2, 7};
int Connections_B[9] = {2, -2, -6, -15, 5, 2, 1, 5, 7};

struct Robot {
  int tabsens[9];
  int left_speed;
  int right_speed;
};

int RobotBehaviour(struct Robot *system) {
  long int lspeed16, rspeed16;
  int i;

  lspeed16 = 0;
  rspeed16 = 0;

  for(i=0; i<9; i++) {
    lspeed16 -= Connections_B[i] * system->tabsens[i];
    rspeed16 -= Connections_A[i] * system->tabsens[i];
  }
  system->left_speed = ((lspeed16 / BR_IRGAIN) + fwSpeed);
  system->right_speed = ((rspeed16 / BR_IRGAIN) + fwSpeed);

  if(system->left_speed > 0 && system->left_speed < 30)
    system->left_speed = 30;
  if(system->left_speed < 0 && system->left_speed > -30)
    system->left_speed = -30;
  if(system->right_speed > 0 && system->right_speed < 30)
    system->right_speed = 30;
  if(system->right_speed < 0 && system->right_speed > -30)
    system->right_speed = -30;

  if(system->left_speed > 60 || system->left_speed < -60)
    system->left_speed = 0;
  if(system->right_speed > 60 || system->right_speed < -60) 
    system->right_speed = 0;

  return 0;
}

int main(int arc, char *argv[]) {
  char **service;
  int num = 1, i, agent_id, mutex_id = 55;
  MCAgent_t agent;

  service = (char **)malloc(sizeof(char *)*num);
  for(i=0; i<num; i++) {
    service[i] = (char *)malloc(sizeof(char)*20);
  }
  strcpy(service[0], "RobotBehaviour");

  agent = mc_FindAgentByName("service_provider_1");
  agent_id = mc_GetAgentID(agent);

  mc_MutexLock(mutex_id);
  mc_DeregisterService(agent_id, service[0]);
  mc_RegisterService(mc_current_agent, service, num);
  mc_MutexUnlock(mutex_id);

  printf("Service provider 2 has arrived.\n");
  printf("Services provided:\n"); 
  for(i=0; i<num; i++) {
    printf("%s\n", service[i]);
  }

  for(i=0; i<num; i++) {
    free(service[i]);
  }
  free(service);

  return 0;
}