#include "UQuimMotor.hpp" 
#include "od_write_master.h"
#include <stdio.h>
 
// Register the UMachine UObject in the Urbi world. 
UStart(UQuimMotor); 
 
// Bouncing the name to the UObject constructor is mandatory. 
UQuimMotor::UQuimMotor(const std::string& name) 
  : urbi::UObject(name), motorId_m(0) 
{ 
  // Register the Urbi constructor.  This is the only mandatory 
  // part of the C++ constructor. 
  UBindFunction(UQuimMotor, init); 
} 
 
int UQuimMotor::init(int motor_id) 
{ 
  if (! (motor_id == 1 || motor_id == 2) )
  {
    printf("Invalid motor_id\r\n");
    return 1;
  }
  motorId_m = motor_id ;
  
  canSocket_m = init_can("can0");
  if (canSocket_m < 0)
  {
    printf("Failed to init CAN...\r\n");
    return 1;
  }

  // Bind the UVars before using them. 
  UBindVar(UQuimMotor, speed); 

  // Request that speed_set be invoked each time speed is changed.
  UNotifyChange(speed, &UQuimMotor::speed_set); 
 
  // Success. 
  return 0; 
} 

int UQuimMotor::speed_set(urbi::UVar& v) 
{ 
  // Speed entry in QuimMotor entry is at index 0x60FF
  // subidx indicates which motor to set
  od_write(canSocket_m, 0x60FF, motorId_m, int(v));
  return 0 ;
}