/*
 * cds_2_loco_2_04
 * 2 CDS 2 locos forward, Target CDS auto-check
 *
 * 2015.6.8  suzu
 *
 */

#include <avr/wdt.h>
#include <SPI.h>
#include "DSGatewayLib.h"

#define ON   1
#define OFF  0
#define SETTING   1    // Loco Setting 1=Normal 0=opposite
#define SPEED_UP 3000   // speed up rate m-second
#define SPEED_DOWN 1000   // speed down rate m-second
#define SPEED_CONT 7    // speed up/down count
#define CMD_DELAY  100  // command delay time m-second
#define STOP_TIME 3000    // station stop time m-second
#define CDS_A 0    //  ***CDS_A analog port#
#define CDS_B 3    //  ***CDS_B analog port#
#define LIGHT_LEVEL_A 200  // CDS_A Light Level
#define LIGHT_LEVEL_B 200  // CDS_B Light Level
#define SOMETIME 99999  // Sometime happen
#define HMT  12    // ***How many trains on DB loco_d[][]
#define LOCO_A  12  // ***Loco A address (search loco_d[ HMT ][13] )
#define LOCO_B  5  // ***Loco B address (search loco_d[ HMT ][13] )
#define MUTE  OFF  // Sound mute ON/OFF

DSGatewayLib Gateway;

word  loco_d[ HMT ][17]= {
{ 9999, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 9999, 0, 1024, 0 },
{ 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 3, 0, 1024, 0 },
{ 5, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 5, 0, 500, 0 },
{ 6, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 6, 0, 1024, 0 },
{ 9, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 9, 0, 600, 0 },
{ 12, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 12, 0, 450, 0 },
{ 41, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 41, 0, 600, 0 },
{ 59, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 59, 0, 500, 0 },
{ 156, 3, 2, 2, 4, 5, 6, 7, 1, 9, 10, 11, 12, 56, 0, 500, 0 },
{ 236, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 36, 0, 600, 0 },
{ 278, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 78, 0, 400, 0 },
{ 341, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 341, 0, 700, 0 }
};
//  0=***Loco_Number, 1=Bell, 2=Whistle, 3=Short Whistle, 4=Steam Release,
//  5=xxx/Generator, 6=Water/Voice, 7=Light Dimmer/Cupler, 8=Mute sound, 
//  9=Water, 10=Injectors, 11=Break, 12=Cupler, 13=***DCC_Address,
//  14=Speed_cont_tmp, 15=***Normal_Speed_max=1024, 16=Speed_Now

unsigned char Forward = 1;  // ininitial go forward 2=backward
unsigned char  targetA, targetB;    //  target CDS, locoAadr or locoBadr
int  cdsA[2], cdsB[2];    //  cdsA[0] = Target Loco, cdsA[1]=CDS ON/OFF
int  locoAadr = 0, locoBadr = 0;
int  tmp;

//  define functions
void  set_speed( int arg )
{
    Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], arg );
    delay( CMD_DELAY );
    Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], arg );
    delay( CMD_DELAY );
}

void  set_direction( int arg )
{
    Gateway.SetLocoDirection( loco_d[ locoAadr ] [13], arg );
    delay( CMD_DELAY );
    Gateway.SetLocoDirection( loco_d[ locoBadr ] [13], arg );
    delay( CMD_DELAY );
}

void  set_function( unsigned char arg1, int arg2 )  // arg = time m-sec
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ][arg1], ON );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ][arg1], ON );
    delay( CMD_DELAY );
    delay( arg2 ); 
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ][arg1], OFF );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ][arg1], OFF );
    delay( CMD_DELAY );
}

void set_light( unsigned char on_off )
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], 0, on_off );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], 0, on_off );
    delay( CMD_DELAY );
}

void set_bell( int arg )   // arg = time m-sec
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [1], ON );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [1], ON );
    delay( CMD_DELAY );
    delay( arg );
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [1], OFF );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [1], OFF );
    delay( CMD_DELAY );
}

