/*!
  \file
  \brief 座標系の定義と、座標系の変換

  \author Satofumi KAMIMURA

  $Id: coordinateCtrl_target.c 286 2008-10-20 09:40:22Z satofumi $
*/

#include "coordinateCtrl_target.h"
#include "isincos.h"


void initCoordinateInfo(coordinateInfo_t *crd) {
  int i;

  for (i = 0; i < 2; ++i) {
    crd->km[i] = 0;
    crd->m[i] = 0;
    crd->mm[i] = 0;
    crd->mm256[i] = 0;
  }
  crd->div16 = 0;
}


int getSignedDiv16(short div16) {
  int diff = div16;
  if (diff > (0x10000 >> 1)) {
    diff -= 0x10000;
  }
  return diff;
}


void adjustCoordinateRange(coordinateInfo_t *crd) {
  int i;

  for (i = 0; i < 2; ++i) {
    while (crd->mm[i] >= 1000) {
      crd->mm[i] -= 1000;
      ++(crd->m[i]);
    }
    while (crd->mm[i] < 0) {
      crd->mm[i] += 1000;
      --(crd->m[i]);
    }

    while (crd->m[i] >= 1000) {
      crd->m[i] -= 1000;
      ++(crd->km[i]);
    }
    while (crd->m[i] < 0) {
      crd->m[i] += 1000;
      --(crd->km[i]);
    }
  }
  crd->div16 &= 0xffff;
}


void get_mmCoordinate(coordinateInfo_t *crd, int *x_mm, int *y_mm) {
  enum { X = COORD_X, Y = COORD_Y };

  if (x_mm) {
    *x_mm = (1000000 * crd->km[X]) + (1000 * crd->m[X]) + crd->mm[X];
  }
  if (y_mm) {
    *y_mm = (1000000 * crd->km[Y]) + (1000 * crd->m[Y]) + crd->mm[Y];
  }
}


static int multi(const int val32, const int val15) {
  int high16, low16;

  /* isin の値の最大値が 15bit であることに依存した実装 */
  if (val32 < 0) {
    high16 = (~(((~val32) +1) >> 16)) +1;
    low16 = (~(((~val32) +1) & 0x0000ffff)) +1;

  } else {
    high16 = val32 >> 16;
    low16 = val32 & 0x0000ffff;
  }
  return ((high16 * val15) << (16-15)) + ((low16 * val15) >> (16-1));
}


void convertCoordinateInfo(coordinateInfo_t *crd,
				  coordinateInfo_t *offset) {
  enum { X = COORD_X, Y = COORD_Y };
  int icos_value, isin_value;
  int x, y;
  int i;

  // 平行移動
  for (i = 0; i < 2; ++i) {
    crd->mm[i] += offset->mm[i];
    crd->m[i] += offset->m[i];
    crd->km[i] += offset->km[i];
  }

  icos_value = icos(offset->div16);
  isin_value = isin(offset->div16);

  x = (1000000 * crd->km[X]) + (1000 * crd->m[X]) + crd->mm[X];
  y = (1000000 * crd->km[Y]) + (1000 * crd->m[Y]) + crd->mm[Y];
  crd->mm[X] = multi(x, icos_value) + multi(y,-isin_value);
  crd->mm[Y] = multi(x, isin_value) + multi(y, icos_value);
  for (i = 0; i < 2; ++i) {
    crd->m[i] = 0;
    crd->km[i] = 0;
  }

  crd->div16 += offset->div16;
  adjustCoordinateRange(crd);
}
