/*
  runCtrl.h のテスト
  Satofumi KAMIMURA
  $Id: RunSimulateTest.cpp 286 2008-10-20 09:40:22Z satofumi $
*/

#include "RunSimulateTest.h"
#include "typePrint.h"
#include "pathUtils.h"

using namespace VXV;

CPPUNIT_TEST_SUITE_REGISTRATION(RunSimulateTest);
CPPUNIT_TEST_SUITE_NAMED_REGISTRATION(RunSimulateTest, "RunSimulateTest");


void RunSimulateTest::setUp(void) {
  char *argv[] = { "", "--simulator", "--nolog" };
  run = new mRunCtrl();
  int ret_value = run->connect(3, argv);
  CPPUNIT_ASSERT_EQUAL_MESSAGE(run->what(), 0, ret_value);
  CPPUNIT_ASSERT_EQUAL(true, run->isConnected());

  vmonitor::setTimeMagnify(128.0);
}


void RunSimulateTest::tearDown(void) {
  // 切断
  delete run;
  run = NULL;
}


void RunSimulateTest::compileTest(void) {
  run->stop();
  run->followLine(Position(0, 0, Direction::deg(0)), GL);
  run->followCircle(Grid(0, 0), 1000, GL);
  run->followCircleOnTangent(Position(0, 0, Direction::deg(0)),
 			     1000, GL);
  run->stopToLine(Position(0, 0, Direction::deg(0)), GL);
  run->rotateToDirection(Direction::deg(0), GL);
  run->rotateAngle(Direction::deg(0));
  run->spin(Direction::deg(0));
  run->lastMoveCommand(GL);
}


void RunSimulateTest::getPositionTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // エンコーダ値のテスト
  unsigned short cnt[2] = { 0, 0 };
  run->getEncoderValue(0, cnt, 2);
  CPPUNIT_ASSERT_EQUAL((unsigned short)0, cnt[0]);
  CPPUNIT_ASSERT_EQUAL((unsigned short)0, cnt[1]);

  int vel[2] = { 0, 0 };
  run->getEncoderVel(0, vel, 2);
  CPPUNIT_ASSERT_EQUAL(0, vel[0]);
  CPPUNIT_ASSERT_EQUAL(0, vel[1]);

  // 初期状態の位置取得
  Position expected = Position(0, 0, deg(0));
  Position actual = run->getLocalPosition();
  CPPUNIT_ASSERT_EQUAL(expected, actual);
  CPPUNIT_ASSERT_EQUAL(expected, run->getRunPosition());

  // 移動後の位置取得
  run->stopToLine(Position(1000, 0, deg(0)));
  while (!run->isStable()) {
    Delay(100);
  }
  expected = Position(1000, 0, deg(0));
  Position run_pos = run->getRunPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, run_pos.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, run_pos.y, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), run_pos.zt.to_deg(), 5);

  // 回転後の位置取得
  run->rotateToDirection(deg(180));
  while (!run->isStable()) {
    Delay(100);
  }
  expected = Position(1000, 0, deg(180));
  actual = run->getRunPosition();
  CPPUNIT_ASSERT_EQUAL(expected.x, actual.x);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 5.0);
  CPPUNIT_ASSERT_EQUAL(expected.zt.to_deg(), actual.zt.to_deg());
}


void RunSimulateTest::adjustRunPositionTest(void) {
  Position expected = Position(0, 0, deg(0));
  CPPUNIT_ASSERT_EQUAL(expected, run->getRunPosition());

  // 位置の更新テスト
  run->adjustRunPosition(Position(100, 0, deg(0)));
  expected = Position(100, 0, deg(0));
  CPPUNIT_ASSERT_EQUAL(expected, run->getRunPosition());

  run->adjustRunPosition(Position(1000, 1000, deg(90)));
  expected = Position(1000, 1000, deg(90));
  CPPUNIT_ASSERT_EQUAL(expected, run->getRunPosition());
}


