// Production 17 Function DCC Decoder 
// Version 3.0  Geoff Bunza 2014
// Uses modified software servo Lib
//
// This configuration supports 5 Modes per pin:
// 0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
// It is recommended that you NOT MIX pulsed and servo control
// simultaneously as the servo timing will be off
// ******** UNLESS YOU WANT ALL CV'S RESET UPON EVERY POWER UP
// ******** AFTER THE INITIAL DECODER LOAD REMOVE THE "//" IN THE FOOLOWING LINE!!
//#define DECODER_LOADED
  
#include <NmraDcc.h>
#include <SoftwareServo.h> 

SoftwareServo servo0;
SoftwareServo servo1;
SoftwareServo servo2;
SoftwareServo servo3;
SoftwareServo servo4;
SoftwareServo servo5;
SoftwareServo servo6;
SoftwareServo servo7;
SoftwareServo servo8;
SoftwareServo servo9;
SoftwareServo servo10;
SoftwareServo servo11;
SoftwareServo servo12;
SoftwareServo servo13;
SoftwareServo servo14;
SoftwareServo servo15;
SoftwareServo servo16;
#define servo_start_delay 50
#define servo_init_delay 7

int tim_delay = 500;
int numfpins = 17;
byte fpins [] = {3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19};
const int FunctionPin0 = 3;
const int FunctionPin1 = 4;
const int FunctionPin2 = 5;
const int FunctionPin3 = 6;
const int FunctionPin4 = 7;

const int FunctionPin5 = 8;
const int FunctionPin6 = 9;
const int FunctionPin7 = 10;
const int FunctionPin8 = 11;

const int FunctionPin9 = 12;
const int FunctionPin10 = 13;
const int FunctionPin11 = 14;     //A0
const int FunctionPin12 = 15;     //A1

const int FunctionPin13 = 16;     //A2
const int FunctionPin14 = 17;     //A3 & LOAD ACK
const int FunctionPin15 = 18;     //A4
const int FunctionPin16 = 19;     //A5
NmraDcc  Dcc ;
DCC_MSG  Packet ;
uint8_t CV_DECODER_MASTER_RESET = 120;
int t;  // temp
#define This_Decoder_Address 24
struct QUEUE
{
  int inuse;
  int current_position;
  int increment;
  int stop_value;
  int start_value;
};
QUEUE *ftn_queue = new QUEUE[16];

