#include "Controller.h"


// Header code defined in the model

// End of header code defined in the model

#define STATE__START__STATE 0
#define STATE__START 1
#define STATE__CHECK_ENVIRONMENT 2
#define STATE__choice__0 3
#define STATE__CHECK_COLOR 4
#define STATE__GO_FORWARDS 5
#define STATE__CHECK_CONTACT 6
#define STATE__choice__1 7
#define STATE__GO_BACKWARDS 8
#define STATE__TURN_LEFT 9
#define STATE__MOTOR_OFF 10
#define STATE__STOP__STATE 11

void *mainFunc__Controller(void *arg){
  int myGlob__MOTOR_OFF_DELAY = 1;
  int myGlob__MOTOR_ON_DELAY = 1;
  int myGlob__FACTOR_ACC = 6;
  int myGlob__FACTOR_DEC = 5;
  int myGlob__MOTOR_SPEED = 500;
  int myGlob__SONAR_DELAY = 2;
  int myGlob__CONTACT_DELAY = 1;
  int myGlob__COLOR_DELAY = 1;
  int factor = 0;
  int speed = 0;
  int distance = 0;
  int contact = 0;
  int hasBeenPushed = 0;
  int flagA = 0;
  int flagB = 0;
  int flag = 0;
  int deg = 0;
  int color = 0;
  
  int __currentState = STATE__START__STATE;
  __attribute__((unused)) request __req0;
  __attribute__((unused))int *__params0[2];
  __attribute__((unused)) request __req1;
  __attribute__((unused))int *__params1[2];
  __attribute__((unused))setOfRequests __list;
  __attribute__((unused))pthread_cond_t __myCond;
  __attribute__((unused))request *__returnRequest;
  
  char * __myname = (char *)arg;
  
  pthread_cond_init(&__myCond, NULL);
  
  fillListOfRequests(&__list, __myname, &__myCond, &__mainMutex);
  //printf("my name = %s\n", __myname);
  
  /* Main loop on states */
  while(__currentState != STATE__STOP__STATE) {
    switch(__currentState) {
      case STATE__START__STATE: 
      traceStateEntering(__myname, "__StartState");
      __currentState = STATE__START;
      break;
      
      case STATE__START: 
      traceStateEntering(__myname, "START");
      __currentState = STATE__CHECK_ENVIRONMENT;
      break;
      
      case STATE__CHECK_ENVIRONMENT: 
      traceStateEntering(__myname, "CHECK_ENVIRONMENT");
      makeNewRequest(&__req0, 2704, SEND_SYNC_REQUEST, 0, 0, 0, 0, __params0);
      __req0.syncChannel = &__Sonar_ready_to_receive__Controller_ready_to_receive_dist;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &distance;
      makeNewRequest(&__req0, 2705, RECEIVE_SYNC_REQUEST, 0, 0, 0, 1, __params0);
      __req0.syncChannel = &__Sonar_sendDistance__Controller_receiveDistance;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __currentState = STATE__choice__0;
      break;
      
      case STATE__choice__0: 
      traceStateEntering(__myname, "choice__0");
      if (distance < 100) {
        makeNewRequest(&__req0, 2752, IMMEDIATE, 0, 0, 0, 0, __params0);
        addRequestToList(&__list, &__req0);
      }
      if (!(distance < 100)) {
        makeNewRequest(&__req1, 2756, IMMEDIATE, 0, 0, 0, 0, __params1);
        addRequestToList(&__list, &__req1);
      }
      if (nbOfRequests(&__list) == 0) {
        debug2Msg(__myname, "No possible request");
        __currentState = STATE__STOP__STATE;
        break;
      }
      __returnRequest = executeListOfRequests(&__list);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
       if (__returnRequest == &__req0) {
        __currentState = STATE__CHECK_COLOR;
        
      }
      else  if (__returnRequest == &__req1) {
        __currentState = STATE__GO_FORWARDS;
        
      }
      break;
      
      case STATE__CHECK_COLOR: 
      traceStateEntering(__myname, "CHECK_COLOR");
      makeNewRequest(&__req0, 2708, SEND_SYNC_REQUEST, 0, 0, 0, 0, __params0);
      __req0.syncChannel = &__Color_ready_to_receive__Controller_ready_to_receive_color;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &color;
      makeNewRequest(&__req0, 2709, RECEIVE_SYNC_REQUEST, 0, 0, 0, 1, __params0);
      __req0.syncChannel = &__Color_sendColor__Controller_receiveColor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      waitFor((myGlob__COLOR_DELAY)*1000000, (myGlob__COLOR_DELAY)*1000000);
      __currentState = STATE__CHECK_COLOR;
      break;
      
      case STATE__GO_FORWARDS: 
      traceStateEntering(__myname, "GO_FORWARDS");
      speed = myGlob__MOTOR_SPEED;
      traceVariableModification("Controller", "speed", speed,0);
      flagA = 0;
      traceVariableModification("Controller", "flagA", flagA,0);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2711, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor1__Motor1_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2712, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor2__Motor2_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      waitFor((myGlob__MOTOR_ON_DELAY)*1000000, (myGlob__MOTOR_ON_DELAY)*1000000);
      __currentState = STATE__CHECK_CONTACT;
      break;
      
      case STATE__CHECK_CONTACT: 
      traceStateEntering(__myname, "CHECK_CONTACT");
      makeNewRequest(&__req0, 2714, SEND_SYNC_REQUEST, 0, 0, 0, 0, __params0);
      __req0.syncChannel = &__Controller_ready_to_receive_cont__Contact_ready_to_receive;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &contact;
      __params0[1] = &hasBeenPushed;
      makeNewRequest(&__req0, 2715, RECEIVE_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_receiveContact__Contact_sendContact;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __currentState = STATE__choice__1;
      break;
      
      case STATE__choice__1: 
      traceStateEntering(__myname, "choice__1");
      if ((hasBeenPushed == 1)||(contact == 1)) {
        makeNewRequest(&__req0, 2726, IMMEDIATE, 0, 0, 0, 0, __params0);
        addRequestToList(&__list, &__req0);
      }
      if (!((hasBeenPushed == 1)||(contact == 1))) {
        makeNewRequest(&__req1, 2774, IMMEDIATE, 0, 0, 0, 0, __params1);
        addRequestToList(&__list, &__req1);
      }
      if (nbOfRequests(&__list) == 0) {
        debug2Msg(__myname, "No possible request");
        __currentState = STATE__STOP__STATE;
        break;
      }
      __returnRequest = executeListOfRequests(&__list);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
       if (__returnRequest == &__req0) {
        __currentState = STATE__GO_BACKWARDS;
        
      }
      else  if (__returnRequest == &__req1) {
        __currentState = STATE__MOTOR_OFF;
        
      }
      break;
      
      case STATE__GO_BACKWARDS: 
      traceStateEntering(__myname, "GO_BACKWARDS");
      speed = myGlob__MOTOR_SPEED;
      traceVariableModification("Controller", "speed", speed,0);
      flagA = 1;
      traceVariableModification("Controller", "flagA", flagA,0);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2718, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor1__Motor1_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2719, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor2__Motor2_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      waitFor((1)*1000000, (1)*1000000);
      __currentState = STATE__TURN_LEFT;
      break;
      
      case STATE__TURN_LEFT: 
      traceStateEntering(__myname, "TURN_LEFT");
      flag = 0;
      traceVariableModification("Controller", "flag", flag,0);
      deg = 200;
      traceVariableModification("Controller", "deg", deg,0);
      __params0[0] = &flag;
      __params0[1] = &deg;
      makeNewRequest(&__req0, 2721, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_turn1__Motor1_turn;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &flag;
      __params0[1] = &deg;
      makeNewRequest(&__req0, 2722, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_turn2__Motor2_turn;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      waitFor((myGlob__MOTOR_OFF_DELAY)*1000000, (myGlob__MOTOR_OFF_DELAY)*1000000);
      __currentState = STATE__CHECK_ENVIRONMENT;
      break;
      
      case STATE__MOTOR_OFF: 
      traceStateEntering(__myname, "MOTOR_OFF");
      speed = 0;
      traceVariableModification("Controller", "speed", speed,0);
      flagA = 1;
      traceVariableModification("Controller", "flagA", flagA,0);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2724, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor1__Motor1_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      __params0[0] = &speed;
      __params0[1] = &flagA;
      makeNewRequest(&__req0, 2725, SEND_SYNC_REQUEST, 0, 0, 0, 2, __params0);
      __req0.syncChannel = &__Controller_run_motor2__Motor2_run_motor;
      __returnRequest = executeOneRequest(&__list, &__req0);
      clearListOfRequests(&__list);
      traceRequest(__myname, __returnRequest);
      waitFor((myGlob__MOTOR_OFF_DELAY)*1000000, (myGlob__MOTOR_OFF_DELAY)*1000000);
      __currentState = STATE__CHECK_ENVIRONMENT;
      break;
      
    }
  }
  //printf("Exiting = %s\n", __myname);
  return NULL;
}