/*
 *  © 2020,2021 Chris Harlow. All rights reserved.
 *  
 *  This file is part of CommandStation-EX
 *
 *  This is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  It is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
 */

/* EXRAILPlus planned FEATURE additions
   F1. DCC accessory packet opcodes (short and long form)
   F2. ONAccessory catchers 
   F3. Turnout descriptions for Withrottle
   F4. Oled announcements (depends on HAL)
   F5. Withrottle roster info
   F6. Multi-occupancy semaphore
   F7. Self starting sequences
   F8. Park/unpark
  */
/* EXRAILPlus planned TRANSPARENT additions
   [DONE]T1. RAM based fast lookup for sequences ON* event catchers and signals.
   T2. Extend to >64k
  */


#include <Arduino.h>
#include "RMFT2.h"
#include "DCC.h"
#include "DCCWaveform.h"
#include "DIAG.h"
#include "WiThrottle.h"
#include "DCCEXParser.h"
#include "Turnouts.h"


// Command parsing keywords
const int16_t HASH_KEYWORD_EXRAIL=15435;    
const int16_t HASH_KEYWORD_ON = 2657;
const int16_t HASH_KEYWORD_START=23232;
const int16_t HASH_KEYWORD_RESERVE=11392;
const int16_t HASH_KEYWORD_FREE=-23052;
const int16_t HASH_KEYWORD_LATCH=1618;  
const int16_t HASH_KEYWORD_UNLATCH=1353;
const int16_t HASH_KEYWORD_PAUSE=-4142;
const int16_t HASH_KEYWORD_RESUME=27609;
const int16_t HASH_KEYWORD_KILL=5218;
const int16_t HASH_KEYWORD_ROUTES=-3702;

// One instance of RMFT clas is used for each "thread" in the automation.
// Each thread manages a loco on a journey through the layout, and/or may manage a scenery automation.
// The thrrads exist in a ring, each time through loop() the next thread in the ring is serviced.

// Statics 
const int16_t LOCO_ID_WAITING=-99; // waiting for loco id from prog track
int16_t RMFT2::progtrackLocoId;  // used for callback when detecting a loco on prograck
bool RMFT2::diag=false;      // <D EXRAIL ON>  
RMFT2 * RMFT2::loopTask=NULL; // loopTask contains the address of ONE of the tasks in a ring.
RMFT2 * RMFT2::pausingTask=NULL; // Task causing a PAUSE.
 // when pausingTask is set, that is the ONLY task that gets any service,
 // and all others will have their locos stopped, then resumed after the pausing task resumes.
byte RMFT2::flags[MAX_FLAGS];

LookList *  RMFT2::sequenceLookup=NULL;
LookList *  RMFT2::signalLookup=NULL;
LookList *  RMFT2::onThrowLookup=NULL;
LookList *  RMFT2::onCloseLookup=NULL;

#define GET_OPCODE GETFLASH(RMFT2::RouteCode+progCounter)
#define GET_OPERAND(n) GETFLASHW(RMFT2::RouteCode+progCounter+1+(n*3))
#define SKIPOP progCounter+=3


   LookList::LookList(int16_t size) {
     m_size=size;
     m_loaded=0;
     if (size) {
       m_lookupArray=new int16_t[size];
       m_resultArray=new int16_t[size];
     }    
   } 
   
   void LookList::add(int16_t lookup, int16_t result) {
     if (m_loaded==m_size) return; // and forget
     m_lookupArray[m_loaded]=lookup;
     m_resultArray[m_loaded]=result;
     m_loaded++;     
   }
   
   int16_t LookList::find(int16_t value) {
     for (int16_t i=0;i<m_size;i++) {
       if (m_lookupArray[i]==value) return m_resultArray[i];
     }
     return -1; 
   }

