/* Functions
   whileencoders
   reset_encoders
   encoder
 */

/**********************
 * Macros for sensors *
 **********************/


/* Button Ports: */
#define nextbutton stop_button()
#define prevbutton start_button()
#define enterbutton digital(15)

/* Orientation Sensors */
#define light0 analog(16)
#define light1 analog(17)
#define light2 analog(18)
#define light3 analog(19)

/* Groundlights */
/* Groundlight0 left */
/* Groundlight1 right */
#define ground0 analog(3)
#define ground1 analog(4)

/* Starting Light Sensors */
#define startport 2

/* Bump sensors */
#define bumpfl digital(9)
#define bumpfr digital(10)
#define bumpbl digital(11)
#define bumpbr digital(12)

/* Shaft Encoders */
#define leftencoderport 7
#define rightencoderport 8

void reset_encoders() {
  reset_encoder(leftencoderport);
  reset_encoder(rightencoderport);
}

int encoder(int choice) {
  if (choice == 1)
    return read_encoder(leftencoderport);
  else
    return read_encoder(rightencoderport);
}


/* Continues until encoders reach a given point. */
/* If wheels are stuck, then print stuck */
/* direction 1= forward, direction 2= backward; 3=right, 4=left 5=other */
/* State: 
 */
int whileencoders(int state, int num) {
  int counter = 0;
  int prevvalue1 = 0;
  int prevvalue2 = 0;
  int curvalue1 = 0;
  int curvalue2 = 0;

  reset_encoders();
  while(curvalue1 < num || curvalue2 < num) {
    curvalue1 = encoder(1);
    curvalue2 = encoder(2);
    counter++;
    if (counter == 2000) {
      counter = 0;
      if (prevvalue1 == curvalue1)
	if (prevvalue2 == curvalue2) {
	  if (state == 1 || state == 2) { /* Stuck going forwards  => Ram*/
	    num = num - (curvalue1 + curvalue2) / 2;
	    ram(state);
	    reset_encoders();
	  }
	  if (state == 3 || state == 4) { /* Stuck turning */
	    num = num - (curvalue1 + curvalue2) / 2;
	    readjust(state);
	    reset_encoders();
	  }
	}
	else {
	  if (state == 1 || state == 2) { /* Stuck going forwards  => Ram*/
	    num = num - (curvalue1 + curvalue2) / 2;
	    ram(state);
	    reset_encoders();
	  }
	  
	  printf("left stuck...\n");
	}
      else
	if (prevvalue2 == curvalue2) {
	  if (state == 1 || state == 2) { /* Stuck going forwards  => Ram*/
	    num = num - (curvalue1 + curvalue2) / 2;
	    ram(state);
	    reset_encoders();
	  }

	  printf("right stuck...\n");
	}
	else
	  ;
      prevvalue1 = curvalue1;
      prevvalue2 = curvalue2;
    }
  }
}

void whileencoders2(int num) {
  reset_encoders();
  while(encoder(1) < num || encoder(2) < num)
    ;
}