#define MDATA 4
#define MCLK 5

int motor1Pin1 = 8; 
int motor1Pin2 = 9; 

int motor2Pin1 = 7; 
int motor2Pin2 = 6; 

int motorsPinEnable = 2;

int valueX = 0;
int valueY = 0;

void gohi(int pin)
{
  pinMode(pin, INPUT);
  digitalWrite(pin, HIGH);
}

void golo(int pin)
{
  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
}

void mouse_write(char data)
{
  char i;
  char parity = 1;

  /* put pins in output mode */
  gohi(MDATA);
  gohi(MCLK);
  delayMicroseconds(300);
  golo(MCLK);
  delayMicroseconds(300);
  golo(MDATA);
  delayMicroseconds(10);
  /* start bit */
  gohi(MCLK);
  /* wait for mouse to take control of clock); */
  while (digitalRead(MCLK) == HIGH)
    ;
  /* clock is low, and we are clear to send data */
  for (i=0; i < 8; i++) {
    if (data & 0x01) {
      gohi(MDATA);
    } 
    else {
      golo(MDATA);
    }
    /* wait for clock cycle */
    while (digitalRead(MCLK) == LOW)
      ;
    while (digitalRead(MCLK) == HIGH)
      ;
    parity = parity ^ (data & 0x01);
    data = data >> 1;
  }  
  /* parity */
  if (parity) {
    gohi(MDATA);
  } 
  else {
    golo(MDATA);
  }
  while (digitalRead(MCLK) == LOW)
    ;
  while (digitalRead(MCLK) == HIGH)
    ;
  /* stop bit */
  gohi(MDATA);
  delayMicroseconds(50);
  while (digitalRead(MCLK) == HIGH)
    ;
  /* wait for mouse to switch modes */
  while ((digitalRead(MCLK) == LOW) || (digitalRead(MDATA) == LOW))
    ;
  /* put a hold on the incoming data. */
  golo(MCLK);
  //  Serial.print("done.\n");
}

/*
 * Get a byte of data from the mouse
 */
char mouse_read(void)
{
  char data = 0x00;
  int i;
  char bit = 0x01;

  //  Serial.print("reading byte from mouse\n");
  /* start the clock */
  gohi(MCLK);
  gohi(MDATA);
  delayMicroseconds(50);
  while (digitalRead(MCLK) == HIGH)
    ;
  delayMicroseconds(5);  /* not sure why */
  while (digitalRead(MCLK) == LOW) /* eat start bit */
    ;
  for (i=0; i < 8; i++) {
    while (digitalRead(MCLK) == HIGH)
      ;
    if (digitalRead(MDATA) == HIGH) {
      data = data | bit;
    }
    while (digitalRead(MCLK) == LOW)
      ;
    bit = bit << 1;
  }
  /* eat parity bit, which we ignore */
  while (digitalRead(MCLK) == HIGH)
    ;
  while (digitalRead(MCLK) == LOW)
    ;
  /* eat stop bit */
  while (digitalRead(MCLK) == HIGH)
    ;
  while (digitalRead(MCLK) == LOW)
    ;

  /* put a hold on the incoming data. */
  golo(MCLK);

  return data;
}

void mouse_init()
{
  gohi(MCLK);
  gohi(MDATA);
  //  Serial.print("Sending reset to mouse\n");
  mouse_write(0xff);
  mouse_read();  /* ack byte */
  //  Serial.print("Read ack byte1\n");
  mouse_read();  /* blank */
  mouse_read();  /* blank */
  //  Serial.print("Sending remote mode code\n");
  mouse_write(0xf0);  /* remote mode */
  mouse_read();  /* ack */
  //  Serial.print("Read ack byte2\n");
  delayMicroseconds(100);
}

void setup()
{
  Serial.begin(9600);
  mouse_init();
  pinMode(motor1Pin1, OUTPUT); 
  pinMode(motor1Pin2, OUTPUT); 
  pinMode(motor2Pin1, OUTPUT); 
  pinMode(motor2Pin2, OUTPUT); 
  pinMode(motorsPinEnable, OUTPUT);

  digitalWrite(motorsPinEnable,HIGH); 


}

/*
 * get a reading from the mouse and report it back to the
 * host via the serial line.
 */
void loop()
{
  char mstat;
  char mx;
  char my;
  int inByte = 0;         // incoming serial byte
  
  /* get a reading from the mouse */
  mouse_write(0xeb);  /* give me data! */
  mouse_read();      /* ignore ack */
  mstat = mouse_read();
  mx = mouse_read();
  my = mouse_read();
  valueX +=mx;
  valueY +=my;
  
 if (Serial.available() > 0) {

  /* send the data back up */
  //Serial.print(mstat, BIN);
  inByte = Serial.read();
  switch(inByte){
    case 'X':
      Serial.println(valueX, DEC);
    break;
    case 'Y': 
     Serial.println(valueY, DEC);
     break;
     default:
     break;
  }
 }
  delay(10);
  Serial.print("\tX=");
  Serial.print(mx, DEC);
  Serial.print("\tY=");
  Serial.print(my, DEC);
  Serial.print("\tXX=  ");
  Serial.print(valueX, DEC);
  Serial.print("\tYY=  ");
  Serial.print(valueY, DEC);
  Serial.println();
  delay(20);  /* twiddle */
 
 if(my < 0)
   backward(my);
 else if(my > 0)
   forward(my);
   
 if(mx < 0)
   left(mx);
 else if (mx > 0)
    right(mx);

}

void forward(int del){
   Serial.println("Begin");
    digitalWrite(motorsPinEnable,HIGH);
    digitalWrite(motor1Pin1, LOW); 
    digitalWrite(motor1Pin2, HIGH);
    
    digitalWrite(motor2Pin1, HIGH); 
    digitalWrite(motor2Pin2, LOW);
   
    delay(abs(del)); 
    digitalWrite(motorsPinEnable,LOW);
    Serial.println("Einde");
}

void backward(int del){
    digitalWrite(motorsPinEnable,HIGH);
    digitalWrite(motor1Pin1, HIGH); 
    digitalWrite(motor1Pin2, LOW); 
    
    digitalWrite(motor2Pin1, LOW);
    digitalWrite(motor2Pin2, HIGH);
    
    delay(abs(del)); 
    digitalWrite(motorsPinEnable,LOW);
}

void left(int del){
    digitalWrite(motorsPinEnable,HIGH);
    digitalWrite(motor1Pin1, LOW); 
    digitalWrite(motor1Pin2, HIGH);  
    
    digitalWrite(motor2Pin1, LOW);
    digitalWrite(motor2Pin2, HIGH);
    
    delay(abs(del)); 
    digitalWrite(motorsPinEnable,LOW);
}

void right(int del){
    digitalWrite(motorsPinEnable,HIGH);
    digitalWrite(motor1Pin1, HIGH); 
    digitalWrite(motor1Pin2, LOW);  
    
    digitalWrite(motor2Pin1, HIGH);
    digitalWrite(motor2Pin2, LOW);
    
    delay(abs(del));  
    digitalWrite(motorsPinEnable,LOW);
}