File content as of revision 6:6cb13ac483e0:

#include "mbed.h"
#include "AccCar.h"
#include "mqtt.h"
#include <stdlib.h>
#include <algorithm>

// ACC macros
#define TICK 1000
#define MONITOR_DIST 17
#define SAFETY_GAP 2

//#define DEBUG_ACC
#define ACC_TAG "[ACC] "

// speed for a legal stop
#define STOPPED_SPEED 0

AccCar::AccCar(int id, Road* road, int flag, mqtt* singleton)
{
    this->car_id = id;
    this->road = road;
    this->flag = flag;
    this->lead = false;
    this->thread = NULL;
    this->state = NORMAL_STATE;
    this->waited = false;
    this->cycles = 0;
    this->position = 0;
    this->target_speed = 0;
    this->speed = 0;
    this->wait_time = 0;
    this->msg = new position_msg_t;
    this->singleton = singleton;
}

void AccCar::make_position_msg(position_msg_t* msg)
{
    assert(msg != NULL);
    msg->position = this->position;
    msg->speed = this->speed;
    msg->car_id = this->car_id;
    msg->road_id = this->road->get_road_id(); 
    msg->counter = this->road->get_road_clock();
    //msg->state = this->state; 
}

// if a null pointer is sent this means no car is in front and it is the lead car.
// this makes the update section slightly different
void AccCar::set_forward_car(AccCar* car)
{
    if(car == NULL) {
        lead = true;
    } else {
        this->forward_car = car;
        lead = false;
    }
}

// update your speed based on forward car
// unless you are lead car then just mozy on
void AccCar::update_speed()
{
    if( !lead )
    {
        if (forward_car->position - position < MONITOR_DIST) {
            road->done_flags.wait_all(forward_car->flag, osWaitForever, false);

            int diff = forward_car->position - position;
            int maxSafeSpeed = diff - SAFETY_GAP;
            speed = std::min(maxSafeSpeed, target_speed);
            assert(speed >= 0);
        }   else {
            speed = target_speed;
        }
    }
    else{
        speed = target_speed;    
    }
}

// gets new speed from 5-15
void AccCar::get_new_target_speed()
{
    int temp_speed = 0;
    // get a valid speed 5-15
    temp_speed = rand() %11;
    temp_speed +=5;
    
    this->target_speed = temp_speed;
}

// update FSM 
void AccCar::update()
{
    while (true) 
    {
        ThisThread::sleep_for(TICK);
        road->go_flags.wait_all(flag);
        cycles++;
#ifndef PUBLISH_ONLY 
        if(cycles > 1)
        {
            // get control msg  
            control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
            if(c_msg == NULL)
            {
                // test if we actually have a large delay
                assert(c_msg != NULL);
                //speed = enter_safety_mode();
            }
            else{
            //
            // do something with the control message
                assert(c_msg->car_id == car_id );
                speed = c_msg->speed;
            
                //
                if(speed == 0)
                {
                    state = STOPPED_STATE;    
                }
                else
                {
                    state = NORMAL_STATE;   
                }
                
                //  
                delete c_msg;
            }
        }
#endif

        position = position + speed;
        
        // ensure correctness 
        assert(position <= this->forward_car->position - 2);
        
        
        if( (cycles % 5) == 0)
        {
            get_new_target_speed();    
        }
        
        // update position and check intersections
        if( state == NORMAL_STATE ) 
        {
            drive_normal();
        }
        else if( state == STOPPED_STATE )
        {
            wait_time++;    
        }
        else
        {   
            // case should not happen
            assert(0);
        }
        
//#ifdef DEBUG_ACC
//        printf(ACC_TAG "sending position to server DELIM");
//#endif
        
//#ifdef DEBUG_ACC
//        printf(ACC_TAG "car %d done DELIM",car_id);
//#endif

        // make a message
        make_position_msg(msg);
        
        // stuff a message in the queue
        this->singleton->add_to_position_queue(msg);
        
//#ifndef PUBLISH_ONLY       
//        //
//        // get_control message may need to malloc the message or something
//        control_msg_t* c_msg = this->singleton->get_control_msg(this->car_id);
//        
//        //
//        // do something with the control message
//        assert(c_msg->car_id == car_id );
//        speed = c_msg->speed;
//    
//        //
//        // delete the control message
//        delete c_msg;
//#endif
        // set flag for completion of cycle
        road->done_flags.set(this->flag);
    }
}

void AccCar::reset(int speed)
{
    road->done_flags.clear(flag);

    if (thread != NULL) {
        thread->terminate();
    }


    thread = new Thread();
    assert(thread != NULL);
    thread->start( callback(this, &AccCar::update) );

    this->position = 0;
    this->speed = speed;
    this->target_speed = speed;
}

void AccCar::stop()
{
    if (thread != NULL) {
        thread->terminate();
    }
    delete thread;
    delete msg;
}


// check if we are going to go through an intersection
// and stop or update speed accordingly
void AccCar::drive_normal()
{
    assert( state == NORMAL_STATE );
    // first update our speed
    if( !lead ) 
    {
        update_speed();
    } else 
    {
        speed = target_speed;
    }
}

int AccCar::get_car_id()
{
    return car_id;
}

int AccCar::get_cycles()
{
    return cycles;
}