struct CVPair
{
  uint16_t  CV;
  uint8_t   Value;
};
CVPair FactoryDefaultCVs [] =
{
  {CV_MULTIFUNCTION_PRIMARY_ADDRESS, This_Decoder_Address},
  {CV_ACCESSORY_DECODER_ADDRESS_MSB, 0},
  {CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB, 0},
  {CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB, 0},
  {CV_DECODER_MASTER_RESET, 0},
  {30, 4}, //F0 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {31, 10},    //F0 Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {32, 28},   //F0  Start Position F0=0
  {33, 140},  //F0  End Position   F0=1
  {34, 0},   //F0  Current Position
  {35, 4},  //F1 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {36, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {37, 28},   //  Start Position Fx=0
  {38, 140},  //  End Position   Fx=1
  {39, 0},  //  Current Position
  {40, 4},  //F2 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {41, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {42, 28},   //  Start Position Fx=0
  {43, 140},  //  End Position   Fx=1
  {44, 0},    //  Current Position
  {45, 4}, //F3 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {46, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {47, 28},   //  Start Position Fx=0
  {48, 140},  //  End Position   Fx=1
  {49, 0},    //  Current Position
  {50, 4}, //F4 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {51, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {52, 28},    //  Start Position Fx=0
  {53, 140},    //  End Position   Fx=1
  {54, 0},    //  Current Position
  {55, 4}, //F5 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {56, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {57, 28},    //  Start Position Fx=0
  {58, 140},    //  End Position   Fx=1
  {59, 0},    //  Current Position
  {60, 4}, //F6 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {61, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {62, 28},    //  Start Position Fx=0
  {63, 140},    //  End Position   Fx=1
  {64, 0},    //  Current Position
  {65, 4}, //F7 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {66, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {67, 1},   //  Start Position Fx=0
  {68,35},  //  End Position   Fx=1
  {69, 0},    //  Current Position
  {70, 4}, //F8 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {71, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {72, 1},   //  Start Position Fx=0
  {73, 100},  //  End Position   Fx=1
  {74, 0},    //  Current Position
  {75, 4}, //F9 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {76, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {77, 1},   //  Start Position Fx=0
  {78, 10},  //  End Position   Fx=1
  {79, 0},    //  Current Position
  {80, 4}, //F10 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {81, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {82, 1},   //  Start Position Fx=0
  {83, 5},  //  End Position   Fx=1
  {84, 0},    //  Current Position
  {85, 4}, //F11 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {86, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {87, 1},   //  Start Position Fx=0
  {88, 5},  //  End Position   Fx=1
  {89, 0},    //  Current Position
  {90, 4}, //F12 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {91, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {92, 1},   //  Start Position Fx=0
  {93, 20},  //  End Position   Fx=1
  {94, 0},    //  Current Position
  {95, 4}, //F13 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {96, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {97, 1},   //  Start Position Fx=0
  {98, 35},  //  End Position   Fx=1
  {99, 0},    //  Current Position
  {100, 4}, //F14 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {101, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {102, 1},   //  Start Position Fx=0
  {103, 4},  //  End Position   Fx=1
  {104, 0},    //  Current Position
  {105, 4}, //F15 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {106, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {107, 1},   //  Start Position Fx=0
  {108, 60},  //  End Position   Fx=1
  {109, 0},    //  Current Position
  {110, 4}, //F16 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {111, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {112, 1},   //  Start Position Fx=0
  {113, 4},  //  End Position   Fx=1
  {114, 0},    //  Current Position
//FUTURE USE
  {115, 0}, //F17 Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink,4=Pulsed
  {116, 10},    // Rate  Blink=Eate,PWM=Rate,Servo=Rate,Pulsed=Milliseconds/10
  {117, 28},   //  Start Position Fx=0
  {118, 50},  //  End Position   Fx=1
  {119, 28},    //  Current Position
};

uint8_t FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs)/sizeof(CVPair);
void notifyCVResetFactoryDefault()
{
  // Make FactoryDefaultCVIndex non-zero and equal to num CV's to be reset 
  // to flag to the loop() function that a reset to Factory Defaults needs to be done
  FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs)/sizeof(CVPair);
};

