/*
 * xchg_loco_2_02
 * exchange 2 locos forward, 4 CDS stop
 *
 * 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=Normail 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 LIGHT_LEVEL 200  // CDS Light Level
#define CDS_0 0    // CDS_0 analog port#0 (Don't change)
#define CDS_1 1    // CDS_1 analog port#1 (Don't change)
#define CDS_2 2    // CDS_2 analog port#2 (Don't change)
#define CDS_3 3    // CDS_3 analog port#3 (Don't change)
#define POINT_A  7  // point DCC Address
#define POINT_B  6  // point DCC Address
#define POINT_C  7  // point DCC Address
#define POINT_D  6  // point DCC Address
#define POINT_E  8  // point DCC Address  
#define SOMETIME 99999  // Sometime happen
#define HMT  12    // ***How many trains on DB loco_d[ HMT ][]
#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 },  // Dummy (Don't delete)
{ 3, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 3, 0, 500, 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, 550, 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, 800, 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
unsigned char cds[4];  // cds[port#0-3] = CDS ON/OFF
int  locoAadr, locoBadr, 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 );
}

void  point( int point, boolean straight )
{
    if( straight == true )
    {
      Gateway.SetTurnout( ADDR_ACC_DCC + point, false );
      delay( 5000 );
      Gateway.SetTurnout( ADDR_ACC_DCC + point, true );
    }
    
    if( straight == false )
    {
      Gateway.SetTurnout( ADDR_ACC_DCC + point, true );
      delay( 5000 );
      Gateway.SetTurnout( ADDR_ACC_DCC + point, false );
    }
}


//  ******************* 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_0;  // LocoA initial target CDSport#=0
      targetB = CDS_1;  // LocoB initial target CDSport#=1
    #else
      targetA = CDS_3;  // LocoA initial target CDSport#=3
      targetB = CDS_2;  // LocoB initial target CDSport#=2
    #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
   for ( int i = 0 ; i <= (HMT -1) ; i++ )
   {
       loco_d[i] [13] = ADDR_DCC + loco_d[i] [13];    // set system data
       loco_d[i] [14] = loco_d[i] [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 );

//  Point Set
//  Serial.println( "Point 8 = false" ); 
//  point( 8, false );
    #if  SETTING
      Serial.println( "POINT_B = false" );   //  true = outside, false = inside
      point( POINT_B, false );
      Serial.println( "POINT_A = true" );   //  true = outside, false = inside
      point( POINT_A, true );
    #else
      Serial.println( "POINT_B = true" );   //  true = outside, false = inside
      point( POINT_B, true );
      Serial.println( "POINT_A = false" );   //  true = outside, false = inside
      point( POINT_A, false );
    #endif
    
    delay( 2000 );
    
//  Gateway.SetLocoFunction( ( ADDR_DCC +56 ), 1, ON );  //  for Roco 36-156 only

}


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

void loop()
{
    Serial.println( "loop" );

    randomSeed( analogRead( CDS_0 ) );
    set_light( ON );
    set_whistle( 1000 );
    cds[0] = OFF;  // Not yet catched
    cds[1] = OFF;  // Not yet catched
    cds[2] = OFF;  // Not yet catched
    cds[3] = OFF;  // Not yet catched 
    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( "cds = " );
    for( int i = 0 ; i <= 3 ; i++ )
    {
      Serial.print( cds[i] );
      Serial.print( ", " );
    }
    Serial.println( "" );

//  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.println( loco_d[ locoBadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
        delay( 100 );   
        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( targetA ) > LIGHT_LEVEL && analogRead( targetB ) > LIGHT_LEVEL )
    {
      if( random( SOMETIME ) == 1 )  //  It happens sometime
      {
          tmp = ( random( SOMETIME ) % 12 ) + 1;
          if( (tmp % 2) == 0 )
          {
            if( tmp != 8 )
            {
              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 )
            {
              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( targetA ) <= LIGHT_LEVEL )  // Is it true?
    {
      cds[ targetA ] = ON;
      Serial.print( "Catch port = " );
      Serial.println( targetA );
      Serial.print( "cds = " );
      for( int i = 0 ; i <= 3 ; i++ )
      {
        Serial.print( cds[i] );
        Serial.print( ", " );
      }
      Serial.println( "" );
      break;
    }
    if( analogRead( targetB ) <= LIGHT_LEVEL )  // Is it true?
    {
      cds[ targetB ] = ON;
      Serial.print( "Catch port = " );
      Serial.println( targetB );
      Serial.print( "cds = " );
      for( int i = 0 ; i <= 3 ; i++ )
      {
        Serial.print( cds[i] );
        Serial.print( ", " );
      }
      Serial.println( "" );
      break;
    }
  }  // while(true)

  Serial.print( " cds = " );  
    for( int i = 0 ; i <= 3 ; i++ )
    {
      Serial.print( cds[i] );
      Serial.print( ", " );
    }
    Serial.println( "" );

//  Speed down
  Serial.println( "Speed down" );
  do{
        if( cds[ targetA ] == ON )
        {
          loco_d[ locoAadr ] [16] = loco_d[ locoAadr ] [16] - loco_d[ locoAadr ] [14];
          if( loco_d[ locoAadr ] [16] < 0 )
              loco_d[ locoAadr ] [16] = 0;
          Serial.print( " #" );              
          Serial.print( loco_d[ locoAadr ] [0] );            
          Serial.print( " = " );           
          Serial.println( loco_d[ locoAadr ] [16] );
          Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [16] );
          delay( 100 );
          if( cds[ targetB ] == OFF )  // Check another CDS
          {
            if( analogRead( targetB ) <= LIGHT_LEVEL )
            {
              cds[ targetB ] = 2;
              Serial.print( "Catch port = " );
              Serial.println( targetB );
              Serial.print( "cds = " );
              for( int i = 0 ; i <= 3 ; i++ )
              {
                Serial.print( cds[i] );
                Serial.print( ", " );
              }
              Serial.println( "" );
            }
          }
          
          if( cds[ targetB ] == 2 )
          {
            loco_d[ locoBadr ] [16] = loco_d[ locoBadr ] [16] - loco_d[ locoBadr ] [14];
            if( loco_d[ locoBadr ] [16] < 0 )
              loco_d[ locoBadr ] [16] = 0;
            Serial.print( " #" );              
            Serial.print( loco_d[ locoBadr ] [0] );           
            Serial.print( " = " );           
            Serial.println( loco_d[ locoBadr ] [16] );
            Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
            delay( CMD_DELAY );
           }        
        }
        
        else    //  cds[ targetB ] == ON 
        {
        loco_d[ locoBadr ] [16] = loco_d[ locoBadr ] [16] - loco_d[ locoBadr ] [14];
        if( loco_d[ locoBadr ] [16] < 0 )
            loco_d[ locoBadr ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ locoBadr ] [0] );            
        Serial.print( " = " );          
        Serial.println( loco_d[ locoBadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
        delay( 100 );
        if( cds[ targetA ] == OFF )  // Check another CDS
        {
            if( analogRead( targetA ) <= LIGHT_LEVEL )
            {
              cds[ targetA ] = 2;
              Serial.print( "Catch port = " );
              Serial.println( targetA );
              Serial.print( "cds = " );
              for( int i = 0 ; i <= 3 ; i++ )
              {
                Serial.print( cds[i] );
                Serial.print( ", " );
              }
              Serial.println( "" );
            }
          }
          if( cds[ targetA ] == 2 )
          {
            loco_d[ targetA ] [16] = loco_d[ targetA ][16] - loco_d[ targetA ] [14];
            if( loco_d[ targetA ] [16] < 0 )
              loco_d[ targetA ] [16] = 0;
            Serial.print( " #" );              
            Serial.print( loco_d[ targetA ] [0] );         
            Serial.print( " = " );          
            Serial.println( loco_d[ targetA ] [16] );
            Gateway.SetLocoSpeed( loco_d[ targetA ] [13], loco_d[ targetA ] [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( cds[ targetA ] == 2 )
    cds[ targetA ] = ON;
  if( cds[ targetB ] == 2 )
    cds[ targetB ] = ON;   

//  Speed down continue
  Serial.println( "Speed down continue" );
  Serial.print( "cds = " );
  for( int i = 0 ; i <= 3 ; i++ )
  {
    Serial.print( cds[i] );
    Serial.print( ", " );
  }
  Serial.println( "" );
   
  if( cds[ targetA ] == ON && loco_d[ locoAadr ] [16] > 0 )
  {
    while( loco_d[ locoAadr ] [16] > 0 )
    {
        loco_d[ locoAadr ] [16] = loco_d[ locoAadr ] [16] - loco_d[ locoAadr ] [14];
        if( loco_d[ locoAadr ] [16] < 0 )
          break;
        Serial.print( " #" );              
        Serial.print( loco_d[ locoAadr ] [0] );            
        Serial.print( " = " );           
        Serial.println( loco_d[ locoAadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], 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( cds[ targetB ] == ON && loco_d[ locoBadr ] [16] > 0 )
  {
    while( loco_d[ locoBadr ] [16] > 0 )
    {
        loco_d[ locoBadr ] [16] = loco_d[ locoBadr ] [16] - loco_d[ locoBadr ] [14];
        if( loco_d[ locoBadr ] [16] < 0 )
          break;
        Serial.print( " #" );              
        Serial.print( loco_d[ locoBadr ] [0] );            
        Serial.print( " = " );          
        Serial.println( loco_d[ locoBadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], 0 );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], OFF );
  }  //  if( cds[ targetB ] == ON && loco_d[ locoBadr ] [16] > 0 )

//  Last Continue
  Serial.println( "Last Continue" );
  if( cds[ targetA ] != ON )
  {
    while( analogRead( targetA ) > LIGHT_LEVEL )
    { 
      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[ locoAadr ] [13], tmp, ON );
            delay( 500 );
            Gateway.SetLocoFunction( loco_d[ locoAadr ] [13], tmp, OFF );
          }
      }
    }
    cds[ targetA ] = ON;
    Serial.print( "Catch port = " );
    Serial.println( targetA );
    Serial.print( "cds = " );
    for( int i = 0 ; i <= 3 ; i++ )
    {
      Serial.print( cds[i] );
      Serial.print( ", " );
    }
    Serial.println( "" );
    while( loco_d[ locoAadr ] [16] > 0 )
    {
        loco_d[ locoAadr ] [16] = loco_d[ locoAadr ] [16] - loco_d[ locoAadr ] [14];
        if( loco_d[ locoAadr ] [16] < 0 )
          loco_d[ locoAadr ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ locoAadr ] [0] );            
        Serial.print( " = " );         
        Serial.println( loco_d[ locoAadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], loco_d[ locoAadr ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ locoAadr ] [13], 0 );    //  Last STOP locoAadr
    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( cds[ targetA ] != ON )

  if( cds[ targetB ] != ON )
  {
    while( analogRead( targetB ) > LIGHT_LEVEL )
    {
      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[ locoBadr ] [13], tmp, ON );
            delay( 500 );
            Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], tmp, OFF );
          }
      }
    }
    cds[ targetB ] = ON;
    Serial.print( "Catch port = " );
    Serial.println( targetA );
    Serial.print( "cds = " );
    for( int i = 0 ; i <= 3 ; i++ )
    {
      Serial.print( cds[i] );
      Serial.print( ", " );
    }
    Serial.println( "" );
    while( loco_d[ locoBadr ] [16] > 0 )
    {
        loco_d[ locoBadr ] [16] = loco_d[ locoBadr ] [16] - loco_d[ locoBadr ] [14];
        if( loco_d[ locoBadr ] [16] < 0 )
          loco_d[ locoBadr ] [16] = 0;
        Serial.print( " #" );              
        Serial.print( loco_d[ locoBadr ] [0] );         
        Serial.print( " = " );         
        Serial.println( loco_d[ locoBadr ] [16] );
        Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [16] );
        delay( CMD_DELAY );       
        delay( SPEED_DOWN );   
    }
    Gateway.SetLocoSpeed( loco_d[ locoBadr ] [13], 0 );    //  Last STOP locoBadr
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], ON );
    delay( 1000 );
    Gateway.SetLocoFunction( loco_d[ locoBadr ] [13], loco_d[ locoBadr ] [4], OFF );
  }  // if( cds[ targetB ] != ON )

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

//  Change target CDS & point
  if( targetA == CDS_0 )
  {
    targetA = CDS_3;
    targetB = CDS_2;
    Serial.println( "POINT_A = false" );   //  false = inside
    point( POINT_A, false );
    Serial.println( "POINT_B = true" );   //  true = outside
    point( POINT_B, true );
  }
  else
  {
    targetA = CDS_0;
    targetB = CDS_1;
    Serial.println( "POINT_A = true" );   //  true = outside
    point( POINT_A, true ); 
    Serial.println( "POINT_B = false" );   //  false = inside
    point( POINT_B, false );
  }
  
  delay( 1000 );

}  // loop