/*
 * cds_2_loco_1_01
 * CDS x2 loco x1 forward
 *
 * 2015.6.7  suzu
 *
 */

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

#define ON   1
#define OFF  0
#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 STOP_TIME 3000    // station stop time m-second
#define CMD_DELAY 100  // command delay 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 9999  // Sometime happen
#define HMT  10    // ***How many trains on DB loco_d[][]
#define MUTE  OFF  // Sound mute ON/OFF

DSGatewayLib Gateway;

int  loco_d[ HMT ][17]= {
{ 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, 400, 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 },
{ 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, 41, 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
int  tmp;

//  define functions
void  any_speed( int arg )
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
    Gateway.SetLocoSpeed( loco_d[i] [13], arg );
    delay( CMD_DELAY );
  }
}

void  any_direction( int arg )
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
    Gateway.SetLocoDirection( loco_d[i] [13], arg );
    delay( CMD_DELAY );
  }
}

void  any_function( unsigned char arg1, int arg2 )  // arg = time m-sec
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][arg1], ON );
      delay( CMD_DELAY );
  }
  delay( arg2 ); 
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][arg1], OFF );
      delay( CMD_DELAY );
  }
}

void light( unsigned char on_off )
{
    for ( int i = 0 ; i <= (HMT -1) ; i++ )
    {
      Gateway.SetLocoFunction( loco_d[i] [13], 0, on_off );
      delay( CMD_DELAY );
    }
}

void bell( int arg )   // arg = time m-sec
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][1], ON );
      delay( CMD_DELAY );
  }
  delay( arg );
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][1], OFF );
      delay( CMD_DELAY );
  }
}

void whistle( int arg )    // arg = time m-sec
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][2], ON );
      delay( CMD_DELAY );
  }
  delay( arg );  
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][2], OFF );
      delay( CMD_DELAY );
  }
}

void s_whistle( int arg )    // arg = time m-sec
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][3], ON );
      delay( CMD_DELAY );
  }
  delay( arg ); 
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][3], OFF );
      delay( CMD_DELAY );
  }
}

void steam( int arg )    // arg = time m-sec
{
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][4], ON );
      delay( CMD_DELAY );
  }
  delay( arg ); 
  for ( int i = 0 ; i <= (HMT -1) ; i++ )
  {
      Gateway.SetLocoFunction( loco_d[i] [13], loco_d[i][4], OFF );
      delay( CMD_DELAY );
  }
}

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


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

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

    Serial.println( "Function initialize" );  
    any_speed( 0 );
    any_direction( Forward );
    light( OFF );

    for ( int i = 0 ; i <= (HMT -1) ; i++ )
    {
      Serial.print( "#" );
      Serial.print( loco_d[i] [0]  );
      Serial.print( ", " );
     }
     Serial.println( "start." );

   for ( int i = 0 ; i <= (HMT -1) ; i++ )
   {
     loco_d[i] [13] = ADDR_DCC + loco_d[i] [13];  // set system address
     loco_d[i] [14] = loco_d[i] [15] / SPEED_CONT;
   }

//  Mute ON/OFF
   for ( int i = 0 ; i <= (HMT -1) ; i++ )
   {
      Gateway.SetLocoFunction( loco_d[i] [13], 8, MUTE );
      delay( CMD_DELAY );
    }

//    Gateway.SetLocoFunction( ( ADDR_DCC +56 ), 1, ON );  //  for Roco 36-156 only

}


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

void loop()
{
  Serial.println( "loop" );
  randomSeed( analogRead( CDS_A ) );
  light( ON );
  whistle( 1000 );
  
  for( int i = 0 ; i <= (HMT -1) ; i++ )
      loco_d[i] [16] = 0;     // All Loco Speed 0

//  Speed up
  Serial.println( "Speed up" );
  tmp = 0;
  do{
      for( int i = 0 ; i <= (HMT -1) ; i++ )
      {
        loco_d[i] [16] = loco_d[i] [16] + loco_d[i] [14];
        if( loco_d[i] [16] > loco_d[i] [15] )
            loco_d[i] [16] = loco_d[i] [15];
        Serial.print( loco_d[i] [16] );
        Serial.print( " " );  
        Gateway.SetLocoSpeed( loco_d[i] [13], loco_d[i] [16] );
        delay( 100 );
      }
      Serial.println( "" );      
      delay( SPEED_UP );
      tmp++;
    }while( tmp < SPEED_CONT );
  
//  Max Speed
  Serial.println( "Max Speed" );
  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 != 8 )    // except Mute
          {
            Serial.print( "Loco_Func =" );
            Serial.println( tmp );
            any_function( tmp, 500 );
          }
      }
    }

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

//  Speed down
  Serial.println( "Speed down" );
  tmp = 0;
  do{
      for( int i = 0 ; i <= (HMT -1) ; i++ )
      {
        loco_d[i] [16] = loco_d[i] [16] - loco_d[i] [14];
        if( loco_d[i] [16] < 0 )
            loco_d[i] [16] = 0;
        Serial.print( loco_d[i] [16] );
        Serial.print( " " );
        Gateway.SetLocoSpeed( loco_d[i] [13], loco_d[i] [16] );
        delay( 100 );
      }
      Serial.println( "" );      
      delay( SPEED_DOWN );
      tmp++;
  }while( tmp < SPEED_CONT );

//  STOP
  Serial.println( "STOP" );
  steam( 500 );
  any_speed( 0 );
  bell( 2000 );
  light( OFF );
  delay( STOP_TIME );

}