// DCC Decoder wac 12 Nov 2019
// new sketch for v2 PCB dated Oct 2019
// 8 x servo points 6 led on/off  2 flashing led
  
#include <NmraDcc.h>
#include <VarSpeedServo.h> 

VarSpeedServo myservo3; // T1
VarSpeedServo myservo4; // T2
VarSpeedServo myservo5; // T3
VarSpeedServo myservo6; // T4
VarSpeedServo myservo7; // T5
VarSpeedServo myservo8; // T6
VarSpeedServo myservo9; // T7
VarSpeedServo myservo10; // T8

int min_pos[20];
int max_pos[20];
int cur_pos[20];
int servo_speed;

int MainAddr;
int last_tun = 0;
int tun;
int state;
int last_state;

int addr1; int addr2; int addr3; int addr4;
             
int led17State = LOW; // ledState used to set the LED
int led18State = LOW;
int led19State = LOW;

long interval;
long previousMillis = 0;       
unsigned long currentMillis;

boolean led17, led18;  // o/p 17,18 set for flashing led

NmraDcc  Dcc ;
DCC_MSG  Packet ;

struct CVPair{
  uint16_t  CV;
  uint8_t   Value;
};

void setup() {
  Serial.begin(115200);
 
  addr1 = 1; // gives decoder address of 1 to 4
  addr2 = 2; // gives decoder address of 5 to 8
  addr3 = 3; // gives decoder address of 9 to 12
  addr4 = 4; // gives decoder address of 13 to 16
  servo_speed = 15; // sets speed of servo movement
  interval = 500;  // interval at which to flash led (milliseconds)
  // led pins
  pinMode(11, OUTPUT); // T9 led on/off
  pinMode(12, OUTPUT); // T10 led on/off
  pinMode(13, OUTPUT); // T11 led on/off
  pinMode(14, OUTPUT); // T12 led on/off
  pinMode(15, OUTPUT); // T13 led on/off 
  pinMode(16, OUTPUT); // T14 led on/off 
  pinMode(17, OUTPUT); // T15  flash
  pinMode(18, OUTPUT); // T16  flash alternate with 19
  pinMode(19, OUTPUT); // T16  flash alternate with 18
     
  led17 = false;
  led18 = false;

  // Setup which External Interrupt, the Pin it's associated with that we're using, disable pullup.
  Dcc.pin(0, 2, 0);
  // Call the main DCC Init function to enable the DCC Receiver
  Dcc.init( MAN_ID_DIY, 100,   FLAGS_DCC_ACCESSORY_DECODER , 0 ); 
  Serial.println("Ready");
  delay(1000);
  for (int i = 0; i<=20; i++){
    min_pos[i] = 30; max_pos[i] = 90; cur_pos[i] = 30;
  }
   min_pos[3] = 35; max_pos[3] = 82; //D3 servo 1
   min_pos[4] = 56; max_pos[4] = 90; // D4 servo 2  
   min_pos[5] = 35; max_pos[5] = 82; // D5 servo 3  
   min_pos[6] = 36; max_pos[6] = 100; // D6 servo 4  
   min_pos[7] = 55; max_pos[7] = 135; // D7 servo 5  
   min_pos[8] = 40; max_pos[8] = 110; // D8 servo 6  
   min_pos[9] = 55; max_pos[9] = 88;  // D9 servo 7  
   min_pos[10] = 30; max_pos[10] = 84; // D10 servo 8  
}

void loop(){
  // You MUST call the NmraDcc.process() method frequently from the Arduino loop() function for correct library operation
  MainAddr = 0;
  currentMillis = millis();  // led flash timer
     if(currentMillis - previousMillis > interval) {
         previousMillis = currentMillis;   
            flash17();
            flash1819();   
      }
  Dcc.process();
}

