/*-------------------------------------------------------
 * インターフェースボード(ポイント制御出力)
 *-------------------------------------------------------*/
#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"

/* MCP23017 ポート割付
 *   GPA0,GPA1: ポイント1
 *   GPA2,GPA3: ポイント2
 *   GPA4,GPA5: ポイント3
 *   GPA6,GPA7: ポイント4
 *   GPB0,GPB1: ポイント5
 *   GPB2,GPB3: ポイント6
 *   GPB4,GPB5: ポイント7
 *   GPB6,GPB7: ポイント8
 */

/* インターフェースボードの接続の有無 */
static uint8_t if_module[IF_MAX_MODULES];
/* ポートA,ポートBの書き込み値 */
static uint8_t if_wr_gpa[IF_MAX_MODULES];
static uint8_t if_wr_gpb[IF_MAX_MODULES]; 

/*-------------------------------------------------------
 * 初期化
 *-------------------------------------------------------*/
void if_turnout_init(void)
{
  /* I2C モジュール初期化 */
  I2C_Init(I2C_BAUD_RATE_SSPxADD_1MHZ(_XTAL_FREQ));
  /* 各I2Cアドレスに対してMCP23017の接続を確認し初期化 */
  uint8_t caddr = 0;
  for (caddr = 0; caddr < IF_MAX_MODULES; caddr++) {
    CLRWDT();
    /* 書き込み値 */ 
    if_wr_gpa[caddr] = 0x00;
    if_wr_gpb[caddr] = 0x00; 
    if (!I2C_Exist(I2C_MCP23017_CTLBYTE, caddr)) {
      /* このアドレスに接続無し */
      if_module[caddr] = 0;
    } else {
      /* このアドレスに接続有り */
      if_module[caddr] = 1;
      /* 初期化 */
      i2c_mcp23017_init(caddr);
      if ((0 <= caddr) && (caddr < IF_MAX_MODULES)) {
        /* IODIR: I/O方向 (出力) */
        i2c_mcp23017_a_dir_set(caddr, 0x00);
        i2c_mcp23017_b_dir_set(caddr, 0x00);
      }
      /* ポート初期値書き込み */
      i2c_mcp23017_a_write(caddr, if_wr_gpa[caddr]);
      i2c_mcp23017_b_write(caddr, if_wr_gpb[caddr]);
    }
  }
}

/*-------------------------------------------------------
 * 切替実行
 *-------------------------------------------------------*/
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;
  int8_t g = p - 1;
  while (g >= N_TURNOUTS_PER_MOD) {
    g -= N_TURNOUTS_PER_MOD;
    caddr++;
  }
  /* ポイント制御出力の更新 (ポート割付依存) */
  const uint8_t bit_mask[4] = { 0x03, 0x0C, 0x30, 0xC0 };
  const uint8_t bit_nor[4]  = { 0x01, 0x04, 0x10, 0x40 }; /*定位*/
  const uint8_t bit_rev[4]  = { 0x02, 0x08, 0x20, 0x80 }; /*反位*/
  /* 出力 (ポート割付依存) */
  if ((0 <= g) && (g <= 3)) { /* ポートA */
    if_wr_gpa[caddr] &= ~bit_mask[g];
    if (d == TURNOUT_DIR_NORMAL) { /*定位*/
      if_wr_gpa[caddr] |= bit_nor[g];
    } else {                       /*反位*/
      if_wr_gpa[caddr] |= bit_rev[g];
    }
    if (if_module[caddr] != 0) {
      i2c_mcp23017_a_write(caddr, if_wr_gpa[caddr]);
    }
  } else if ((4 <= g) && (g <= 7)) { /* ポートB */
    if_wr_gpb[caddr] &= ~bit_mask[g - 4];
    if (d == TURNOUT_DIR_NORMAL) { /*定位*/
      if_wr_gpb[caddr] |= bit_nor[g - 4];
    } else {                       /*反位*/
      if_wr_gpb[caddr] |= bit_rev[g - 4];
    }
    if (if_module[caddr] != 0) {
      i2c_mcp23017_b_write(caddr, if_wr_gpb[caddr]);
    }
  }
  /* 返り値: 実際にインターフェースボード接続があるか否か */
  return (if_module[caddr] != 0);
}