/* static */ void RMFT2::begin() { 
  DCCEXParser::setRMFTFilter(RMFT2::ComandFilter);
  for (int f=0;f<MAX_FLAGS;f++) flags[f]=0;
  int progCounter;
  
  // counters to create lookup arrays 
  int sequenceCount=0; // to allow for seq 0 at start 
  int onThrowCount=0;
  int onCloseCount=0;
  int signalCount=0;

  // first pass count sizes for fast lookup arrays
  for (progCounter=0;; SKIPOP) {
     byte opcode=GET_OPCODE;
     if (opcode==OPCODE_ENDEXRAIL) break;
     switch (opcode) {
     case OPCODE_SIGNAL: 
        signalCount++; 
        break;
      
     case OPCODE_ROUTE: 
     case OPCODE_AUTOMATION:
     case OPCODE_SEQUENCE:
        sequenceCount++;
        break;
     
     case OPCODE_ONTHROW: 
        onThrowCount++;
        break;
     
     case OPCODE_ONCLOSE: 
        onCloseCount++;
        break;

     default: // Ignore  
      break;  
     }     
  } 
  
  // create lookups
  sequenceLookup=new LookList(sequenceCount);
  signalLookup=new LookList(signalCount);
  onThrowLookup=new LookList(onThrowCount);
  onCloseLookup=new LookList(onCloseCount);
         
  // Second pass startup, define any turnouts or servos, set signals red
  // add signals, sequences onRoutines to the lookups 
  for (progCounter=0;; SKIPOP){
     byte opcode=GET_OPCODE;
     if (opcode==OPCODE_ENDEXRAIL) break;
     VPIN operand=GET_OPERAND(0);
     
     switch (opcode) {
     case OPCODE_AT:
     case OPCODE_AFTER:
     case OPCODE_IF:
     case OPCODE_IFNOT: {
       int16_t pin = (int16_t)operand;
       if (pin<0) pin = -pin;
       IODevice::configureInput((VPIN)pin,true);
       break;
     }
     

     case OPCODE_SIGNAL: {
      VPIN red=operand;
      VPIN amber=GET_OPERAND(1);
      VPIN green=GET_OPERAND(2);
      IODevice::write(red,true);
      if (amber) IODevice::write(amber,false);
      IODevice::write(green,false);
      signalLookup->add(red,progCounter); 
      break;
     }
     
     case OPCODE_TURNOUT: {
      VPIN id=operand;
      int addr=GET_OPERAND(1);
      byte subAddr=GET_OPERAND(2);
      DCCTurnout::create(id,addr,subAddr);
      break;
     }

     case OPCODE_SERVOTURNOUT: {
      VPIN id=operand;
      VPIN pin=GET_OPERAND(1);
      int activeAngle=GET_OPERAND(2);
      int inactiveAngle=GET_OPERAND(3);
      int profile=GET_OPERAND(4);
      ServoTurnout::create(id,pin,activeAngle,inactiveAngle,profile);
      break;
     }

     case OPCODE_PINTURNOUT: {
      VPIN id=operand;
      VPIN pin=GET_OPERAND(1);
      VpinTurnout::create(id,pin);
      break;
     }

     case OPCODE_ROUTE: 
     case OPCODE_AUTOMATION:
     case OPCODE_SEQUENCE:
          sequenceLookup->add(operand,progCounter);
          break;
     
     case OPCODE_ONTHROW: 
        onThrowLookup->add(operand,progCounter);
        break;
     
     case OPCODE_ONCLOSE: 
        onCloseLookup->add(operand,progCounter);
        break;

     case OPCODE_AUTOSTART:
         // automatically create a task from here at startup. 
         new RMFT2(progCounter);
         break;

     default: // Ignore   
      break; 
     }     
  } 
  SKIPOP; // include ENDROUTES opcode
  
  DIAG(F("EXRAIL %db, fl=%d seq=%d, sig=%d, onT=%d, onC=%d"),
        progCounter,MAX_FLAGS,
        sequenceCount, signalCount, onThrowCount, onCloseCount);
        
  new RMFT2(0); // add the startup route
}

