File content as of revision 92:52a91656458a:

#include "SequenceController.hpp"
#include "StaticDefs.hpp"

SequenceController::SequenceController() {
    _sequence_counter = 0;
}

void SequenceController::loadSequence() {
    xbee().printf("\n\rLoading Dive Sequence File:");
    
    ConfigFile read_sequence_cfg;
    char value[256];
    char bux2[256];   
    
    //read configuration file stored on MBED
    if (!read_sequence_cfg.read("/local/sequence.txt")) {
        xbee().printf("\n\rERROR:Failure to read sequence.txt file.");
    }
    else {        
        /* Read values from the file until you reach an "exit" character" */
        //up to 256 items in the sequence
        for (int i = 0; i < 256; i++) {                  //works
            /* convert INT to string */
            char buf[256];
            sprintf(buf, "%d", i);                  //searching for 0,1,2,3...
            /* convert INT to string */
        
            if (read_sequence_cfg.getValue(buf, &value[0], sizeof(value))) {
                xbee().printf("\n\rsequence %d = %s",i,value);
                
                sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring)
            }
            
            if (sequenceStructLoaded[i].title == "exit") {
                _number_of_sequences = i;   //before the exit
                sprintf(bux2,"\n\rin loadsequence(): found final exit line num-sequences=%d\n", i);
                   mbedLogger().appendDiagFile(bux2,0);
                break;
            }
        }
    }   //end of successful read
}

sequenceStruct SequenceController::process(string randomstring) {
    //this is the struct that is loaded from the config variables
    char bux2[256];
    sequenceStruct loadStruct; //local struct
    
    /* CONVERT STRING TO CHAR ARRAY */
    const char *cstr = randomstring.c_str();
    /* CONVERT STRING TO CHAR ARRAY */
    
    /* DIVE */
    //this can only be in the first position
    if ((signed int) randomstring.find("dive") != -1) {
        loadStruct.title = "dive";
        loadStruct.state = MULTI_DIVE;      //NEW: separate state handles multiple dives
    }
    /* DIVE */
    
    /* PITCH */
    if ((signed int) randomstring.find("neutral") != -1) {
        loadStruct.title = "neutral";
        xbee().printf("\n\rLOAD neutral. %d", randomstring.find("neutral"));
        loadStruct.state = FIND_NEUTRAL;
         sprintf(bux2,"\n\rin sequence().process(string): FOUND NEUTRAL in randomstring.find \n");
                   mbedLogger().appendDiagFile(bux2,3);
    }
    /* PITCH */
    
    /* EXIT */
    if ((signed int) randomstring.find("exit") != -1) {
        loadStruct.title = "exit";
        xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
        sprintf(bux2,"\n\rin sequence().process(string): FOUND EXIT in randomstring.find \n");
                   mbedLogger().appendDiagFile(bux2,0);
        loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
    }
    /* EXIT */
    
    /* DEPTH TO FLOAT */
    if ((signed int) randomstring.find("depth") != -1) {
        if (randomstring.find("neutral") || randomstring.find("dive")) {
            int depth_pos = randomstring.find("depth") + 6;     //11 in example literally "depth="
            char depth_array[256] = {0};    //clear memory
            int depth_counter = 0;
            for (int i = depth_pos; i < randomstring.length(); i++) {
                if (cstr[i] == ',')
                    break;
                else if (cstr[i] == ';')
                    break;
                else {
                    depth_array[depth_counter] = cstr[i];
                    depth_counter++;
                }
            }
            loadStruct.depth = atof(depth_array);
        }
    }    
    /* DEPTH TO FLOAT */
    
    /* PITCH TO FLOAT */
    if ((signed int) randomstring.find("pitch") != -1) {
        if (randomstring.find("neutral") || randomstring.find("dive")) {
            int pitch_pos = randomstring.find("pitch") + 6;     //11 in example
            char pitch_array[256] = {0};    //clear memory
            int pitch_counter = 0;
            for (int i = pitch_pos; i < randomstring.length(); i++) {
                if (cstr[i] == ',') 
                    break;
                else if (cstr[i] == ';') 
                    break;
                else {
                    pitch_array[pitch_counter] = cstr[i]; 
                    pitch_counter++;
                }
            }
            loadStruct.pitch = atof(pitch_array);
        }
    }
    /* PITCH TO FLOAT */
    
    /* PAUSE */
    if ((signed int) randomstring.find("pause") != -1) {
        loadStruct.title = "pause";
    }
    /* PAUSE */
    
    /* TIME TO FLOAT */
    if ((signed int) randomstring.find("timeout") != -1) { 
           
        int time_pos = randomstring.find("timeout") + 8;    //position of timeout + "timeout=" so 8
        char time_array[256] = {0};
        int time_counter = 0;
        for (int i = time_pos; i < randomstring.length(); i++) {
            //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
            
            if (cstr[i] == ',') 
                break;
            else if (cstr[i] == ';') 
                break;
            else {
                //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
                time_array[time_counter] = cstr[i]; 
                time_counter++;
            }
        }
        loadStruct.timeout = atof(time_array);
    }
    /* TIME TO FLOAT */  
    
//    /* EXIT */
//    if (randomstring.find("exit") != 0) {
//        loadStruct.title = "exit";
//        xbee().printf("\n\rEXIT.");
//    }
//    /* EXIT */
    
    return loadStruct;  //each iteration of this returns a completed struct
}