void set_whistle( int arg )    // arg = time m-sec
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [2], ON );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [2], ON );
    delay( CMD_DELAY );
    delay( arg );  
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [2], OFF );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [2], OFF );
    delay( CMD_DELAY );
}

void set_s_whistle( int arg )    // arg = time m-sec
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [3], ON );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [3], ON );
    delay( CMD_DELAY );
    delay( arg ); 
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [3], OFF );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [3], OFF );
    delay( CMD_DELAY );
}

void set_steam( int arg )    // arg = time m-sec
{
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [4], ON );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], ON );
    delay( CMD_DELAY );
    delay( arg ); 
    Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [4], OFF );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], OFF );
    delay( CMD_DELAY );
}

void set_blink( int arg )  // arg = time m-sec
{
    set_light( OFF );
    delay ( arg );
    set_light( ON );
}


//  ******************* setup ***********************

void setup()
{
    Serial.begin( 9600 );
    Gateway.begin();
    delay( 5000 );
    Gateway.SetPower( true );

    for( int i = 0 ; i < HMT; i++ )
    {
      if( loco_d[ i ] [13] == LOCO_A )
        locoAadr = i;
      if( loco_d[ i ] [13] == LOCO_B )
        locoBadr = i;
    } 

    #if  SETTING
      targetA = CDS_A;
      targetB = CDS_B;
      cdsA[0] = locoAadr;  // CDS_A waiting for LocoA
      cdsB[0] = locoBadr;  // CDS_B waiting for LocoB    
    #else
      targetA = CDS_B;
      targetB = CDS_A;
      cdsA[0] = locoBadr;
      cdsB[0] = locoAadr;      
    #endif
    
    Serial.print( "#" );
    Serial.print( loco_d[ locoAadr ] [0] );
    Serial.print( " =" );
    Serial.print( loco_d[ locoAadr ] [13] );
    Serial.print( " speed ->" );
    Serial.println( loco_d[ locoAadr ] [15] );
    Serial.print( "#" );
    Serial.print( loco_d[ locoBadr ] [0] );
    Serial.print( " =" );
    Serial.print( loco_d[ locoBadr ] [13] );
    Serial.print( " speed ->" );
    Serial.println( loco_d[ locoBadr ] [15] );
    Serial.println( "start." );

// Set system address
   loco_d[ locoAadr ] [13] = ADDR_DCC + loco_d[ locoAadr ] [13];
   loco_d[ locoAadr ] [14] = loco_d[ locoAadr ] [15] / SPEED_CONT;
   loco_d[ locoBadr ] [13] = ADDR_DCC + loco_d[ locoBadr ] [13];
   loco_d[ locoBadr ] [14] = loco_d[ locoBadr ] [15] / SPEED_CONT;

    Serial.print( "LocoA = #" );
    Serial.print( loco_d[ locoAadr ] [0] );
    Serial.print( " sysytem adr =" );
    Serial.println( loco_d[ locoAadr ] [13] );
    Serial.print( "LocoB = #" );
    Serial.print( loco_d[ locoBadr ] [0] );
    Serial.print( " sysytem adr =" );
    Serial.println( loco_d[ locoBadr ] [13] );

//  Mute ON/OFF
    delay( CMD_DELAY );    // it need!
    Gateway.SetLocoFunction( loco_d[ locoAadr ][13], 8, MUTE );
    delay( CMD_DELAY );
    Gateway.SetLocoFunction( loco_d[ locoBadr ][13], 8, MUTE );
    
    Serial.println( "Function initialize" );  
    set_speed( 0 );
    set_direction( Forward );
    set_light( OFF );
  
//  Gateway.SetLocoFunction( ( ADDR_DCC +56 ), 1, ON );  //  for Roco 36-156 only
  
}


//  ******************* loop ***********************