void setup()   //******************************************************
{
  int i;
  uint8_t cv_value;
  //Serial.begin(115200);
  // initialize the digital pins as outputs
    for (int i=0; i < numfpins; i++) {
      pinMode(fpins[i], OUTPUT);
      digitalWrite(fpins[i], 0);
     }
  for (int i=0; i < numfpins; i++) {
     digitalWrite(fpins[i], 1);
     delay (tim_delay/10);
  }
  delay( tim_delay);
  for (int i=0; i < numfpins; i++) {
     digitalWrite(fpins[i], 0);
     delay (tim_delay/10);
  }
  delay( tim_delay);
  
  // Setup which External Interrupt, the Pin it's associated with that we're using 
  Dcc.pin(0, 2, 0);
  // Call the main DCC Init function to enable the DCC Receiver
  Dcc.init( MAN_ID_DIY, 100, FLAGS_MY_ADDRESS_ONLY, 0 );
  delay(800);
   
  #if defined(DECODER_LOADED)
  if ( Dcc.getCV(CV_DECODER_MASTER_RESET)== CV_DECODER_MASTER_RESET ) 
  #endif  
  
     {
       for (int j=0; j < FactoryDefaultCVIndex; j++ )
         Dcc.setCV( FactoryDefaultCVs[j].CV, FactoryDefaultCVs[j].Value);
         digitalWrite(fpins[14], 1);
         delay (1000);
         digitalWrite(fpins[14], 0);
     }
  for ( i=0; i < numfpins; i++) {
    cv_value = Dcc.getCV( 30+(i*5)) ;   
    //Serial.print(" cv_value: ");
    //Serial.println(cv_value, DEC) ;
    switch ( cv_value ) {
      case 0:   // LED on/off
        ftn_queue[i].inuse = 0;
        break;
      case 1:   // LED Blink
         {
           ftn_queue[i].inuse = 0;
		   ftn_queue[i].current_position = 0;
           ftn_queue[i].start_value = 0;
           ftn_queue[i].increment = int (char (Dcc.getCV( 31+(i*5))));
           digitalWrite(fpins[i], 0);
           ftn_queue[i].stop_value = int(Dcc.getCV( 33+(i*5))) ;
         }
        break;
      case 2:   //servo
       { ftn_queue[i].current_position =int (Dcc.getCV( 34+(i*5)));
         ftn_queue[i].stop_value = int (Dcc.getCV( 33+(i*5)));
         ftn_queue[i].start_value = int (Dcc.getCV( 32+(i*5)));
         ftn_queue[i].increment = -int (char (Dcc.getCV( 31+(i*5)))); 
         switch ( i ) {
         case 0: servo0.attach(FunctionPin0);  // attaches servo on pin to the servo object 
           ftn_queue[i].inuse = 1;
           servo0.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 1:  servo1.attach(FunctionPin1);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo1.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 2: servo2.attach(FunctionPin2);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo2.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 3: servo3.attach(FunctionPin3);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo3.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 4: servo4.attach(FunctionPin4);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo4.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 5: servo5.attach(FunctionPin5);  // attaches servo on pin to the servo object  
           ftn_queue[i].inuse = 1;
           servo5.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 6: servo6.attach(FunctionPin6);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo6.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 7: servo7.attach(FunctionPin7);  // attaches servo on pin to the servo object  
           ftn_queue[i].inuse = 1;
           servo7.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 8: servo8.attach(FunctionPin8);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo8.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 9: servo9.attach(FunctionPin9);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo9.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 10: servo10.attach(FunctionPin10);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo10.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 11: servo11.attach(FunctionPin11);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo11.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 12: servo12.attach(FunctionPin12);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo12.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 13: servo13.attach(FunctionPin13);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo13.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 14: servo14.attach(FunctionPin14);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo14.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 15: servo15.attach(FunctionPin15);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo15.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         case 16: servo16.attach(FunctionPin16);  // attaches servo on pin to the servo object
           ftn_queue[i].inuse = 1;
           servo16.write(ftn_queue[i].start_value);
           for (t=0; t<servo_start_delay; t++) {SoftwareServo::refresh();delay(servo_init_delay);}
           break;
         default:
           break;
          }
        }
        break;
       case 3:   // DOUBLE ALTERNATING LED Blink
         {
           ftn_queue[i].inuse = 0;
		   ftn_queue[i].current_position = 0;
           ftn_queue[i].start_value = 0;
           ftn_queue[i].increment = Dcc.getCV( 31+(i*5));
           digitalWrite(fpins[i], 0);
           digitalWrite(fpins[i+1], 0);
           ftn_queue[i].stop_value = int(Dcc.getCV( 33+(i*5)));
         }
         break;
	   case 4:   // Simple Pulsed Output based on saved Rate =10*Rate in Milliseconds
		 {
		   ftn_queue[i].inuse = 0;
		   ftn_queue[i].current_position = 0;
           ftn_queue[i].increment = 10 * int (char (Dcc.getCV( 31+(i*5))));
           digitalWrite(fpins[i], 0);
		 }
         break;
       case 5:   //FUTURE FUNCTION
         break;         
       default:
         break;
    }
  }
}

