File content as of revision 7:fd8e0604faaa:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include <cctype>
#include "AccCar.h"
#include "Road.h"
#include "mqtt.h"

#define MAGIC_NUMBER 2

//#define SIM_DEBUG
#define SIM_TAG "[SIM] "

Serial pc(USBTX, USBRX);

// prints statistics
static void print_wait_time(Road* road)
{
    int active_cars = road->get_active_cars();
    for( int i = 0; i < active_cars; i++)
    {
            AccCar* car = road->get_car(i);
            assert(car != NULL);
            pc.printf(SIM_TAG "Car %d Road %d wait_time %d"DELIM, i,road->get_road_id(), car->wait_time); 
    }
}

// handles adding a new car safely to the intersection
static void add_new_car(Road* road,int* cycles,mqtt* mqtt_singleton)
{
        // get rand speed
        int temp_speed = rand() %11;
        temp_speed += 5;
            
        // 
        assert( temp_speed >=5 && temp_speed <= 15);
        
        if(road->can_car_enter( temp_speed ))
        {
            *cycles = 0; // we added a car reset our timer
            
            int id = road->get_new_car_id();
            
            AccCar* car = new AccCar(id, road, (1 << id),mqtt_singleton );
            assert(car != NULL);
            
#ifdef SIM_DEBUG
            pc.printf(SIM_TAG "adding new car to road %d car %d"DELIM,road->get_road_id(),id);
#endif

            road->add_acc_car(car);
            
            AccCar* forward_car = road->get_forward_car(id);
            
            // lead car will get a null pointer
            car->set_forward_car( forward_car ); 
            
            // this needs to be a rand number [0,15]
            car->reset(temp_speed);
            
        }
}

void print_positions(Road* road)
{
//#ifdef SIM_DEBUG
            pc.printf(SIM_TAG " -------------- "DELIM);
//#endif  
    int active_cars = road->get_active_cars();
    for( int i = 0; i < active_cars; i++)
    {
            AccCar* car = road->get_car(i);
            assert(car != NULL);
            pc.printf(SIM_TAG "Car %d Road %d position %d speed %d target %d cycles %d car_cycles %d "DELIM, i,road->get_road_id(), car->position,car->speed,car->get_target_speed(),road->get_road_clock(), car->get_cycles()); 
    }
//#ifdef SIM_DEBUG
            pc.printf(SIM_TAG " -------------- "DELIM);
//#endif 
}


int start_simulation(mqtt* mqtt_singleton)
{   
    // ------------------------------------------------------------------------------
    // The following three variables are used for timing statistics, do not modify them
    Timer stopwatch;    // A timer to keep track of how long the updates take, for statistics purposes
    int numberCycles = 0;
    int totalUpdateTime = 0;
    
    // every 3 cycles we need to add a new car
    int new_car_cycles = 0;

    // ------------------------------------------------------------------------------
    // seed rand number generator
    //srand(mqtt_singleton->mqtt_id);
    
    // make new road based off of ID
    Road* road = new Road(mqtt_singleton->mqtt_id, mqtt_singleton);
        
    // add first cars to the road to kick everything off
    add_new_car(road,&new_car_cycles,mqtt_singleton);
    
    stopwatch.start();
    
    stopwatch.reset();
    
    int other_simulating;
    int simulating; 
    
    // 
    //road->synchronize(simulating);
    
    do {
        
        // if we get a random number thats right OR if we hit our max of 3
        // then add a new car
        if( (rand() %3) == MAGIC_NUMBER || new_car_cycles >= 3)
        {
            // add new car to road and then set the forward car to it
            add_new_car(road,&new_car_cycles,mqtt_singleton);  
        }
        
        //
        road->let_cars_update();
        
        //
        road->wait_for_car_update();
    
        // are we still simulating? true if yes
        simulating = road->simulating();

#ifndef PUBLISH_ONLY
        // is the other road still simulating? true if yes
        other_simulating = road->synchronize(simulating);
#endif           

        // ------------------------------------------------------------------
        // Timing statistics logic, do not modify
        totalUpdateTime += stopwatch.read_ms();
        numberCycles++;
        new_car_cycles++;
        stopwatch.reset();
        // ------------------------------------------------------------------
        
        print_positions(road);

#ifndef PUBLISH_ONLY
    // TODO figure out how to end simulation like they want
    } while (other_simulating || simulating); 
#else
    } while (simulating);
#endif
    // ----------------------------------------------------------------------
    // Timing statistics printout, do not modify
    pc.printf(SIM_TAG "Average update cycle took: %fms "DELIM, (totalUpdateTime*1.0)/(numberCycles*1.0));
    totalUpdateTime = 0;
    numberCycles = 0;
    
    // print wait times
    print_wait_time(road);
    
    // ----------------------------------------------------------------------
    road->DESTROY_ALL_CARS();
    road->free_msg();
    delete road;
    return 0;
}