void loop()
{

  cdsA[1] = OFF;
  cdsB[1] = OFF;
    
  Serial.println( "loop" );
  randomSeed( analogRead( CDS_A ) );
  set_light( ON );
  set_whistle( 1000 );

  Serial.print( "LocoA = #" );
  Serial.print( loco_d[ locoAadr ] [0] );
  Serial.print( " targetA =" );
  Serial.println( targetA );
  Serial.print( "LocoB = #" );
  Serial.print( loco_d[ locoBadr ] [0] );
  Serial.print( " targetB =" );
  Serial.println( targetB );

  Serial.print( "cdsA = " );
  Serial.println( cdsA[1] );
  Serial.print( "cdsB = " );
  Serial.println( cdsB[1] );

//  Speed up
  Serial.println( "Speed up" );
  tmp = 1;
  do{
      loco_d[ locoAadr ] [16] = tmp * loco_d[ locoAadr ] [14];
      if( loco_d[ locoAadr ] [16] > loco_d[ locoAadr ] [15] )
          loco_d[ locoAadr ] [16] = loco_d[ locoAadr ] [15];
      Serial.print( "#" );            
      Serial.print( loco_d[ locoAadr ] [0] );
      Serial.print( "=" );
      Serial.print( loco_d[ locoAadr ] [16] );
      Serial.print( ", " );
      Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [16] );
      delay( 100 );
      loco_d[ locoBadr ] [16] = tmp * loco_d[ locoBadr ] [14];
      if( loco_d[ locoBadr ] [16] > loco_d[ locoBadr ] [15] )
          loco_d[ locoBadr ] [16] = loco_d[ locoBadr ] [15];
      Serial.print( "#" );
      Serial.print( loco_d[ locoBadr ] [0] );
      Serial.print( "=" );
      Serial.print( loco_d[ locoBadr ] [16] );
      Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
      delay( 100 );
      Serial.println( "" );      
      delay( SPEED_UP );
      tmp++;
    }while( tmp < SPEED_CONT +1 );
  
//  Max Speed
  Serial.println( "Max Speed" );
  set_s_whistle( 1000 );

//  Continue
  Serial.println( "Continue" );
  while( true )
  {
    while( analogRead( CDS_A ) > LIGHT_LEVEL_A && analogRead( CDS_B ) > LIGHT_LEVEL_B )
    {
      if( random( SOMETIME ) == 1 )  //  It happens sometime
      {
          tmp = ( random( SOMETIME ) % 12 ) + 1;
          if( (tmp % 2) == 0 )
          {
            if( tmp != 8 )    // Except Mute
            {
              Serial.print( "LocoA_Func =" );
              Serial.println( tmp );
              Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], tmp, ON );
              delay( 500 );
              Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], tmp, OFF );
            }
          }
          else
          {
            if( tmp != 8 )    // Except Mute
            {
              Serial.print( "LocoB_Func =" );
              Serial.println( tmp );
              Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], tmp, ON );
              delay( 500 );
              Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], tmp, OFF );
            }
          }
      }
    }

    delay( 200 );  // just a moment
    if( analogRead( CDS_A ) <= LIGHT_LEVEL_A )  // Is it true?
    {
        cdsA[1] = ON;
        Serial.println( "Catch CDS_A" );
        break;
    }
    if( analogRead( CDS_B ) <= LIGHT_LEVEL_B )  // Is it true?
    {
        cdsB[1] = ON;
        Serial.println( "Catch CDS_B" );
        break;
    }
  }  //  while( true )
  Serial.print( "cdsA = " );
  Serial.println( cdsA[1] );
  Serial.print( "cdsB = " );
  Serial.println( cdsB[1] );