// This filter intercepts <> commands to do the following:
// - Implement RMFT specific commands/diagnostics 
// - Reject/modify JMRI commands that would interfere with RMFT processing 
void RMFT2::ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]) {
    (void)stream; // avoid compiler warning if we don't access this parameter 
    bool reject=false;
    switch(opcode) {
        
     case 'D':
        if (p[0]==HASH_KEYWORD_EXRAIL) { // <D EXRAIL ON/OFF>
           diag = paramCount==2 && (p[1]==HASH_KEYWORD_ON || p[1]==1);
           opcode=0;
        }
        break;
          
     case '/':  // New EXRAIL command
          reject=!parseSlash(stream,paramCount,p);
          opcode=0;
          break;
          
     default:  // other commands pass through 
     break;       
   }
 if (reject) {
   opcode=0;
   StringFormatter::send(stream,F("<X>"));
   }
}
     
bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {

          if (paramCount==0) { // STATUS
                 StringFormatter::send(stream, F("<* EXRAIL STATUS"));
                 RMFT2 * task=loopTask;
                 while(task) {
                   StringFormatter::send(stream,F("\nID=%d,PC=%d,LOCO=%d%c,SPEED=%d%c"),
                         (int)(task->taskId),task->progCounter,task->loco,
                         task->invert?'I':' ',
                         task->speedo, 
                         task->forward?'F':'R'
                         );
                   task=task->next;      
                   if (task==loopTask) break;      
                 }
                 // Now stream the flags 
                 for (int id=0;id<MAX_FLAGS; id++) {
                   byte flag=flags[id];
                   if (flag & ~TASK_FLAG) { // not interested in TASK_FLAG only. Already shown above 
                     StringFormatter::send(stream,F("\nflags[%d} "),id);
                     if (flag & SECTION_FLAG) StringFormatter::send(stream,F(" RESERVED"));
                     if (flag & LATCH_FLAG) StringFormatter::send(stream,F(" LATCHED"));
                     }                 
                 }
                 StringFormatter::send(stream,F(" *>\n"));
                 return true;
            }
          switch (p[0]) {
            case HASH_KEYWORD_PAUSE: // </ PAUSE>
                 if (paramCount!=1) return false;
                 DCC::setThrottle(0,1,true);  // pause all locos on the track         
                 pausingTask=(RMFT2 *)1; // Impossible task address
                 return true;
                 
            case HASH_KEYWORD_RESUME: // </ RESUME>
                 if (paramCount!=1) return false;
                 pausingTask=NULL;
                 {
                  RMFT2 * task=loopTask;                 
                  while(task) {
                      if (task->loco) task->driveLoco(task->speedo);
                      task=task->next;      
                      if (task==loopTask) break;      
                    }
                 }        
                 return true;
                 
                      
            case HASH_KEYWORD_START: // </ START [cab] route >
                 if (paramCount<2 || paramCount>3) return false;
                 {
                  int route=(paramCount==2) ? p[1] : p[2];
                  uint16_t cab=(paramCount==2)? 0 : p[1];
                  int pc=sequenceLookup->find(route);                 
                  if (pc<0) return false;
                    RMFT2* task=new RMFT2(pc);
                    task->loco=cab;                    
                 }
              return true;

            case HASH_KEYWORD_ROUTES: // </ ROUTES > JMRI withrottle support 
                 if (paramCount>1) return false;
                 StringFormatter::send(stream,F("</ROUTES "));
                 emitWithrottleRouteList(stream);                                  
                 StringFormatter::send(stream,F(">"));
              return true;
              
            default:
              break;
          }
          
          // all other / commands take 1 parameter 0 to MAX_FLAGS-1     

          if (paramCount!=2 || p[1]<0  || p[1]>=MAX_FLAGS) return false;

          switch (p[0]) {  
               case HASH_KEYWORD_KILL: // Kill taskid
                    {
                    RMFT2 * task=loopTask;
                    while(task) {
                      if (task->taskId==p[1]) {
                         delete task;
                         return  true;
                      }
                      task=task->next;      
                      if (task==loopTask) break;      
                    }
                 }
                 return false;
                    
            case HASH_KEYWORD_RESERVE:  // force reserve a section
                 setFlag(p[1],SECTION_FLAG);
                 return true;
    
            case HASH_KEYWORD_FREE:  // force free a section
                 setFlag(p[1],0,SECTION_FLAG);
                 return true;
                
            case HASH_KEYWORD_LATCH:
                 setFlag(p[1], LATCH_FLAG);
                 return true;
   
            case HASH_KEYWORD_UNLATCH:
                 setFlag(p[1], 0, LATCH_FLAG);
                 return true;
                  
            default:
                 return false;                 
          }
    }