extern void notifyDccMsg( DCC_MSG * Msg ) {
  if( ( Msg->Data[0] == 0 ) && ( Msg->Data[1] == 0 ) ) return;  //reset packlet
  if( ( Msg->Data[0] == 0b11111111 ) && ( Msg->Data[1] == 0 ) ) return;  //idle packet
  if(Msg->Data[0] == 100 && Msg->Data[1] == 63) return;
  
if ((Msg->Data[1]>>3) == 0x1f){    // B11111000 (0xf8) shifted 3 places to give B11111 (0x1f)    
  if( (Msg->Data[0] == 0x81) || (Msg->Data[0] == 0x82) || (Msg->Data[0] == 0x83) ||(Msg->Data[0] == 0x84)){ //for Addr = 1, Addr = 2, Addr3 = 3, Addr4 = 4 
  MainAddr = (Msg->Data[0] - 0x80);
  tun = (((Msg->Data[1]) - 0xf8>>1) + 1);
  state = (Msg->Data[1] & 0x01);
   if (last_tun == tun && last_state == state) return;
   last_tun = tun;
   last_state = state;
   Serial.print("MainAddr = ");
   Serial.println(MainAddr);
   
   Serial.print("DccMsg - ");
   Serial.print(Msg->Size);
     Serial.print(" ");
     for(int i=0;i<Msg->Size;i++){
     Serial.print(Msg->Data[i], DEC);
     Serial.print(" ");
     }
   Serial.println();
   Serial.println(Msg->Data[0], BIN);
   Serial.println(Msg->Data[1], BIN);
   Serial.println((Msg->Data[1] - 0xf8)>>1, BIN);
   Serial.println(((Msg->Data[1] - 0xf8)>>1) + 1, DEC);
   Serial.println(((Msg->Data[1] - 0xf8)>>1) + 1 + (MainAddr-1)*4);       
   Serial.println(Msg->Data[1] & 0x01);
   Serial.print("Turnout = ");
   Serial.println(((Msg->Data[1] - 0xf8) >>1) + 1 + (MainAddr-1)*4);
   Serial.print("tun = ");
   Serial.println(tun);
  }
}

if (MainAddr == addr1){   // T1 to T4
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 1){         
         // pin 3 T1
      myservo3.attach(3);delay(15);
      myservo3.write(max_pos[3],servo_speed,true);
      myservo3.detach();     
    }
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 0){       
         // pin 3  T1 
      myservo3.attach(3);delay(15);
      myservo3.write(min_pos[3],servo_speed,true);
      myservo3.detach(); 
    }
    
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 1){         
         // pin 4    T2 
      myservo4.attach(4);delay(15);
      myservo4.write(max_pos[4],servo_speed,true);
      myservo4.detach();    
    }      
    
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 0){        
         // pin 4     T2
      myservo4.attach(4);delay(15);
      myservo4.write(min_pos[4],servo_speed,true);
      myservo4.detach(); 
    } 
      
    if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 1){       
         // pin 5     T3 
     myservo5.attach(5);delay(15);
     myservo5.write(max_pos[5],servo_speed,true);
     myservo5.detach();
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 0){       
            // pin 5    T3  
     myservo5.attach(5);delay(15);
     myservo5.write(min_pos[5],servo_speed,true);
     myservo5.detach(); 
    } 
      
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 1){       
            // pin 6    T4  
     myservo6.attach(6);delay(15);
     myservo6.write(max_pos[6],servo_speed,true);
     myservo6.detach(); 
    } 
    
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 0){        
             // pin 6    T4  
    myservo6.attach(6);delay(15);
    myservo6.write(min_pos[6],servo_speed,true); 
    myservo6.detach(); 
    }   
}