void SequenceController::sequenceFunction() {
    //xbee().printf("sequenceFunction\n\r");    //debug (verified it is working correctly)
    
    int check_current_state = stateMachine().getState();
    xbee().printf("State Machine State: %d\n\r", check_current_state);
        
    if (stateMachine().getState() == SIT_IDLE) {
        //system starts idle
        //set the state machine to the current sequence in the array
        //example, set to "dive" and set pitch and depth and timeout
        
        _current_state = sequenceStructLoaded[_sequence_counter].state;
        xbee().printf("_current_state: %d\n\r", _current_state);
        xbee().printf("_sequence_counter: %d\n\r", _sequence_counter);
        xbee().printf("_number_of_sequences: %d\n\r", _number_of_sequences);
        
        stateMachine().setState(_current_state);
        stateMachine().setDepthCommand(sequenceStructLoaded[_sequence_counter].depth);
        stateMachine().setPitchCommand(sequenceStructLoaded[_sequence_counter].pitch);
        stateMachine().setTimeout(sequenceStructLoaded[_sequence_counter].timeout);
    
        if (_sequence_counter == _number_of_sequences-1)    //end when you finish all of the sequences       
            sequenceTicker.detach();
            
        _sequence_counter++;        //exit ticker when counter complete
    }   
}

int LegController::getLegState() {
    return _current_state;
}
LegController::LegController() {
    _leg_counter = 0;
}

int LegController::loadLeg() {
    xbee().printf("\n\rLoading Leg commands Dive File:");
    
    ConfigFile read_leg_cfg;
    char value[256];   
    //   char buf[256];
    char bux2[256];
    static int default_legstruct_loaded = 0;
    sprintf(bux2,"\n\rin loadleg(): Loading Leg commands Dive File:");
    mbedLogger().appendDiagFile(bux2,0);
    
    if(default_legstruct_loaded == 0 ) {
        legStructLoaded[0] = load_def_leg();  // is this right call pointer vs item?
        legStructLoaded[1].title = "exit";
        default_legstruct_loaded = 1;
      }
    //read configuration file stored on MBED
    if (!read_leg_cfg.read("/local/legfile.txt")) {  //legfile.txt has exact same format as sequence.txt file,with starting 0=leg,.... and ending 1=exit line
        xbee().printf("\n\rERROR:Failure to read legfile.txt file.");
        sprintf(bux2,"\n\rERROR:Failure to read legfile.txt file.");
        mbedLogger().appendDiagFile(bux2,3);
        
    }
    else {        
        /* Read values from the file until you reach an "exit" character" */
        //up to 256 items in the sequence
        for (int i = 0; i < 256; i++) {                  //works
            /* convert INT to string */
            char buf[256];
            sprintf(buf, "%d", i);                  //searching for 0,1,2,3...
            /* convert INT to string */
        
            if (read_leg_cfg.getValue(buf, &value[0], sizeof(value))) {
                xbee().printf("\n\rsequence %d = %s",i,value);
                sprintf(bux2, "\n\r leg values sequence %d = %s\n",i,value);
                mbedLogger().appendDiagFile(bux2,0);
                
                legStructLoaded[i] = process(value); //create the structs using process(string randomstring)
            }
            
            if (legStructLoaded[i].title == "exit") {
                _number_of_legs = i;   //before the exit
                break;
            }
        }
    }   //end of successful read
    if (_number_of_legs  > 0 )  {return 1; }
    else {return 0; }
}