// This emits Routes and Automations to Withrottle
// Automations are given a state to set the button to "handoff" which implies 
// handing over the loco to the automation.
// Routes are given "Set" buttons and do not cause the loco to be handed over. 
void RMFT2::emitWithrottleRouteList(Print* stream) {
   StringFormatter::send(stream,F("PRT]\\[Routes}|{Route]\\[Set}|{2]\\[Handoff}|{4\nPRL"));
   emitWithrottleDescriptions(stream);
   StringFormatter::send(stream,F("\n"));
}


RMFT2::RMFT2(int progCtr) {
  progCounter=progCtr;

  // get an unused  task id from the flags table 
  taskId=255; // in case of overflow
  for (int f=0;f<MAX_FLAGS;f++) {
    if (!getFlag(f,TASK_FLAG)) {
      taskId=f;
      setFlag(f, TASK_FLAG);
      break;
    }
  }
  delayTime=0;
  loco=0;
  speedo=0;
  forward=true;
  invert=false;
  stackDepth=0;
  onTurnoutId=0; // Not handling an ONTHROW/ONCLOSE
   
  // chain into ring of RMFTs
  if (loopTask==NULL) {
    loopTask=this;
    next=this;
  }
  else {
        next=loopTask->next;
        loopTask->next=this;
  }
}


RMFT2::~RMFT2() {
  driveLoco(1); // ESTOP my loco if any 
  setFlag(taskId,0,TASK_FLAG); // we are no longer using this id
  if (next==this) loopTask=NULL;
  else for (RMFT2* ring=next;;ring=ring->next) if (ring->next == this) {
           ring->next=next;
           loopTask=next;
           break;
       }
}

void RMFT2::createNewTask(int route, uint16_t cab) {
      int pc=sequenceLookup->find(route);
      if (pc<0) return;
      RMFT2* task=new RMFT2(pc);
      task->loco=cab;
}

void RMFT2::driveLoco(byte speed) {
     if (loco<=0) return;  // Prevent broadcast! 
     if (diag) DIAG(F("EXRAIL drive %d %d %d"),loco,speed,forward^invert);
     if (DCCWaveform::mainTrack.getPowerMode()==POWERMODE::OFF) {
        DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON); 
        Serial.println(F("<p1>")); // tell JMRI
     }
     DCC::setThrottle(loco,speed, forward^invert);
     speedo=speed;
}

bool RMFT2::readSensor(uint16_t sensorId) {
  // Exrail operands are unsigned but we need the signed version as inserted by the macros.  
  int16_t sId=(int16_t) sensorId;

  VPIN vpin=abs(sId);
  if (getFlag(vpin,LATCH_FLAG)) return true; // latched on

  // negative sensorIds invert the logic (e.g. for a break-beam sensor which goes OFF when detecting)
  bool s= IODevice::read(vpin) ^ (sId<0);
  if (s && diag) DIAG(F("EXRAIL Sensor %d hit"),sId);
  return s;
}

bool RMFT2::skipIfBlock() {
  // returns false if killed
  short nest = 1;
  while (nest > 0) {
    SKIPOP;
    byte opcode =  GET_OPCODE;
    switch(opcode) {
      case OPCODE_ENDEXRAIL: 
           kill(F("missing ENDIF"), nest);
           return false;  
      case OPCODE_IF:
      case OPCODE_IFNOT:
      case OPCODE_IFRANDOM:
      case OPCODE_IFRESERVE:
           nest++;
           break;
      case OPCODE_ENDIF:
           nest--;
           break;
      default:
      break;
    }
  }
  return true; 
}