void RunSimulateTest::judgeCtrlTest(void) {

  // 各種コマンドテスト
  CPPUNIT_ASSERT_EQUAL(1000, run->getLengthToGrid(Grid(1000, 0)));
  CPPUNIT_ASSERT_EQUAL(1000, run->getLengthToGrid(Grid(0, -1000)));
  CPPUNIT_ASSERT_EQUAL((int)(1000 * sqrt(2)),
		       run->getLengthToGrid(Grid(-1000, -1000)));

  CPPUNIT_ASSERT_EQUAL(1000 * RunCtrl::LineNotOver,
		       run->getLengthToLine(Position(1000, 0, deg(0))));
  CPPUNIT_ASSERT_EQUAL(0, run->getLengthToLine(Position(1000, 0, deg(270))));

  // 位置の更新
  run->adjustRunPosition(Position(1000, -100, deg(90)));

  // 各種コマンドテスト
  CPPUNIT_ASSERT_EQUAL(100, run->getLengthToGrid(Grid(1000, 0)));
  CPPUNIT_ASSERT_EQUAL((int)sqrt(1000 * 1000 + 900 * 900),
		       run->getLengthToGrid(Grid(0, -1000)));
  CPPUNIT_ASSERT_EQUAL((int)(sqrt(2000 * 2000 + 900 * 900)),
		       run->getLengthToGrid(Grid(-1000, -1000)));

  CPPUNIT_ASSERT_EQUAL(0, run->getLengthToLine(Position(1000, 0, deg(0))));
  CPPUNIT_ASSERT_EQUAL(100 * RunCtrl::LineOver,
		       run->getLengthToLine(Position(1000, 0, deg(270))));
}


void RunSimulateTest::followLineTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  Position line = Position(1000, 0, deg(90));
  run->followLine(line);
  VXV::Delay(10000);

  Position judge = Position(1000, 0, deg(0));
  for (int i = 0; i < 5; ++i) {
    CPPUNIT_ASSERT_DOUBLES_EQUAL(0, run->getLengthToLine(judge), 25);
    VXV::Delay(200);
  }
}


void RunSimulateTest::stopTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->followLine(Position(0, 0, deg(0)));
  VXV::Delay(1000);
  CPPUNIT_ASSERT(run->getStraightVelDiff(0) > 0);
  run->stop();
  VXV::Delay(2000);
  CPPUNIT_ASSERT_EQUAL(0, run->getStraightVelDiff(0));
}


void RunSimulateTest::followCircleTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->followCircle(Grid(0, 0), 1000 * CCW);
  VXV::Delay(8000);
  Grid center(0, 0);
  for (int i = 0; i < 5; ++i) {
    CPPUNIT_ASSERT_DOUBLES_EQUAL(1000, run->getLengthToGrid(center), 30);
    VXV::Delay(200);
  }
}


void RunSimulateTest::followCircleOnTangentTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->followCircleOnTangent(Position(1000, 0, deg(0)), 1000 * CCW);
  VXV::Delay(8000);
  Grid center(1000, 1000);
  for (int i = 0; i < 5; ++i) {
    CPPUNIT_ASSERT_DOUBLES_EQUAL(1000, run->getLengthToGrid(center), 34.0);
    VXV::Delay(200);
  }
}


void RunSimulateTest::stopToLineTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->stopToLine(Position(0, 1000, deg(90)));
  VXV::Delay(5000);
  CPPUNIT_ASSERT_EQUAL(1000, run->getRunPosition().y);
  CPPUNIT_ASSERT_EQUAL(0, run->getStraightVelDiff(0));
}


void RunSimulateTest::turnToDirectionTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  for (int i = 0; i < 4; ++i) {
    run->rotateToDirection(deg(-45 * (i+1)));
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    CPPUNIT_ASSERT_EQUAL(deg(-45 * (i+1)).to_deg(),
			 run->getLocalPosition().zt.to_deg());

    CPPUNIT_ASSERT_EQUAL(deg(-45 * (i+1)).to_deg(),
			 run->getRunPosition().zt.to_deg());
  }
}


void RunSimulateTest::rotateAngleTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  for (int i = 0; i < 3; ++i) {
    run->rotateAngle(deg(120));
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    CPPUNIT_ASSERT_EQUAL(deg(120 * (i+1)).to_deg(),
			 run->getRunPosition().zt.to_deg());
  }
}


