/*!
  \file
  \brief 走行制御プログラムのタスク制御

  \author Satofumi KAMIMURA

  $Id: taskCtrl.c 360 2008-11-19 23:51:52Z satofumi $
*/

#include "taskCtrl.h"
#include "packetHandleTarget.h"
#include "sh7045lib.h"
#include "emergencyCtrl.h"


static void watchDogTimer(runCtrl_t *run) {
  if (run->watch_dog_msec > 0) {
    if (--run->watch_dog_msec == 0) {
      run->mode.ctrl = MODE_SERVO_CTRL;
      run->mode.straight_ctrl = MODE_STOP_FIRST;
      run->mode.rotate_ctrl = MODE_NO_CTRL;
    }
  }
}


static void resetWatchDogTimer(runCtrl_t *run) {
  run->watch_dog_msec = run->watch_dog_msec_max;
}


void update_tRunCtrlState(runCtrl_t *run) {
  int straight_mm_vel, rotate_div16_vel;
  int cnt[2];
  int i;

  if (++run->msec >= 1000) {
    run->msec = 0;
    ++(run->sec);
  }
  watchDogTimer(run);

  for (i = 0; i < 2; ++i) {
    updateEncDiff(&run->enc[i]);
  }
  for (i = 0; i < 2; ++i) {
    cnt[i] =
      run->bodyInfo.mtr_direction[i] * run->bodyInfo.whl[i]->enc->diff;
  }
  updateBodyPosition(&run->bodyPos, cnt[WHL_RIGHT], cnt[WHL_LEFT]);
  updateCoordinatePosition(&run->gl_crd, &run->bodyPos);
  updateVelocityInfo(&run->velInfo, &run->bodyPos);
  convertCoordinateInfo(&run->gl_crd, &run->gl_crd_offset);

  // !!! 緊急停止用
  if (nowEmergency()) {
    // !!! PF7 に 5V が入っていない場合には、サーボモードにする
    for (i = 0; i < 2; ++i) {
      set_pwm(i, 0);
      set_mode(i, MTR_MODE_CW_BREAK);
    }

  } else {

    if (run->mode.ctrl != MODE_SERVO_CTRL) {
      run->mode.straight_ctrl = MODE_NO_CTRL;
      run->mode.rotate_ctrl = MODE_NO_CTRL;
    }
    switch (run->mode.ctrl) {
    case MODE_SERVO_CTRL:
      run->run_crd = run->gl_crd;
      convertCoordinateInfo(&run->run_crd, &run->run_crd_offset);
      straight_mm_vel = calcStraightRefVel(&run->straight, &run->mode,
                                           &run->velInfo, &run->run_crd);
      rotate_div16_vel = calcRotateRefVel(&run->rotate, &run->mode,
                                          &run->bodyPos, &run->velInfo,
                                          &run->run_crd);
      setBodyMove(&run->bodyInfo, straight_mm_vel, rotate_div16_vel);
      break;

    case MODE_DIRECT_CTRL:
      for (i = 0; i < 2; ++i) {
        set_pwm(i, run->direct[i].pwm);
        if (run->direct[i].mode != DIRECT_MODE_NONE) {
          set_mode(i, run->direct[i].mode);
        }
      }
      break;

    case MODE_DIRECT_WHEEL_CTRL:
      for (i = 0; i < 2; ++i) {
        setWheelMoveVelocity(run->directWhl[i].ref_mm_sec_vel, &run->whl[i]);
      }
      break;

    case MODE_SERVO_FREE:
      for (i = 0; i < 2; ++i) {
        setWheelFree(run->bodyInfo.whl[i]);
      }
      break;
    }
  }
  getRawCount(&run->used_msec);
}


void recv_tRunCtrlPacket(runCtrl_t *run) {
  if (packetRecvHandler(run) > 0) {
    resetWatchDogTimer(run);
  }
}


void init_tRunCtrlState(runCtrl_t *run, long version) {
  int i;

  run->version = version;
  run->msec = 0;
  run->sec = 0;
  run->watch_dog_msec = 0;

  initEmergencyIo();

  initEnc();
  initMotor();
  for (i = 0; i < 2; ++i) {
    initEncInfo(i, &run->enc[i]);
    initMotorInfo(i, &run->mtr[i]);
    initDirectInfo(&run->direct[i]);
    initWheelInfo(&run->whl[i], &run->mtr[i], &run->enc[i], i);
    initDirectWheelCtrlInfo(&run->directWhl[i]);
  }
  initBodyInfo(&run->bodyInfo, &run->whl[WHL_RIGHT], &run->whl[WHL_LEFT]);
  initBodyPositionInfo(&run->bodyPos, &run->bodyInfo);
  initVelocityCtrlInfo(&run->straight);
  initVelocityCtrlInfo(&run->rotate);
  initVelocityInfo(&run->velInfo);

  initCoordinateInfo(&run->gl_crd);
  initCoordinateInfo(&run->gl_crd_offset);
  initCoordinateInfo(&run->run_crd);
  initCoordinateInfo(&run->run_crd_offset);
  initModeInfo(&run->mode);
}


void reset_tRunCtrlPosition(runCtrl_t *run) {
  initBodyPositionInfo(&run->bodyPos, &run->bodyInfo);
}