//  Speed down
  Serial.println( "Speed down" );
  do{
        if( cdsA[1] == ON )
        {
          loco_d[ cdsA[0] ] [16] = loco_d[ cdsA[0] ] [16] - loco_d[ cdsA[0] ] [14];
          if( loco_d[ cdsA[0] ] [16] < 0 )
              loco_d[ cdsA[0] ] [16] = 0;
          Serial.print( " #" );              
          Serial.print( loco_d[ cdsA[0] ] [0] );            
          Serial.print( " = " );           
          Serial.println( loco_d[ cdsA[0] ] [16] );
          Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [16] );
          delay( 100 );
          if( cdsB[1] == OFF )  // Check another CDS
          {
            if( analogRead( CDS_B ) <= LIGHT_LEVEL_B )
            {
              cdsB[1] = 2;
              Serial.println( "Catch CDS_B" );
            }
          }
          if( cdsB[1] == 2 )
          {
            loco_d[ cdsB[0] ] [16] = loco_d[ cdsB[0] ] [16] - loco_d[ cdsB[0] ] [14];
            if( loco_d[ cdsB[0] ] [16] < 0 )
              loco_d[ cdsB[0] ] [16] = 0;
            Serial.print( " #" );              
            Serial.print( loco_d[ cdsB[0] ] [0] );           
            Serial.print( " = " );           
            Serial.println( loco_d[ cdsB[0] ] [16] );
            Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [16] );
            delay( CMD_DELAY );
           }        
        }
        else    //  cdsB[1] = ON
        {
        loco_d[ cdsB[0] ] [16] = loco_d[ cdsB[0] ] [16] - loco_d[ cdsB[0] ] [14];
        if( loco_d[ cdsB[0] ] [16] < 0 )
            loco_d[ cdsB[0] ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ cdsB[0] ] [0] );            
        Serial.print( " = " );          
        Serial.println( loco_d[ cdsB[0] ] [16] );
        Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [16] );
        delay( 100 );
          if( cdsA[1] == OFF )  // Check another CDS
          {
            if( analogRead( CDS_A ) <= LIGHT_LEVEL_A )
            {
              cdsA[1] = 2;
              Serial.println( "Catch CDS_A" );
            }
          }
          if( cdsA[1] == 2 )
          {
            loco_d[ cdsA[0] ] [16] = loco_d[ cdsA[0] ][16] - loco_d[ cdsA[0] ] [14];
            if( loco_d[ cdsA[0] ] [16] < 0 )
              loco_d[ cdsA[0] ] [16] = 0;
            Serial.print( " #" );              
            Serial.print( loco_d[ cdsA[0] ] [0] );         
            Serial.print( " = " );          
            Serial.println( loco_d[ cdsA[0] ] [16] );
            Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [16] );
            delay( CMD_DELAY );
          }        
        }
        
        delay( SPEED_DOWN );
      
    }while( loco_d[ locoAadr ] [16] > 0 && loco_d[ locoBadr ] [16] > 0 );

  if( loco_d[ locoAadr ] [16] == 0 )
  {
      Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [4], ON );
      delay( 1000 );
      Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [4], OFF );
  }
  if( loco_d[ locoBadr ] [16] == 0 )
  {
      Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoAadr ] [4], ON );
      delay( 1000 );
      Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoAadr ] [4], OFF );
  }
  
  if( cdsA[1] == 2 )
    cdsA[1] = ON;
  if( cdsB[1] == 2 )
    cdsB[1] = ON;   


//  Speed down continue
  Serial.println( "Speed down continue" );
  Serial.print( "cdsA = " );
  Serial.println( cdsA[1] );
  Serial.print( "cdsB = " );
  Serial.println( cdsB[1] );
   
  if( cdsA[1] == ON && loco_d[ cdsA[0] ] [16] > 0 )
  {
    while( loco_d[ cdsA[0] ] [16] > 0 )
    {
        loco_d[ cdsA[0] ] [16] = loco_d[ cdsA[0] ] [16] - loco_d[ cdsA[0] ] [14];
        if( loco_d[ cdsA[0] ] [16] < 0 )
          break;
        Serial.print( " #" );              
        Serial.print( loco_d[ cdsA[0] ] [0] );            
        Serial.print( " = " );           
        Serial.println( loco_d[ cdsA[0] ] [16] );
        Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], 0 );
    Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [4], OFF );
  }
  
  if( cdsB[1] == ON && loco_d[ cdsB[0] ] [16] > 0 )
  {
    while( loco_d[ cdsB[0] ] [16] > 0 )
    {
        loco_d[ cdsB[0] ] [16] = loco_d[ cdsB[0] ] [16] - loco_d[ cdsB[0] ] [14];
        if( loco_d[ cdsB[0] ] [16] < 0 )
          break;
        Serial.print( " #" );              
        Serial.print( loco_d[ cdsB[0] ] [0] );            
        Serial.print( " = " );          
        Serial.println( loco_d[ cdsB[0] ] [16] );
        Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], 0 );
    Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [4], OFF );
  }