if (MainAddr == addr2){  // T5 to T8
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 1){       
         // pin 7      T5
     myservo7.attach(7);delay(15);
     myservo7.write(max_pos[7],servo_speed,true);
     myservo7.detach(); 
    }  
        
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 0){        
             // pin 7     T5 
      myservo7.attach(7);delay(15);
      myservo7.write(min_pos[7],servo_speed,true); 
      myservo7.detach();   
    }   

  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 1){       
             // pin 8     T6 
      myservo8.attach(8);delay(15);
      myservo8.write(max_pos[8],servo_speed,true);
      myservo8.detach(); 
    } 
         
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 0){  
           // pin 8       T6
      myservo8.attach(8);delay(15);
      myservo8.write(min_pos[8],servo_speed,true); 
      myservo8.detach(); 
    }   
    
 if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 1){         
             // pin 9     T7 
       myservo9.attach(9);delay(15);
       myservo9.write(max_pos[9],servo_speed,true);
       myservo9.detach();
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 0){         
          // pin 9   T7
       myservo9.attach(9);delay(15);
       myservo9.write(min_pos[9],servo_speed,true); 
       myservo9.detach(); 
    }  
     
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 1){         
            // pin 10     T8
      myservo10.attach(10);delay(15);
      myservo10.write(max_pos[10],servo_speed,true);
      myservo10.detach();
    }
    
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 0){         
           // pin 10      T8
      myservo10.attach(10);delay(15);
      myservo10.write(min_pos[10],servo_speed,true); 
      myservo10.detach();
   }   
}

if (MainAddr == addr3){   // T9 to T12
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 1){         //    pin 11 led on 
         // pin 11      T9
      digitalWrite(11,1);
    }   
       
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 0){         //    pin 11 led off
             // pin 11     T9 
      digitalWrite(11,0);
    } 
      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 1){         //    pin 12 led on
             // pin 12      T10
      digitalWrite(12,1);
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 0){         //    pin 12 led off
          // pin 12     T10
      digitalWrite(12,0);  
    } 
     
 if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 1){         //   pin 13 led on
             // pin 13      T11
        digitalWrite(13,1);
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 0){         //   pin 13 led off
          // pin 13      T11
        digitalWrite(13,0);
    }   
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 1){         //  pin A0 led on
            // pin 14     A0  T12
        digitalWrite(14,1);
    }
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 0){         //   pin A0 led off
           // pin 14      A0  T12
        digitalWrite(14,0);
   }   
}

if (MainAddr == addr4){   // T13 to T16
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 1){         //    pin A1 led on 
         // pin 15      A1  T13
        digitalWrite(15,1);
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 1 && (Msg->Data[1] & 0x01) == 0){         //    pin A1 led  off
             // pin 15    A1  T13
        digitalWrite(15,0);
    }   
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 1){         //    pin A2 led on
             // pin 16     A2  T14
       digitalWrite(16,1);
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 2 && (Msg->Data[1] & 0x01) == 0){         //    pin A2 led off
          // pin 16    A2  T14
        digitalWrite(16,0);  
    }   
 if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 1){         //   pin A3 led flash on
             // pin 17     A3   T15
        led17 = true;
    }      
  if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 3 && (Msg->Data[1] & 0x01) == 0){         //   pin A3 led flash off
          // pin 17      A3  T15
        led17 = false;
        digitalWrite(17,0);
    }   
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 1){         //  pin A4 led flash on
            // pin 18     A4   T16
        led18 = true;
    }
   if ((((Msg->Data[1]) - 0xf8>>1) + 1) == 4 && (Msg->Data[1] & 0x01) == 0){         //   pin A4 led flash off
           // pin 18      A4   T16
       led18 = false;
       digitalWrite(18,0);digitalWrite(19,0);
   }   
}
Msg->Data[0] == 0;
}

void flash17(){
   if (led17){
       if (led17State == LOW)
        led17State = HIGH;
        else
        led17State = LOW;
      digitalWrite(17, led17State);
      } 
      else digitalWrite(17,0); 
}

void flash1819(){
  if (led18 == false){
  digitalWrite(18,0);digitalWrite(19,0);}
  if (led18){
   if (led18State == LOW)
        led18State = HIGH;
        else
        led18State = LOW;
      digitalWrite(18,led18State);digitalWrite(19,!led18State);}   
  }