void RunSimulateTest::spinTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->spin(deg(30));
  VXV::Delay(2000);
  for (int i = 0; i < 5; ++i) {
    CPPUNIT_ASSERT_EQUAL(30, run->getRotateVelDiff().to_deg());
  }
}


void RunSimulateTest::lastMoveCommandTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  run->followLine(Position(0, 0, deg(0)));
  run->push_runState();
  VXV::Delay(1000);
  run->stop();
  VXV::Delay(2000);
  CPPUNIT_ASSERT_EQUAL(0, run->getStraightVelDiff());
  run->lastMoveCommand();
  VXV::Delay(1000);
  CPPUNIT_ASSERT_EQUAL(0, run->getStraightVelDiff());

  run->pop_runState();
  run->lastMoveCommand();
  VXV::Delay(2000);
  CPPUNIT_ASSERT(run->getStraightVelDiff() > 0);
}


void RunSimulateTest::fsPositionTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // 90度ずつ回転して、FS 位置の値を取得
  for (int i = 0; i < 4; ++i) {
    run->rotateToDirection(deg(90 * (i+1)));
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    CPPUNIT_ASSERT_EQUAL(deg(90 * (i+1)).to_deg(),
			 run->getRunPosition().zt.to_deg());
  }
}


void RunSimulateTest::fsTurnTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // FS で 120度ずつ回転
  for (int i = 0; i < 3; ++i) {
    run->rotateToDirection(deg(120), &run->FS);
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    CPPUNIT_ASSERT_EQUAL(deg(120 * (i+1)).to_deg(),
			 run->getRunPosition().zt.to_deg());
  }
}


void RunSimulateTest::fsLineTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // FS で目標直線に追従して、矩形軌跡を描く
  int x[] = { 1000, 1000, 0, 0 };
  int y[] = { 0, 1000, 1000, 0 };
  int degree[] = { 90, 180, 270, 0 };
  for (int i = 0; i < 4; ++i) {
    run->stopToLine(Position(1000, 0, deg(0)), &run->FS);
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    Position pos = run->getRunPosition();
    CPPUNIT_ASSERT_DOUBLES_EQUAL(x[i], pos.x, 10.0);
    CPPUNIT_ASSERT_DOUBLES_EQUAL(y[i], pos.y, 10.0);

    run->rotateToDirection(deg(90), &run->FS);
    while (!run->isStable()) {
      VXV::Delay(200);
    }
    CPPUNIT_ASSERT_EQUAL(deg(degree[i]).to_deg(),
			 run->getRunPosition().zt.to_deg());
  }
}


void RunSimulateTest::setURGs(CoordinateCtrl& urg1,
			      CoordinateCtrl& urg2,
			      CoordinateCtrl& urg3) {
  urg1.setOwnCrdToObject(run);

  Position expected = Position(0, 0, deg(0));
  Position actual = urg1.getCrdPosition();
  CPPUNIT_ASSERT_EQUAL(expected, actual);


  urg2.setOwnCrdToObject(run, Position(100, 0, deg(0)));
  expected = Position(100, 0, deg(0));
  actual = urg2.getCrdPosition();
  CPPUNIT_ASSERT_EQUAL(expected, actual);

  urg3.setOwnCrdToObject(run, Position(0, -100, deg(-90)));
  expected = Position(0, -100, deg(-90));
  actual = urg3.getCrdPosition();
  CPPUNIT_ASSERT_EQUAL(expected, actual);
}


void RunSimulateTest::robotMove(void) {
  run->stopToLine(Position(1000, 0, deg(0)));
  while (!run->isStable()) {
    VXV::Delay(200);
  }
  run->rotateToDirection(deg(90));
  while (!run->isStable()) {
    VXV::Delay(200);
  }
  run->stopToLine(Position(1000, 1000, deg(90)));
  while (!run->isStable()) {
    VXV::Delay(200);
  }
  Position expected = Position(1000, 1000, deg(90));
  Position actual = run->getRunPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5);
}


