/*------------------------------------------------------- 
 * 減速度の更新値見積と自動学習(自動運転用)
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "ar.h"
#include "ar-est.h"
#include "incr-lin.h"

/* 3つのパラメータ:
 *   dc = .decel            減速度(今回使用した値)
 *   du = .decel_unreached  減速度(停止センサに到達しなかった事例)
 *   dr = .decel_reached    減速度(停止センサに到達した事例)
 *   (値が小さい: すぐに停止, 値が大きい: ゆっくり停止)
 * 3つの値の間で以下の関係を保つ:
 *   decel_unreached <= decel_current <= decel_reached
 */

/* 最低維持速度で走行しながら停止センサに達した場合 */
void ar_estimate_unreached(void)
{
  ar_decp_id decpi = ar_motor.decpi;
  if (decpi == AR_DECP_ID_NULL) {
    return;
  }
  /* 現在値と過去の事例 */
  uint16_t dc = ar_decp[decpi].decel;
  uint16_t du = ar_decp[decpi].decel_unreached;
  uint16_t dr = ar_decp[decpi].decel_reached;
  /* 過去の事例を更新 */
  du = dc;
  /* 過去と今回の事例が互いに矛盾していれば補正 */
  if ((dr != 0) && (du >= dr)) {
    dr = du + (du >> 3);
  }
  /* 見積 */
  uint16_t dc_next = 0; /* 次回の減速度 */
  uint32_t sb2
    = (uint32_t)ar_motor.spd_brake * ar_motor.spd_brake;
  uint32_t ss2
    = (uint32_t)ar_motor.spd_stop  * ar_motor.spd_stop;
  if (sb2 > ss2) { /* 推測式に基づく推定 */
    dc_next
      = ((uint32_t)2*SPEED_INC_DY*ar_motor.run_dist) / (sb2 - ss2);
  } else { /* 値を1.25倍にして再試行 */
    dc_next = dc + (dc >> 2) + 1;
  }
  /* 過去の事例を基に補正 */
  if (dc_next <= dc) {
    if (dr != 0) {
      dc_next = dc + ((dr - dc) >> 1);
    } else {
      dc_next = dc + (dc >> 2);
    }
  }
  if (dc_next == 0) {
    dc_next = 1;
  }
  /* 更新 */
  ar_decp[decpi].decel           = dc_next;
  ar_decp[decpi].decel_unreached = du;
  ar_decp[decpi].decel_reached   = dr;
}

/* 最低維持速度を越えたまま停止センサに達した場合 */
void ar_estimate_reached(void)
{
  ar_decp_id decpi = ar_motor.decpi;
  if (decpi == AR_DECP_ID_NULL) {
    return;
  }
  /* 現在値と過去の事例 */
  uint16_t dc = ar_decp[decpi].decel;
  uint16_t du = ar_decp[decpi].decel_unreached;
  uint16_t dr = ar_decp[decpi].decel_reached;
  /* 過去の事例を更新 */
  dr = dc;
  /* 過去と今回の事例が互いに矛盾していれば補正 */
  if ((du != 0) && (du > dr)) {
    du = dr - (dr >> 3);
  }
  /* 見積 */
  uint16_t dc_next = 0; /* 次回の減速度 */
  uint32_t sb2
    = (uint32_t)ar_motor.spd_brake * ar_motor.spd_brake;
  uint32_t ss2
    = (uint32_t)ar_motor.spd_stop  * ar_motor.spd_stop;
  if (sb2 > ss2) { /* 推測式に基づく推定 */
    dc_next
      = ((uint32_t)2*SPEED_INC_DY*ar_motor.run_dist) / (sb2 - ss2);
  } else { /* 値を0.75倍にして再試行 */
    dc_next = dc - (dc >> 2) + 1;
  }
  /* 過去の事例を基に補正 */
  if (dc <= dc_next) {
    if (du != 0) {
      dc_next = dc - ((dc - du) >> 2);
    } else {
      dc_next = dc - (dc >> 3);
    }
  }
  if (dc_next == 0) {
    dc_next = 1;
  }
  /* 更新 */
  ar_decp[decpi].decel           = dc_next;
  ar_decp[decpi].decel_unreached = du;
  ar_decp[decpi].decel_reached   = dr;
}