/* static */ void RMFT2::readLocoCallback(int16_t cv) {
     progtrackLocoId=cv;
}

void RMFT2::loop() {
  
  // Round Robin call to a RMFT task each time 
     if (loopTask==NULL) return; 
     loopTask=loopTask->next;
     if (pausingTask==NULL || pausingTask==loopTask) loopTask->loop2();
}    

  
void RMFT2::loop2() {
   if (delayTime!=0 && millis()-delayStart < delayTime) return;
     
  byte opcode = GET_OPCODE;
  int16_t operand =  GET_OPERAND(0);
  // if (diag) DIAG(F("RMFT2 %d %d"),opcode,operand); 
  // Attention: Returning from this switch leaves the program counter unchanged.
  //            This is used for unfinished waits for timers or sensors.
  //            Breaking from this switch will step to the next step in the route. 
  switch ((OPCODE)opcode) {
    
    case OPCODE_THROW:
         Turnout::setClosed(operand, false);
         break;
          
    case OPCODE_CLOSE:
         Turnout::setClosed(operand, true);
         break; 
    
    case OPCODE_REV:
      forward = false;
      driveLoco(operand);
      break;
    
    case OPCODE_FWD:
      forward = true;
      driveLoco(operand);
      break;
      
    case OPCODE_SPEED:
      driveLoco(operand);
      break;
    
    case OPCODE_INVERT_DIRECTION:
      invert= !invert;
      driveLoco(speedo);
      break;
      
    case OPCODE_RESERVE:
      if (getFlag(operand,SECTION_FLAG)) {
        driveLoco(0);
        delayMe(500);
        return;
      }
      setFlag(operand,SECTION_FLAG);
      break;
    
    case OPCODE_FREE:
      setFlag(operand,0,SECTION_FLAG);
      break;
    
    case OPCODE_AT:
      if (readSensor(operand)) break;
      delayMe(50);
      return;
    
    case OPCODE_AFTER: // waits for sensor to hit and then remain off for 0.5 seconds. (must come after an AT operation)
      if (readSensor(operand)) {
        // reset timer to half a second and keep waiting
        waitAfter=millis();
        delayMe(50);
        return; 
      }
      if (millis()-waitAfter < 500 ) return;   
      break;
    
    case OPCODE_LATCH:
      setFlag(operand,LATCH_FLAG);
      break;
    
    case OPCODE_UNLATCH:
      setFlag(operand,0,LATCH_FLAG);
      break;

    case OPCODE_SET:
      IODevice::write(operand,true);
      break;
  
    case OPCODE_RESET:
      IODevice::write(operand,false);
      break;
    
    case OPCODE_PAUSE:
         DCC::setThrottle(0,1,true);  // pause all locos on the track
         pausingTask=this;
         break;

    case OPCODE_POM:
        if (loco) DCC::writeCVByteMain(loco, operand, GET_OPERAND(1));
        break;

    case OPCODE_POWEROFF:
        DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
        DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
        DCC::setProgTrackSyncMain(false);       
        Serial.println(F("<p0>")); // Tell JMRI
        break;

    case OPCODE_RESUME:
         pausingTask=NULL;
         driveLoco(speedo);
         for (RMFT2 * t=next; t!=this;t=t->next) if (t->loco >0) t->driveLoco(t->speedo);
          break;        
    
    case OPCODE_IF: // do next operand if sensor set
      if (!readSensor(operand)) if (!skipIfBlock()) return;
      break;
    
    case OPCODE_IFNOT: // do next operand if sensor not set
      if (readSensor(operand)) if (!skipIfBlock()) return;
      break;
   
    case OPCODE_IFRANDOM: // do block on random percentage
      if (random(100)>=operand) if (!skipIfBlock()) return;
      break;
   
    case OPCODE_IFRESERVE: // do block if we successfully RERSERVE
      if (!getFlag(operand,SECTION_FLAG)) setFlag(operand,SECTION_FLAG);
      else if (!skipIfBlock()) return;
      break;
      
    case OPCODE_ENDIF:
      break;
    
    case OPCODE_DELAY:
      delayMe(operand*100L);
      break;
   
    case OPCODE_DELAYMINS:
      delayMe(operand*60L*1000L);
      break;
    
    case OPCODE_RANDWAIT:
      delayMe(random(operand)*100L);
      break;
    
    case OPCODE_RED:
      doSignal(operand,true,false,false);
      break;
    
    case OPCODE_AMBER:
      doSignal(operand,false,true,false);
      break;
    
    case OPCODE_GREEN:
      doSignal(operand,false,false,true);
      break;
       
    case OPCODE_FON:      
      if (loco) DCC::setFn(loco,operand,true);
      break;
     
    case OPCODE_FOFF:
      if (loco) DCC::setFn(loco,operand,false);
      break;
    
    case OPCODE_XFON:      
      DCC::setFn(operand,GET_OPERAND(1),true);
      break;
   
    case OPCODE_XFOFF:      
      DCC::setFn(operand,GET_OPERAND(1),false);
      break;
   
    case OPCODE_FOLLOW:
      progCounter=sequenceLookup->find(operand);
      if (progCounter<0) kill(F("FOLLOW unknown"), operand); 
      return;
  
    case OPCODE_CALL:
      if (stackDepth==MAX_STACK_DEPTH) {
        kill(F("CALL stack"), stackDepth);
        return;
      }
      callStack[stackDepth++]=progCounter+3;
      progCounter=sequenceLookup->find(operand);
      if (progCounter<0) kill(F("CALL unknown"),operand); 
      return;

    case OPCODE_RETURN:
      if (stackDepth==0) {
        kill(F("RETURN stack"));
        return;
      }
      progCounter=callStack[--stackDepth];
      return;
      
    case OPCODE_ENDTASK:
    case OPCODE_ENDEXRAIL:
      kill();
      return;
      
    case OPCODE_JOIN:
       DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON); 
       DCCWaveform::progTrack.setPowerMode(POWERMODE::ON); 
       DCC::setProgTrackSyncMain(true);
       Serial.println(F("<p1 JOIN>")); // Tell JMRI
       break;

    case OPCODE_UNJOIN:
       DCC::setProgTrackSyncMain(false);
       break;
       
    case OPCODE_READ_LOCO1: // READ_LOCO is implemented as 2 separate opcodes
       progtrackLocoId=LOCO_ID_WAITING;  // Nothing found yet
       DCC::getLocoId(readLocoCallback);
       break;
      
      case OPCODE_READ_LOCO2:
       if (progtrackLocoId==LOCO_ID_WAITING) {
        delayMe(100);
        return; // still waiting for callback
       }
       if (progtrackLocoId<0) {
        kill(F("No Loco Found"),progtrackLocoId);
        return; // still waiting for callback
       }
       
       loco=progtrackLocoId;
       speedo=0;
       forward=true;
       invert=false;
       break;
       
       case OPCODE_START:
           {
            int newPc=sequenceLookup->find(operand);
            if (newPc<0) break; 
            new RMFT2(newPc); 
           }
           break;
           
      case OPCODE_SENDLOCO:  // cab, route
           {
            int newPc=sequenceLookup->find(GET_OPERAND(1));
            if (newPc<0) break; 
            RMFT2* newtask=new RMFT2(newPc); // create new task 
            newtask->loco=operand; 
           }
           break;
           
       case OPCODE_SETLOCO:
           {
             loco=operand; 
             speedo=0;
             forward=true;
             invert=false;
            }
       break;

          
       case OPCODE_SERVO: // OPCODE_SERVO,V(vpin),OPCODE_PAD,V(position),OPCODE_PAD,V(profile),OPCODE_PAD,V(duration)
        IODevice::writeAnalogue(operand,GET_OPERAND(1),GET_OPERAND(2),GET_OPERAND(3));          
        break;
       
       case OPCODE_WAITFOR: // OPCODE_SERVO,V(pin)
            if (IODevice::isBusy(operand)) {
              delayMe(100);
              return;
            }
            break;

       case OPCODE_PRINT:
            printMessage(operand);
            break;
               
       case OPCODE_ROUTE:
       case OPCODE_AUTOMATION:
       case OPCODE_SEQUENCE:
          if (diag) DIAG(F("EXRAIL begin(%d)"),operand);
          break;

       case OPCODE_AUTOSTART: // Handled only during begin process
       case OPCODE_PAD: // Just a padding for previous opcode needing >1 operad byte.
       case OPCODE_SIGNAL: // Signal definition ignore at run time
       case OPCODE_TURNOUT: // Turnout definition ignored at runtime
       case OPCODE_SERVOTURNOUT: // Turnout definition ignored at runtime
       case OPCODE_PINTURNOUT: // Turnout definition ignored at runtime
       case OPCODE_ONCLOSE: // Turnout event catcers ignored here
       case OPCODE_ONTHROW: // Turnout definition ignored at runtime
       break;
    
    default:
      kill(F("INVOP"),operand);
    }
    // Falling out of the switch means move on to the next opcode
    SKIPOP;
}

