#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"

#include <iostream>

//std::string robot_device="A603LOBS";
std::string robot_device="A4008atn";
std::string config_file="../src/xml/darwin_action.xml";

int main(int argc, char *argv[])
{
  int i=0,num_servos;
  std::vector<int> servos;
  std::vector<double> angles,speeds,accels,offsets;

  try{
    CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
    darwin.mm_load_config(config_file);
    num_servos=darwin.mm_get_num_servos();
    std::cout << "Found " << num_servos << " servos" << std::endl;
    std::cout << "Motion manager period " << darwin.mm_get_period() << " ms" << std::endl;
    // execute the walk ready action
    std::cout << "Assign servos to the action module" << std::endl;
    for(i=1;i<=20;i++)
    {
      darwin.mm_enable_servo(i);
      darwin.mm_assign_module(i,DARWIN_MM_ACTION);
    }
    for(i=0;i<=MAX_NUM_SERVOS;i++)
    {
      if(darwin.mm_is_servo_enabled(i))
        std::cout << "Servo " << i << " enabled" << std::endl;
      else
        std::cout << "Servo " << i << " disabled" << std::endl;
      std::cout << "Servo " << i;
      switch(darwin.mm_get_module(i))
      {
        case DARWIN_MM_NONE: std::cout << " not assigned to any module" << std::endl;
                             break;
        case DARWIN_MM_ACTION: std::cout << " assigned to the action module" << std::endl;
                               break;
        case DARWIN_MM_WALKING: std::cout << " assigned to the walking module" << std::endl;
                                break;
        case DARWIN_MM_JOINTS: std::cout << " assigned to the joints module" << std::endl;
                               break;
        case DARWIN_MM_HEAD: std::cout << " assigned to the head module" << std::endl;
                             break;
        default: std::cout << " assigned to an unknown module" << std::endl;
                 break;
      }
    }
    if(darwin.mm_is_running())
      std::cout << "Motion manager is running" << std::endl;
    else
      std::cout << "Motion manager is not running" << std::endl;
    darwin.mm_start();
    if(darwin.mm_is_running())
      std::cout << "Motion manager is running" << std::endl;
    else
      std::cout << "Motion manager is not running" << std::endl;
    sleep(10);
    darwin.mm_stop();
    if(darwin.mm_is_running())
      std::cout << "Motion manager is running" << std::endl;
    else
      std::cout << "Motion manager is not running" << std::endl;
    if(darwin.mm_is_power_enabled())
      std::cout << "Servos are powered" << std::endl;
    else
      std::cout << "Servos are not powered" << std::endl;
    darwin.mm_enable_power();
    if(darwin.mm_is_power_enabled())
      std::cout << "Servos are powered" << std::endl;
    else
      std::cout << "Servos are not powered" << std::endl;
    darwin.mm_disable_power();
    if(darwin.mm_is_power_enabled())
      std::cout << "Servos are powered" << std::endl;
    else
      std::cout << "Servos are not powered" << std::endl;
    std::cout << "Servo offsets: " << std::endl;
    offsets=darwin.mm_get_servo_offsets();
    for(i=0;i<MAX_NUM_SERVOS;i++)
      std::cout << "  Servo " << i << ": " << offsets[i] << std::endl;
    darwin.mm_set_servo_offset(5,3.3);
    std::cout << "Servo offsets: " << std::endl;
    offsets=darwin.mm_get_servo_offsets();
    for(i=0;i<MAX_NUM_SERVOS;i++)
      std::cout << "  Servo " << i << ": " << offsets[i] << std::endl;
  }catch(CException &e){
    std::cout << e.what() << std::endl;
  }
}