void loop()   //**********************************************************************
{
  //MUST call the NmraDcc.process() method frequently 
  // from the Arduino loop() function for correct library operation

  Dcc.process();
  SoftwareServo::refresh();
  delay(8);
  for (int i=0; i < numfpins; i++) {
    if (ftn_queue[i].inuse==1)  {
    ftn_queue[i].current_position = ftn_queue[i].current_position + ftn_queue[i].increment;
    switch (Dcc.getCV( 30+(i*5))) {
      case 0:
        break;
      case 1:
        if (ftn_queue[i].current_position > ftn_queue[i].stop_value) {
          ftn_queue[i].start_value = ~ftn_queue[i].start_value;
          digitalWrite(fpins[i], ftn_queue[i].start_value);
          ftn_queue[i].current_position = 0;
          ftn_queue[i].stop_value = int(Dcc.getCV( 33+(i*5)));
        }
        break;
      case 2:
        {
        if (ftn_queue[i].increment > 0) {
          if (ftn_queue[i].current_position > ftn_queue[i].stop_value) 
            ftn_queue[i].current_position = ftn_queue[i].stop_value;
        } 
        if (ftn_queue[i].increment < 0) { 
          if (ftn_queue[i].current_position < ftn_queue[i].start_value) 
            ftn_queue[i].current_position = ftn_queue[i].start_value;
        }
        set_servo(i, ftn_queue[i].current_position);
        }
        break;
      case 3:
        if (ftn_queue[i].current_position > ftn_queue[i].stop_value) {
          ftn_queue[i].start_value = ~ftn_queue[i].start_value;
          digitalWrite(fpins[i], ftn_queue[i].start_value);
          digitalWrite(fpins[i]+1, ~ftn_queue[i].start_value);
          ftn_queue[i].current_position = 0;
          ftn_queue[i].stop_value = int(Dcc.getCV( 33+(i*5)));
        }
        i++;
        break;
      case 4:   // Simple Pulsed Output based on saved Rate =10*Rate in Milliseconds
	    break;
	  case 5:  //FUTURE FUNCTION
        break;
      default:
        break;  
      }
    }
  }
}

