/*!
  \file
  \brief 自己位置の計算

  \author Satofumi KAMIMURA

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

#include "position_calculate.h"
#include "isincos.h"

#include <stdio.h>

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


void position_initialize(position_t *position)
{
  int i;

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

  position_reset(position);
}


void position_reset(position_t *position)
{
  int i;

  for (i = 0; i < 2; ++i) {
    position->xy_count_high[i] = 0;
    position->xy_count_low[i] = 0;
  }

  position->dir16_diff = 0;
  position->dir16_count =
    (position->dir16 * (POSITION_T_DIR16_COUNT_MAX >> 6)) >> 10;
}


//! \todo DIR16_COUNT_MAX を定数にする。パラメータを可変にするため
void position_update(int right_encoder_difference,
                     int left_encoder_difference, position_t *position)
{
  int straight_count;
  int rotate_count;
  int dir16_count = position->dir16_count;
  long dir16;
  long count_add[2];
  int i;

  // 並進、回転のカウンタ変位を計算
  straight_count = right_encoder_difference + left_encoder_difference;
  rotate_count = right_encoder_difference - left_encoder_difference;

  // 16 bit 幅の向き(dir16) を計算
  dir16_count += rotate_count;
  if (dir16_count < 0) {
    dir16_count += POSITION_T_DIR16_COUNT_MAX;
  } else if (dir16_count >= POSITION_T_DIR16_COUNT_MAX) {
    dir16_count -= POSITION_T_DIR16_COUNT_MAX;
  }
  dir16 = (dir16_count << 10) / (POSITION_T_DIR16_COUNT_MAX >> 6);
  if (dir16 < 0) {
    dir16 += 65536;
  }

  // エンコーダでの X, Y 位置を計算する
  count_add[X] = straight_count * icos(dir16) + position->xy_count_low[X];
  count_add[Y] = straight_count * isin(dir16) + position->xy_count_low[Y];

  for (i = 0; i < 2; ++i) {
    int add = count_add[i] >> ISINCOS_VALUE_SHIFT;
    position->xy_count_high[i] += add;
    position->xy_count_low[i] = count_add[i] - (add << ISINCOS_VALUE_SHIFT);
  }

  for (i = 0; i < 2; ++i) {
    long add = position->xy_count_high[i] / (POSITION_T_COUNT_PER_MM << 1);
    position->xy_count_high[i] -= add * (POSITION_T_COUNT_PER_MM << 1);

    // mm での X, Y 位置を計算する
    position->mm[i] += add;
  }

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

  position->dir16_diff = dir16 - position->dir16;
  position->dir16 = dir16;
  position->dir16_count = dir16_count;
}


short position_dir16diff(long target, long current)
{
  long diff = target - current;
  while (diff < -(65536 >> 1)) {
    diff += 65536;
  }
  while (diff >= (65536 >> 1)) {
    diff -= 65536;
  }
  return diff;
}