void RMFT2::delayMe(long delay) {
     delayTime=delay;
     delayStart=millis();
}

void RMFT2::setFlag(VPIN id,byte onMask, byte offMask) {  
   if (FLAGOVERFLOW(id)) return; // Outside range limit
   byte f=flags[id];
   f &= ~offMask;
   f |= onMask;
   flags[id]=f;
}

bool RMFT2::getFlag(VPIN id,byte mask) {
   if (FLAGOVERFLOW(id)) return 0; // Outside range limit
   return flags[id]&mask;   
}

void RMFT2::kill(const FSH * reason, int operand) {
     if (reason) DIAG(F("EXRAIL ERROR pc=%d, cab=%d, %S %d"), progCounter,loco, reason, operand);
     else if (diag) DIAG(F("ENDTASK at pc=%d"), progCounter); 
     delete this;
}

/* static */ void RMFT2::doSignal(VPIN id,bool red, bool amber, bool green) { 
  // CAUTION: hides class member progCounter
  int progCounter=signalLookup->find(id);
  if (progCounter<0) return; 
  VPIN redpin=GET_OPERAND(0);
  if (redpin!=id) return; // something wrong in lookup
  VPIN amberpin=GET_OPERAND(1);
  VPIN greenpin=GET_OPERAND(2);
  // If amberpin is zero, synthesise amber from red+green
  IODevice::write(redpin,red || (amber && (amberpin==0)));
  if (amberpin) IODevice::write(amberpin,amber);
  if (greenpin) IODevice::write(greenpin,green || (amber && (amberpin==0)));
  }

 void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {
    // Hunt for an ONTHROW/ONCLOSE for this turnout 
    // caution hides class progCounter;
    int progCounter= (closed?onCloseLookup:onThrowLookup)->find(turnoutId);
    if (progCounter<0) return;
    
    // Check we dont already have a task running this turnout
    RMFT2 * task=loopTask;                 
    while(task) {
      if (task->onTurnoutId==turnoutId) {
        DIAG(F("Recursive ONTHROW/ONCLOSE for Turnout %d"),turnoutId);
        return;
        }
      task=task->next;      
      if (task==loopTask) break;      
      }
    
     task->onTurnoutId=turnoutId; // flag for recursion detector
     task=new RMFT2(progCounter);  // new task starts at this instruction
 }
 
 void RMFT2::printMessage2(const FSH * msg) {
        DIAG(F("EXRAIL(%d) %S"),loco,msg);
   }

// This is called by emitRouteDescriptions to emit a withrottle description for a route or autoomation. 
void RMFT2::emitRouteDescription(Print * stream, char type, int id, const FSH * description) {
 StringFormatter::send(stream,F("]\\[%c%d}|{%S}|{%c"),
              type,id,description, type=='R'?'2':'4');
}