/*!
  \file
  \brief 経路走行の管理

  \author Satofumi KAMIMURA

  $Id: path_manage.c 1381 2009-10-08 19:45:31Z satofumi $
*/

#include "path_manage.h"
#include "position_calculate.h"
#include "follow_control.h"
#include "isincos.h"

#include <stdio.h>

enum {
  X = 0,
  Y = 1,
};


void path_initialize(path_t *path)
{
  path->reset = 0;
  path->reset_offset = 0;
  path->mode = ServoFree;

  // !!! マクロにすること
  path->follow_radius = 300;

  path->rotate_direction = 0;
}


void path_reset(path_t *path)
{
  (void)path;

  // !!!
}


void path_resetOffset(position_t *follow_position, const position_t *position)
{
  long x;
  long y;
  unsigned short dir16 = follow_position->dir16;
  int i;

  for (i = 0; i < 2; ++i) {
    follow_position->mm[i] += position->mm[i];
  }
  follow_position->dir16 += position->dir16;
  position_reset(follow_position);

  // x, y 位置を position 中心に position->dir16 だけ回転させる
  x = follow_position->mm[X];
  y = follow_position->mm[Y];

  follow_position->mm[X] =
    ((x * icos(dir16)) - (y * isin(dir16))) >> ISINCOS_VALUE_SHIFT;
  follow_position->mm[Y] =
    ((x * isin(dir16)) + (y * icos(dir16))) >> ISINCOS_VALUE_SHIFT;

  //fprintf(stderr, "(%ld, %ld, %d), ", follow_position->mm[0], follow_position->mm[1], (360 * position->dir16) >> 16);
}


void path_first(position_t *follow_position)
{
  (void)follow_position;

  //position_reset(follow_position);
  // !!! 現在の車輪速を読み出す
}


void path_calcluateBodyVelocity(long *mm_per_sec,
                                long *dir16, path_t *path,
                                follow_t *straight, follow_t *rotate,
                                const position_t *follow_position,
                                const position_t *position)
{
  long straight_velocity = 0;
  long rotate_velocity = 0;
  long diff;

  switch (path->mode) {
  case ServoFree:
    // !!!
    break;

  case Stop:
    straight_velocity = follow_velocityControl(straight);
    rotate_velocity = follow_velocityControl(rotate);
    break;

  case Line:
    straight_velocity = follow_velocityControl(straight);
    rotate_velocity = follow_line(rotate, follow_position, path);
    break;

  case Line_stop:
    straight_velocity =
      follow_positionControl(straight, -follow_position->mm[X]);
    rotate_velocity = follow_line(rotate, follow_position, path);
    break;

  case Circle:
    straight_velocity = follow_velocityControl(straight);
    rotate_velocity = follow_circle(rotate, follow_position, path);
    break;

  case Circle_stop:
    // !!!
    break;

  case Rotation:
    straight_velocity = 0;
    straight->is_stable = Stable;
    rotate_velocity = follow_positionControl(rotate, path->rotate_direction);
    path->rotate_direction -= position->dir16_diff;
    break;

  case Rotation_stop:
    straight_velocity = 0;
    straight->is_stable = Stable;
    diff = position_dir16diff(path->rotate_direction, position->dir16);
    rotate_velocity = follow_positionControl(rotate, diff);
    break;
  }

  //fprintf(stderr, "[%d, %d], ", straight->is_stable, rotate->is_stable);

  *mm_per_sec = straight_velocity;
  *dir16 = rotate_velocity;
  //fprintf(stderr, "(%ld, %ld), ", straight_velocity, rotate_velocity);
}
