#include <ps2.h>

 // Pin 5 il segnale DATA dal mouse, pin 6 è il clock
#define MDATA 5
#define MCLK 6

int distanza = 0;
unsigned long time1=0, time2=0,delta=0, sp=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;

  gohi(MDATA);
  gohi(MCLK);
  delayMicroseconds(300);
  golo(MCLK);
  delayMicroseconds(300);
  golo(MDATA);
  delayMicroseconds(10);
  /* inizio bit */
  gohi(MCLK);
  /* attende che il mouse controlla il CLK */
  while (digitalRead(MCLK) == HIGH)
    ;
  /* il CLK è LOW inizia a inviare i dati */
  for (i=0; i < 8; i++) {
    if (data & 0x01) {
      gohi(MDATA);
    }
    else {
      golo(MDATA);
    }
    /* attesa per il ciclo CLK */
    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)
    ;
  /* attende */
  while ((digitalRead(MCLK) == LOW) || (digitalRead(MDATA) == LOW))
    ;

  golo(MCLK);
  //  Serial.print("done.\n");
}

char mouse_read(void)
{
  char data = 0x00;
  int i;
  char bit = 0x01;

  //  Serial.print("reading byte from mouse\n");
  /*  clock */
  gohi(MCLK);
  gohi(MDATA);
  delayMicroseconds(50);
  while (digitalRead(MCLK) == HIGH)
    ;
  delayMicroseconds(5);
  while (digitalRead(MCLK) == LOW)
    ;
  for (i=0; i < 8; i++) {
    while (digitalRead(MCLK) == HIGH)
      ;
    if (digitalRead(MDATA) == HIGH) {
      data = data | bit;
    }
    while (digitalRead(MCLK) == LOW)
      ;
    bit = bit << 1;
  }

  while (digitalRead(MCLK) == HIGH)
    ;
  while (digitalRead(MCLK) == LOW)
    ;

  while (digitalRead(MCLK) == HIGH)
    ;
  while (digitalRead(MCLK) == LOW)
    ;


  golo(MCLK);
  //  Serial.print("Recvd data ");
  //  Serial.print(data, HEX);
  //  Serial.print(" from mouse\n");
  return data;
}

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

void setup()
{
  Serial.begin(9600);
  mouse_init();
}


void loop()
{
  char mstat;
  char mx;



  mouse_write(0xeb);
  mouse_read();
  mstat = mouse_read();
  mx = mouse_read();
  time2=millis();
  delta = time2-time1;

  if (delta>0) sp = 1000*abs(mx)/delta;
  distanza+=mx;



  Serial.print(delta, DEC);
  //Serial.print("\t");
  //Serial.print(time1, DEC);
  //Serial.print("\t");
  //Serial.print(time2, DEC);
  Serial.print("\tX=");
  Serial.print(mx, DEC);
  Serial.print("\tV=");
  Serial.print(sp, DEC);
  Serial.print("\t");
  Serial.print(distanza, DEC);
  Serial.println();
  delay(20);
  time1=time2;
}