/*!
  \file
  \brief ロボットの制御

  \author Satofumi KAMIMURA

  $Id: robot_control.c 1687 2010-02-11 05:26:50Z satofumi $
*/

#include "robot_control.h"
#include "robot_macro.h"
#include "system_handler.h"
#include "mtu_control.h"
#include "pwm_control.h"
#include "encoder_control.h"
#include "motor_control.h"
#include "wheel_control.h"
#include "body_control.h"
#include "position_calculate.h"
#include "path_manage.h"
#include "follow_control.h"
#include "direct_handler.h"


static robot_t* robot_ = 0x0;


static void initialize_modules(robot_t *robot)
{
  int i;

  robot->reset = 0;
  system_initialize(&robot->system);

  mtu_initialize();
  for (i = 0; i < 2; ++i) {
    encoder_initialize(i, &robot->encoder[i]);
    motor_initialize(i, &robot->motor[i]);
    wheel_initialize(&robot->wheel[i]);
  }
  body_initialize(&robot->body);
  position_initialize(&robot->position);
  position_initialize(&robot->follow_position);
  path_initialize(&robot->path);
  follow_initialize(&robot->straight, FOLLOW_T_DEFAULT_STRAIGHT_ACCELERATION);
  follow_initialize(&robot->rotate, FOLLOW_T_DEFAULT_ROTATE_ACCELERATION);

  direct_initialize(&robot->direct);
}


void robot_initialize(robot_t *robot)
{
  robot_ = robot;
  initialize_modules(robot);
}


void robot_reset(void)
{
  int i;

  system_reset(&robot_->system);

  for (i = 0; i < 2; ++i) {
    encoder_reset(&robot_->encoder[i]);
    motor_reset(&robot_->motor[i]);
    wheel_reset(&robot_->wheel[i]);
  }

  body_reset(&robot_->body);
  position_reset(&robot_->position);
  position_reset(&robot_->follow_position);

  path_reset(&robot_->path);
  follow_reset(&robot_->straight);
  follow_reset(&robot_->rotate);

  direct_reset(&robot_->direct);
}


static void setWheelVelocity(long wheel_velocity[])
{
  int i;

  // 各車輪へのモータ回転速度を設定
  for (i = 0; i < 2; ++i) {
    int target_count_per_msec =
      wheel_calculateMotorVelocity(wheel_velocity[i], &robot_->wheel[i]);
    motor_setVelocity(&robot_->motor[i], target_count_per_msec,
                      encoder_difference(&robot_->encoder[i]));
  }
}


static void normalControl_first(void)
{
  path_first(&robot_->follow_position);
}


static void normalControl(void)
{
  long wheel_velocity[2];
  long target_mm_per_sec;
  long target_dir16;

  if (robot_->path.reset) {
    path_reset(&robot_->path);
    robot_->path.reset = 0;
  }

  // 指示されている制御モードでの並進速度・回転速度を取得
  path_calcluateBodyVelocity(&target_mm_per_sec, &target_dir16, &robot_->path,
                             &robot_->straight, &robot_->rotate,
                             &robot_->follow_position, &robot_->position);
  //fprintf(stderr, "(%ld, %ld), ", target_mm_per_sec, (360 * target_dir16) >> 16);

  // 両輪の速度を計算
  body_calculateWheelVelocity(wheel_velocity,
                              target_mm_per_sec, target_dir16, &robot_->body);

  setWheelVelocity(wheel_velocity);
}


static void directControl_wheel_first(void)
{
  // 等加速度の制御に用いる車輪速の初期化する
  // !!! 車輪速の取得関数を作成したら
  // !!! robot_->direct.control_wheel_velocity[i] = XXX;
}


static void directControl_wheel(void)
{
  long wheel_velocity[2];
  int i;

  // 登録されている車輪速で車輪を制御する
  for (i = 0; i < 2; ++i) {
    wheel_velocity[i] = follow_velocityControl(&robot_->direct.wheel[i]);
  }
  setWheelVelocity(wheel_velocity);
}


void robot_update(void)
{
  static ControlMode previous_system_mode = NormalControl;
  ControlMode system_mode = robot_->system.mode;
  short encoder_move[2];
  int i;

  if (robot_->reset) {
    robot_reset();
    robot_->reset = 0;
  }

  // 自己位置の更新
  for (i = 0; i < 2; ++i) {
    if (robot_->encoder[i].reset) {
      encoder_reset(&robot_->encoder[i]);
      robot_->encoder[i].reset = 0;
    }
    encoder_update(&robot_->encoder[i]);
  }
  encoder_move[RightId] = encoder_difference(&robot_->encoder[RightId]);
  encoder_move[LeftId] = encoder_difference(&robot_->encoder[LeftId]);

  if (robot_->position.reset) {
    position_reset(&robot_->position);
    robot_->position.reset = 0;
  }
  position_update(encoder_move[RightId], encoder_move[LeftId],
                  &robot_->position);

  if (robot_->path.reset_offset) {
    path_resetOffset(&robot_->follow_position, &robot_->position);
    robot_->path.reset_offset = 0;
  }
  if (robot_->follow_position.reset) {
    position_reset(&robot_->follow_position);
    robot_->follow_position.reset = 0;
  }
  position_update(encoder_move[RightId], encoder_move[LeftId],
                  &robot_->follow_position);
  //fprintf(stderr, "%d, ", encoder_move[RightId] - encoder_move[LeftId]);
  //fprintf(stderr, "(%d, %d), ", encoder_move[RightId], encoder_move[LeftId]);
  //fprintf(stderr, "%d, ", (360 * robot_->position.dir16) >> 16);
  //fprintf(stderr, "(%ld, %ld, %d), ", robot_->follow_position.mm[0], robot_->follow_position.mm[1], (360 * robot_->position.dir16) >> 16);

  // 制御
  switch (system_mode) {
  case DirectControl_wheel:
    if (system_mode != previous_system_mode) {
      directControl_wheel_first();
    }
    directControl_wheel();
    break;

  case DirectControl_motor:
    // !!! directControl_first();
    // !!! 登録されている PWM, IO でモータを制御する
    break;

  case NormalControl:
    if (system_mode != previous_system_mode) {
      normalControl_first();
    }
    normalControl();
    break;
  }
  previous_system_mode = system_mode;
}
