#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "io.h"
#include "turnout.h"

/* 有限状態機械(ポイント制御入力用) */
enum e_instate {
  INSTATE_UNDEF = 0,
  INSTATE_F_F,     /* XA=F, XB=F */
  INSTATE_T_F,     /* XA=T, XB=F */
  INSTATE_F_T,     /* XA=F, XB=T */
  INSTATE_T_T,     /* XA=T, XB=T */
};

static enum e_instate curr = INSTATE_UNDEF;
static enum e_instate instate_read(void);

enum e_movereq instate_init(void)
{
  curr = INSTATE_UNDEF;
  return instate_next();
}

enum e_movereq instate_next(void)
{
  enum e_movereq movereq = MOVEREQ_NONE;
  enum e_instate next = instate_read();
  switch (curr) {
  default:
  case INSTATE_UNDEF:
    switch (next) {
    case INSTATE_F_F: movereq = MOVEREQ_A; break;
    case INSTATE_T_F: movereq = MOVEREQ_A; break;
    case INSTATE_F_T: movereq = MOVEREQ_B; break;
    case INSTATE_T_T: movereq = MOVEREQ_B; break;
    }
    break;
  case INSTATE_F_F:
    switch (next) {
    case INSTATE_F_F: break;
    case INSTATE_T_F: movereq = MOVEREQ_A; break;
    case INSTATE_F_T: movereq = MOVEREQ_B; break;
    case INSTATE_T_T: break; 
    }
    break;
  case INSTATE_T_F:
    switch (next) {
    case INSTATE_F_F: break;
    case INSTATE_T_F: break;
    case INSTATE_F_T: movereq = MOVEREQ_B; break;
    case INSTATE_T_T: movereq = MOVEREQ_B; break;
    }
    break;
  case INSTATE_F_T:
    switch (next) {
    case INSTATE_F_F: break;
    case INSTATE_T_F: movereq = MOVEREQ_A; break;
    case INSTATE_F_T: break; 
    case INSTATE_T_T: movereq = MOVEREQ_A; break;
    }
    break;
  case INSTATE_T_T:
    switch (next) {
    case INSTATE_F_F: break;
    case INSTATE_T_F: movereq = MOVEREQ_A; break;
    case INSTATE_F_T: movereq = MOVEREQ_B; break;
    case INSTATE_T_T: break; 
    }
    break;
  }
  curr = next;
  return movereq;
}

static enum e_instate instate_read(void)
{
  int8_t in_a_curr = DIR_IN_A();
  int8_t in_b_curr = DIR_IN_B();
  enum e_instate instate = INSTATE_UNDEF;
  if        (!in_a_curr && !in_b_curr) {
    instate = INSTATE_F_F;
  } else if ( in_a_curr && !in_b_curr) {
    instate = INSTATE_T_F;
  } else if (!in_a_curr &&  in_b_curr) {
    instate = INSTATE_F_T;
  } else if ( in_a_curr &&  in_b_curr) {
    instate = INSTATE_T_T;
  }
  return instate;
}