//  Last Continue
  Serial.println( "Last Continue" );
  if( cdsA[1] != ON )
  {
    while( analogRead( CDS_A ) > LIGHT_LEVEL_A )
    { 
      if( random( SOMETIME ) == 1 )  //  It happens sometime
      {
          tmp = ( random( SOMETIME ) % 12 ) + 1;
          if ( tmp != 8 )
            {
              Serial.print( "LocoA_Func A =" );
              Serial.println( tmp );
              Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], tmp, ON );
              delay( 500 );
              Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], tmp, OFF );
            }
      }
    }
    cdsA[1] = ON;
    Serial.println( "Catch CDS_A" );
    while( loco_d[ cdsA[0] ] [16] > 0 )
    {
        loco_d[ cdsA[0] ] [16] = loco_d[ cdsA[0] ] [16] - loco_d[ cdsA[0] ] [14];
        if( loco_d[ cdsA[0] ] [16] < 0 )
          loco_d[ cdsA[0] ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ cdsA[0] ] [0] );            
        Serial.print( " = " );         
        Serial.println( loco_d[ cdsA[0] ] [16] );
        Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ cdsA[0] ] [13], 0 );    //  Last STOP
    Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ cdsA[0] ] [13], loco_d[ cdsA[0] ] [4], OFF );
  }  //  if( cdsA[1] != ON )

  if( cdsB[1] != ON )
  {
    while( analogRead( CDS_B ) > LIGHT_LEVEL_B )
    { 
      if( random( SOMETIME ) == 1 )  //  It happens sometime
      {
          tmp = ( random( SOMETIME ) % 12 ) + 1;
          if ( tmp != 8 )
            {
              Serial.print( "Loco_Func B =" );
              Serial.println( tmp );
              Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], tmp, ON );
              delay( 500 );
              Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], tmp, OFF );
            }
      }
    }
    cdsB[1] = ON;
    Serial.println( "Catch CDS_B" ); 
    while( loco_d[ cdsB[0] ] [16] > 0 )
    {
        loco_d[ cdsB[0] ] [16] = loco_d[ cdsB[0] ] [16] - loco_d[ cdsB[0] ] [14];
        if( loco_d[ cdsB[0] ] [16] < 0 )
          loco_d[ cdsB[0] ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ cdsB[0] ] [0] );         
        Serial.print( " = " );         
        Serial.println( loco_d[ cdsB[0] ] [16] );
        Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ cdsB[0] ] [13], 0 );    //  Last STOP
    Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ cdsB[0] ] [13], loco_d[ cdsB[0] ] [4], OFF );
  }  //  if( cdsB[1] != ON )
  
  Serial.println( "STOP" );
  Serial.print( "cdsA = " );
  Serial.println( cdsA[1] );
  Serial.print( "cdsB = " );
  Serial.println( cdsB[1] );

//  STOP
  set_speed( 0 );
  set_bell( 2000 );
  set_light( OFF );
  delay( STOP_TIME );

//  Change target CDS
  if( targetA == CDS_A )
  {
    targetA = CDS_B;
    targetB = CDS_A;
    cdsA[0] = locoBadr;
    cdsB[0] = locoAadr;  
  }
  else
  {
    targetA = CDS_A;
    targetB = CDS_B;
    cdsA[0] = locoAadr;
    cdsB[0] = locoBadr;  
  } 

}  //  loop