/*!
  \example controller/samples/estimateWiiPoseSample.cpp

  estimateWiiPose の動作サンプル

  \author Satofumi KAMIMURA

  $Id: estimateWiiPoseSample.cpp 1392 2009-10-10 10:59:39Z satofumi $
*/

#include <SDL.h>
#include "WiiJoystick.h"
#include "WiiUtils.h"
#include "Angle.h"
#include "delay.h"
#include "MathUtils.h"

using namespace qrk;


int main(int argc, char *argv[]) {

  WiiJoystick joystick;
  if (! joystick.connect()) {
    printf("WiiJoystick::connect: %s\n", joystick.what());
    exit(1);
  }
  joystick.setAccelerationAverageSize(4);
  printf("O.K.\n");

  while (1) {
    Point3d<double> acceleration;
    joystick.acceleration(acceleration);

    // 姿勢情報
    Angle vertical_direction;
    Angle wii_rotation;
    estimateWiiPose(vertical_direction, wii_rotation, acceleration);

#if 1
    printf("%.2f # [deg], %d [deg], ",
           180.0 * vertical_direction.to_rad() / M_PI, wii_rotation.to_deg());

    // 加速度情報
    printf("%.2f, %.2f, %.2f, ",
           acceleration.x, acceleration.y, acceleration.z);
    printf("\n");

#else
    printf("%d [deg], %d [deg], ",
           vertical_direction.to_deg(), wii_rotation.to_deg());

    // 加速度情報
    printf("%.2f, %.2f, %.2f, ",
           acceleration.x, acceleration.y, acceleration.z);
    printf("\n");
#endif

    delay(25);
  }

  return 0;
}