legStruct LegController::load_def_leg() {  // default leg structure and if used, will NOT start leg mode
    legStruct loadStruct;
    
    loadStruct.title = "sit_idle";
    loadStruct.state = SIT_IDLE ;    //    LEG_POSITION_DIVE;
    loadStruct.max_depth = 15;
    loadStruct.min_depth =  5;
    loadStruct.yo_time =  100;
    loadStruct.timeout =  600;
    loadStruct.heading =  90; 
    return loadStruct;
}    
    
legStruct LegController::process(string randomstring) {
    //this is the struct that is loaded from the config variables
    
    legStruct loadStruct; //local struct
    static int callcount=0;
    /* CONVERT STRING TO CHAR ARRAY */
    const char *cstr = randomstring.c_str();
    char bux2[256];
    /* CONVERT STRING TO CHAR ARRAY */
    
    /* leg position DIVing */
    //this can only be in the first position
    
    callcount++;
    if ((signed int) randomstring.find("leg") != -1) {
        loadStruct.title = "leg";
        loadStruct.state = LEG_POSITION_DIVE;      //NEW: separate state handles multiple dives
        sprintf(bux2, "\n\r process leg file: found leg label, setting state to LEG_POSITION_DIVE");
                mbedLogger().appendDiagFile(bux2,3);
    }
    /* LPD */
    
    /* start-swim */
    if ((signed int) randomstring.find("start_swim") != -1) {
        loadStruct.title = "start_swim";
        xbee().printf("\n\rLOAD start-swim %d", randomstring.find("start_swim"));
        loadStruct.state = START_SWIM;
        sprintf(bux2, "\n\r process leg file: found START-SWIM  label, setting state to same\n");
                mbedLogger().appendDiagFile(bux2,3);
    }
    /* start-swim */
    /* flying_idle */
    if ((signed int) randomstring.find("flying_idle") != -1) {
        loadStruct.title = "flying_idle";
        xbee().printf("\n\rLOAD flying-idle found at %d", randomstring.find("flying_idle"));
        loadStruct.state = FLYING_IDLE;
        sprintf(bux2, "\n\r process leg file: found FLYING_IDLE  label, setting state to FLYING_IDLE \n");
                mbedLogger().appendDiagFile(bux2,3);
    }
    /* flying_idle */
    /* EXIT */
    if ((signed int) randomstring.find("exit") != -1) {
        loadStruct.title = "exit";
        xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
        loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
                sprintf(bux2, "\n\r process(valuestring) legfile: found exit key.  Callcount=%d\n", callcount);
                mbedLogger().appendDiagFile(bux2,3);
    }
    /* EXIT */
    
    /*  max DEPTH TO  cycle to */
 
    if ((signed int) randomstring.find("max_depth") != -1) {

        int depth_pos = randomstring.find("max_depth") + 10;     //11 in example literally "depth="
        char depth_array[256] = {0};    //clear memory
        int depth_counter = 0;
        for (int i = depth_pos; i < randomstring.length(); i++) {
            if (cstr[i] == ',')
                break;
            else if (cstr[i] == ';')
                break;
            else {
                depth_array[depth_counter] = cstr[i];
                depth_counter++;
            }
        }
        loadStruct.max_depth = atof(depth_array);
        sprintf(bux2, "\n\r process legfile: key=max_depth val=%g  process(legstring)count=%d \n", atof(depth_array), callcount);
        mbedLogger().appendDiagFile(bux2,3);

    }
    
 
    if ((signed int) randomstring.find("min_depth") != -1) {

        int depth_pos = randomstring.find("min_depth") + 10;     //11 in example literally "depth="
        char depth_array[256] = {0};    //clear memory
        int depth_counter = 0;
        for (int i = depth_pos; i < randomstring.length(); i++) {
            if (cstr[i] == ',')
                break;
            else if (cstr[i] == ';')
                break;
            else {
                depth_array[depth_counter] = cstr[i];
                depth_counter++;
            }
        }
        loadStruct.min_depth = atof(depth_array);
        sprintf(bux2, "\n\r process legfile: key=min_depth val=%g \n", atof(depth_array));
        mbedLogger().appendDiagFile(bux2,3);

    }
    if ((signed int) randomstring.find("heading") != -1) {
        
            int depth_pos = randomstring.find("heading") + 8;     //11 in example literally "depth="
            char depth_array[256] = {0};    //clear memory
            int depth_counter = 0;
            for (int i = depth_pos; i < randomstring.length(); i++) {
                if (cstr[i] == ',')
                    break;
                else if (cstr[i] == ';')
                    break;
                else {
                    depth_array[depth_counter] = cstr[i];
                    depth_counter++;
                }
            }
            loadStruct.heading = atof(depth_array);
            sprintf(bux2, "\n\r process legfile: key=heading val=%g \n", atof(depth_array));
            mbedLogger().appendDiagFile(bux2,3);
        
    }
    /* DEPTH TO FLOAT */
    
    
    
    /* PAUSE */
    if ((signed int) randomstring.find("pause") != -1) {
        loadStruct.title = "pause";
    }
    /* PAUSE */
    
    /* TIME TO maintain leg yo-yo operations */
    if ((signed int) randomstring.find("timeout") != -1) { 
           
        int time_pos = randomstring.find("timeout") + 8;    //position of timeout + "timeout=" so 8
        char time_array[256] = {0};
        int time_counter = 0;
        for (int i = time_pos; i < randomstring.length(); i++) {
            //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
            
            if (cstr[i] == ',') 
                break;
            else if (cstr[i] == ';') 
                break;
            else {
                //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
                time_array[time_counter] = cstr[i]; 
                time_counter++;
            }
        }
        loadStruct.timeout = atof(time_array);
        sprintf(bux2, "\n\r process legfile: key=timeout val=%g \n", atof(time_array));
                mbedLogger().appendDiagFile(bux2,3);
    }
    // yo_time is to avoid a single down/or up segment going on too long - maybe 10 minutes?
    if ((signed int) randomstring.find("yo_time") != -1) { 
           
        int time_pos = randomstring.find("yo_time") + 8;    //position of timeout + "timeout=" so 8
        char time_array[256] = {0};
        int time_counter = 0;
        for (int i = time_pos; i < randomstring.length(); i++) {
            //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
            
            if (cstr[i] == ',') 
                break;
            else if (cstr[i] == ';') 
                break;
            else {
                //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
                time_array[time_counter] = cstr[i]; 
                time_counter++;
            }
        }
        loadStruct.yo_time = atof(time_array);
        sprintf(bux2, "\n\r process legfile: key=yo_time val=%g \n", atof(time_array));
                mbedLogger().appendDiagFile(bux2,3);
    }
    /* TIME TO FLOAT */  
    
//    /* EXIT */
//    if (randomstring.find("exit") != 0) {
//        loadStruct.title = "exit";
//        xbee().printf("\n\rEXIT.");
//    }
//    /* EXIT */
    
    return loadStruct;  //each iteration this returns a completed struct
}