/*------------------------------------------------------- 
 * ポイント制御出力 (センサ・ポイントインターフェースボード)
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "io.h"
#include "if.h"
#include "if-toq.h"
#include "i2c.h"
#include "i2c-mcp23017.h"

/*-------------------------------------------------------
 * 初期化 
 *-------------------------------------------------------*/
void if_turnout_init(void)
{
  /* ポート初期値 */
  uint8_t caddr = 0;
  for ( ; caddr < IF_MAX_MODULES_TYPE_A; caddr++) {
    /* (ポート割付依存) */
    if_wr_gpa[caddr] = (unsigned)if_wr_gpa[caddr] & ~0x00; 
    if_wr_gpb[caddr] = (unsigned)if_wr_gpb[caddr] & ~0xFC;
  }
}

/*-------------------------------------------------------
 * 切替の実行 
 *-------------------------------------------------------*/
int8_t if_turnout_action(if_to_id p,
                         enum if_turnout_dir_e d)
{
  if ((p == IF_TO_NULL) || (IF_MAX_TURNOUTS < p)) {
    return 0;
  }
  /* 該当のインターフェースボード番号とポート番号 
   * caddr := (p-1) / N_TURNOUTS_PER_MOD 
   *     g := (p-1) % N_TURNOUTS_PER_MOD */
  uint8_t caddr = 0;
  uint8_t g = (uint8_t) (p - 1);
  while (g >= N_TURNOUTS_PER_MOD) {
    g -= N_TURNOUTS_PER_MOD;
    caddr++;
  }
  /* ポイント制御出力の更新 (ポート割付依存) */
  const uint8_t bit_mask[3] = { 0x0C, 0x30, 0xC0 };
  const uint8_t bit_nor[3]  = { 0x04, 0x10, 0x40 };
  const uint8_t bit_rev[3]  = { 0x08, 0x20, 0x80 };
  if_wr_gpb[caddr] &= (unsigned)~bit_mask[g];
  if (d == TURNOUT_DIR_NORMAL) {
    if_wr_gpb[caddr] |= bit_nor[g];
  } else {
    if_wr_gpb[caddr] |= bit_rev[g];
  }
  /* 出力 */
  if (if_module[caddr] != 0) {
    i2c_mcp23017_b_write(caddr, if_wr_gpb[caddr]);
  }
  /* 返り値: 実際にインターフェースボード接続があるか否か */
  return (if_module[caddr] != 0);
}
