/*!
  \file
  \brief 経路追従

  \author Satofumi KAMIMURA

  $Id: follow_control.c 1405 2009-10-12 11:59:45Z satofumi $
*/

#include "follow_control.h"
#include "position_calculate.h"
#include "isqrt.h"
#include "iatan2.h"

#include <stdio.h>
#include <stdlib.h>

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


void follow_initialize(follow_t *follow, long acceleration)
{
  follow->reset = 0;
  follow->is_stable = Unstable;
  follow->target_velocity = 0;
  follow->control_velocity = 0;
  follow->control_acceleration = acceleration;
}


void follow_reset(follow_t *follow)
{
  (void)follow;

  // !!!
}


long follow_velocityControl(follow_t *follow)
{
  long target_velocity = follow->target_velocity;
  long control_velocity = follow->control_velocity;
  long acceleration = follow->control_acceleration;

  follow->is_stable = Unstable;

  if ((control_velocity + (acceleration >> FOLLOW_SHIFT)) < target_velocity) {
    control_velocity += acceleration >> FOLLOW_SHIFT;

  } else if ((control_velocity
              - (acceleration >> FOLLOW_SHIFT)) > target_velocity) {
    control_velocity -= acceleration >> FOLLOW_SHIFT;

  } else {
    control_velocity = target_velocity;
    follow->is_stable = Stable;
  }

  follow->control_velocity = control_velocity;
  return control_velocity >> FOLLOW_SHIFT;
}


long follow_positionControl(follow_t *follow, long difference)
{
  const long target_velocity = follow->target_velocity;
  long velocity;
  int sign;
  char is_stable;
  long control_velocity;

  // v^2 = 2al から計算した速度を用いて速度制御を行う
  sign = (difference >= 0) ? +1 : -1;
  if (sign < 0) {
    difference = -difference;
  }

  // １つの isqrt() で計算すると変数の bit 幅が 32 bit の場合におかしくなる
  velocity =
    isqrt(follow->control_acceleration >> (FOLLOW_SHIFT - 1)) *
    isqrt(difference);
  velocity <<= FOLLOW_SHIFT;

  if (velocity > target_velocity) {
    velocity = target_velocity;
  } else if (velocity < -target_velocity) {
    velocity = -target_velocity;
  }

  // !!! 安定したとみなす数値をマクロ化する
  is_stable = (velocity < 10) ? Stable : Unstable;

  if (sign < 0) {
    velocity = -velocity;
  }

  follow->target_velocity = velocity;
  control_velocity = follow_velocityControl(follow);
  follow->target_velocity = target_velocity;

  follow->is_stable = is_stable;
  return control_velocity;
}


static long follow_direction(long abs_mm, long follow_radius, int sign)
{
  enum { LengthOffset = 16 };

  long dir16;
  long y;

  // 発振しないための補正
  if (abs_mm > LengthOffset) {
    abs_mm -= LengthOffset;
  } else {
    abs_mm = 0;
  }

  // 円弧に接する直線の傾き方向に、向きを制御する
  // theta = atan2(y, sqrt(y * (2 * r - y)));
  y = isqrt(abs_mm * ((follow_radius << 1) - abs_mm));
  dir16 = iatan2(y, follow_radius - abs_mm);
  if (sign > 0) {
    dir16 = -dir16;
  }

  return dir16;
}


long follow_line(follow_t *rotate, const position_t *position,
                 const path_t *path)
{
  // position->mm[Y] との距離に応じて向きの制御を行う
  // 指定直線に対して、円弧の軌跡を描いて近付くようにする

  long follow_radius = path->follow_radius;
  long mm = position->mm[Y];
  int sign = (mm >= 0) ? +1 : -1;
  long abs_mm = (sign > 0) ? mm : -mm;
  long dir16;
  long diff;
  long control_velocity;

  // 経路追従の曲率よりも離れていたら、直線の方向に向かうように制御する
  if (abs_mm > follow_radius) {
    dir16 = -sign * (65536 >> 2);

  } else {
    dir16 = follow_direction(abs_mm, follow_radius, sign);
  }

  diff = position_dir16diff(dir16, position->dir16);
  control_velocity = follow_positionControl(rotate, diff);

  // !!! LengthOffset を使う
  rotate->is_stable = ((diff > -16) && (diff < 16)) ? Stable : Unstable;
  return control_velocity;
}


long follow_circle(follow_t *rotate, const position_t *position,
                   const path_t *path)
{
  long follow_radius = path->follow_radius;
  long x = position->mm[X];
  long y = position->mm[Y];

  // 追従円弧との距離を計算
  long r = isqrt((x * x) + (y * y));
  int circle_sign = (path->circle_radius > 0) ? +1 : -1;
  long mm = path->circle_radius - (circle_sign * r);
  long abs_mm = (mm > 0) ? mm : -mm;
  int mm_sign = (mm > 0) ? +1 : -1;
  long dir16;
  long diff;
  long control_velocity;

  //fprintf(stderr, "%ld, ", mm);

  // 経路追従の曲率よりも離れていたら、円弧の方向に向かうように制御する
  if (abs_mm > follow_radius) {
    dir16 = iatan2(y, x) + ((mm_sign * circle_sign < 0) ? (65536 >> 1) : 0);
    //fprintf(stderr, "<%ld>, ", (360 * dir16) >> 16);
  } else {
    //fprintf(stderr, "<%d>, ", mm_sign);
    dir16 = follow_direction(abs_mm, follow_radius, mm_sign);
    //fprintf(stderr, "<%ld>, ", (360 * dir16) >> 16);

    // 円弧方向の傾き方向を基準に制御する
    dir16 += iatan2(y, x) + (circle_sign * (65536 >> 2));
  }

  // 回転角速度に相当する補正を行う
  // !!! 並進速度が関係してくるはず
  // !!!

  diff = position_dir16diff(dir16, position->dir16);
  //fprintf(stderr, "%ld, %d, %ld\n", dir16, position->dir16, diff);
  control_velocity = follow_positionControl(rotate, diff);

  rotate->is_stable = ((diff > -16) && (diff < 16)) ? Stable : Unstable;
  return control_velocity;
}