void notifyDccFunc( uint16_t Addr, DCC_ADDR_TYPE AddrType, FN_GROUP FuncGrp, uint8_t FuncState)  {
  switch(FuncGrp)
  {
  case FN_0_4:    //Function Group 1 F0 F4 F3 F2 F1
	  exec_function( 0, FunctionPin0, (FuncState & FN_BIT_00)>>4 );
	  exec_function( 1, FunctionPin1, (FuncState & FN_BIT_01));
	  exec_function( 2, FunctionPin2, (FuncState & FN_BIT_02)>>1);
	  exec_function( 3, FunctionPin3, (FuncState & FN_BIT_03)>>2 );
	  exec_function( 4, FunctionPin4, (FuncState & FN_BIT_04)>>3 );
      break;
	  
  case FN_5_8:    //Function Group 1 S FFFF == 1 F8 F7 F6 F5  &  == 0  F12 F11 F10 F9 F8
  	  exec_function( 5, FunctionPin5, (FuncState & FN_BIT_05));
	  exec_function( 6, FunctionPin6, (FuncState & FN_BIT_06)>>1 );
	  exec_function( 7, FunctionPin7, (FuncState & FN_BIT_07)>>2 );
	  exec_function( 8, FunctionPin8, (FuncState & FN_BIT_08)>>3 );
	  break;
	  
  case FN_9_12:
	  exec_function( 9, FunctionPin9,   (FuncState & FN_BIT_09));
	  exec_function( 10, FunctionPin10, (FuncState & FN_BIT_10)>>1 );
	  exec_function( 11, FunctionPin11, (FuncState & FN_BIT_11)>>2 );
	  exec_function( 12, FunctionPin12, (FuncState & FN_BIT_12)>>3 );
	  break;

  case FN_13_20:   //Function Group 2 FuncState == F20-F13 Function Control
      exec_function( 13, FunctionPin13, (FuncState & FN_BIT_13));
	  exec_function( 14, FunctionPin14, (FuncState & FN_BIT_14)>>1 );
	  exec_function( 15, FunctionPin15, (FuncState & FN_BIT_15)>>2 );
	  exec_function( 16, FunctionPin16, (FuncState & FN_BIT_16)>>3 );
      break;
	
  case FN_21_28:
      break;	
  }
}  
void exec_function (int function, int pin, int FuncState)  {
  switch ( Dcc.getCV( 30+(function*5)) )  {  // Config  0=On/Off,1=Blink,2=Servo,3=Double LED Blink
    case 0:    // On - Off LED
      //Serial.println("****************cv:0 ") ;
      digitalWrite (pin, FuncState);
      ftn_queue[function].inuse = 0;
      break;
    case 1:    // Blinking LED
      if ((ftn_queue[function].inuse==0) && (FuncState==1))  {
        ftn_queue[function].inuse = 1;
        ftn_queue[function].start_value = 0;
        digitalWrite(pin, 0);
        ftn_queue[function].stop_value = int(Dcc.getCV( 33+(function*5)));
      } else {
          if ((ftn_queue[function].inuse==1) && (FuncState==0)) {
            ftn_queue[function].inuse = 0;
            digitalWrite(pin, 0);
          }
        }
      break;
    case 2:    // Servo
      ftn_queue[function].inuse = 1;
      if (FuncState==1) ftn_queue[function].increment = char ( Dcc.getCV( 31+(function*5)));
        else ftn_queue[function].increment = - char(Dcc.getCV( 31+(function*5)));
      if (FuncState==1) ftn_queue[function].stop_value = Dcc.getCV( 33+(function*5));
        else ftn_queue[function].stop_value = Dcc.getCV( 32+(function*5));
      /*
      Serial.print("servo inc: ") ;
      Serial.print(ftn_queue[function].increment,DEC) ;
      Serial.print("servo inuse: ") ;
      Serial.print(ftn_queue[function].inuse,DEC) ;
      Serial.print("servo num: ") ;
      Serial.print(function,DEC) ;
      Serial.print(" stop: ");
      Serial.println(ftn_queue[function].stop_value,DEC) ;
      */
      break;
    case 3:    // Blinking LED PAIR
      if ((ftn_queue[function].inuse==0) && (FuncState==1))  {
        ftn_queue[function].inuse = 1;
        ftn_queue[function].start_value = 0;
        digitalWrite(fpins[function], 0);
        digitalWrite(fpins[function+1], 1);
        ftn_queue[function].stop_value = int(Dcc.getCV( 33+(function*5)));
      } else {
          if (FuncState==0) {
            ftn_queue[function].inuse = 0;
            digitalWrite(fpins[function], 0);
            digitalWrite(fpins[function+1], 0);
          }
        }
      break;
    case 4:    // Pulse Output based on Rate*10 Milliseconds
      if ((ftn_queue[function].inuse==0) && (FuncState==1)) {  //First Turn On Detected
        digitalWrite(fpins[function], 1);
		delay (10*ftn_queue[function].increment);
        digitalWrite(fpins[function], 0);
		ftn_queue[function].inuse = 1;                    //inuse set to 1 says we already pulsed
      } else 
          if (FuncState==0)  ftn_queue[function].inuse = 0;
      break;
	case 5:    // Future Function
      ftn_queue[function].inuse = 0;
      break;  
    default:
      ftn_queue[function].inuse = 0;
      break;
  }
}
void set_servo (int servo_num, int servo_pos)  {
    switch (servo_num) {
         case 0: servo0.write(servo_pos);
           break;
         case 1: servo1.write(servo_pos);
           break;
         case 2: servo2.write(servo_pos);
           break;
         case 3: servo3.write(servo_pos);
           break;
         case 4: servo4.write(servo_pos);
           break;
         case 5: servo5.write(servo_pos);
           break;
         case 6: servo6.write(servo_pos);
           break;
         case 7: servo7.write(servo_pos);  
           break;
         case 8: servo8.write(servo_pos);
           break;
         case 9: servo9.write(servo_pos);
           break;
         case 10: servo10.write(servo_pos);
           break;
         case 11: servo11.write(servo_pos);
           break;
         case 12: servo12.write(servo_pos);
           break;
         case 13: servo13.write(servo_pos);
           break;
         case 14: servo14.write(servo_pos);
           break;
         case 15: servo15.write(servo_pos);
           break;
         case 16: servo16.write(servo_pos);
           break;
         default:
           break;
    }
}