void RunSimulateTest::judgeURGsPosition(CoordinateCtrl& urg1,
					CoordinateCtrl& urg2,
					CoordinateCtrl& urg3) {
  Position expected = Position(0, 0, deg(0));
  Position actual = run->getRunPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 5.0);
  // !!! CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 5.0);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5.0);

  expected = Position(0, 0, deg(0));
  actual = urg1.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 5.0);
  // !!! CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 5.0);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5.0);

  expected = Position(100, 0, deg(0));
  actual = urg2.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 5.0);
  // !!! CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 5.0);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5.0);

  expected = Position(0, -100, deg(-90));
  actual = urg3.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 5.0);
  // !!! CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 5.0);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5.0);
}


void RunSimulateTest::multiCoordinateTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  CoordinateCtrl urg1;
  CoordinateCtrl urg2;
  CoordinateCtrl urg3;

  // 座標系をツリー構造にし、移動後の座標系位置を確認
  setURGs(urg1, urg2, urg3);

  // 1000, 1000, deg(90) まで移動する  
  robotMove();

  // 移動後の位置判定
  Position expected = Position(1000, 1000, deg(90));
  Position actual = urg1.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5);

  expected = Position(1000, 1100, deg(90));
  actual = urg2.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5);

  expected = Position(1100, 1000, deg(0));
  actual = urg3.getCrdPosition();
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.x, actual.x, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.y, actual.y, 20);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(expected.zt.to_deg(), actual.zt.to_deg(), 5);
}


void RunSimulateTest::adjustCrdPositionTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // 座標系位置の修正による子座標系の位置を確認
  CoordinateCtrl urg1;
  CoordinateCtrl urg2;
  CoordinateCtrl urg3;

  // 座標系をツリー構造にし、移動後の座標系位置を確認
  setURGs(urg1, urg2, urg3);

  // 1000, 1000, deg(90) まで移動する  
  robotMove();
  run->stop();

  // 位置修正と判定
  Position3D adjust_pos;

  // 座標系原点の移動は、回転、平行移動、の順に行われる
  run->adjustCrdPosition(Position(-1000, 1000, deg(-90)));

  judgeURGsPosition(urg1, urg2, urg3);
}


void RunSimulateTest::adjustRunPositionTest2(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // ロボット位置の修正による子座標系の位置を確認
  CoordinateCtrl urg1;
  CoordinateCtrl urg2;
  CoordinateCtrl urg3;

  // 座標系をツリー構造にし、移動後の座標系位置を確認
  setURGs(urg1, urg2, urg3);

  // 1000, 1000, deg(90) まで移動する  
  robotMove();
  run->stop();

  // 位置修正と判定
  run->adjustRunPosition(Position(0, 0, deg(0)));
  judgeURGsPosition(urg1, urg2, urg3);
}


void RunSimulateTest::CoordinateUpdateDetectTest(void) {
  //vmonitor::show();
  //vmonitor::setTimeMagnify(1.0);

  // 影響なし、に設定
  run->coordinateUpdateDetect(false);

  // 移動開始
  run->followLine(Position(0, 0, deg(0)));

  // 座標系原点の変更
  run->adjustCrdPosition(Position(0, 500, deg(0)));
  VXV::Delay(1000);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(0, run->getRunPosition().zt.to_deg(), 5.0);

  // ロボット位置の修正
  run->adjustRunPosition(Position(0, 500, deg(0)));
  VXV::Delay(1000);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(0, run->getRunPosition().zt.to_deg(), 5.0);

  // 影響あり、に設定
  run->updateParentCrdOffset(Position(0, 0, deg(0)));
  run->coordinateUpdateDetect(true);

  // 座標系原点の変更
  run->adjustCrdPosition(Position(0, 500, deg(0)));
  VXV::waitStable(*run, 100);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(0, run->getRunPosition().y, 30.0);

  // ロボット位置の修正
  run->adjustRunPosition(Position(0, 500, deg(0)));
  VXV::waitStable(*run, 100);
  CPPUNIT_ASSERT_DOUBLES_EQUAL(0, run->getRunPosition().y, 34.0);
}
