/* should keep something within a range */
#define CLAMP(X,MIN,MAX) X = ( (X<MIN) ? MIN : (X>MAX) ? MAX : X ) 

#define MIN_FREQ 300
#define MAX_FREQ 3000
#define SEQUECE_LENGHT 100   /* how high does it go ? */
int seqFreq[SEQUECE_LENGHT]; /* sequenceFrequency */
int seqDur[SEQUECE_LENGHT];  /* sequence duration */

int Z, Y, X;                 /* mouse positions */
byte statePin = LOW;         /* for blinking */
byte playMode = 0;

float modifier = 1.0;        /* playbackspeed modifier */
int head, tail, index;       /* queue housekeeping */

/*
 * Pin 5 is the mouse data pin, pin 6 is the clock pin
 * Pin 6 is connected to the speaker
 * Pin 13 is used because the dicimillia board has a led on it
 * Feel free to use whatever pins are convenient.
 */
#define MDATA 5
#define MCLK 6
#define outpin 3
#define ledPin 13


/* mousestuff */
//#define verbose // for verbose output from the mouse
unsigned char msInitSequence[] = { 
0xF3,  0xC8,  0xF3,  0x64,  0xF3,  0x50,  0xF2 };


void setup()
{
  Serial.begin(9600);
  pinMode(ledPin, OUTPUT);  
  pinMode(5, INPUT);
  randomSeed(analogRead(0));
  mouse_init();
  tail = index = 0;
  head=1;
}


void loop() {

  /* blink */
  statePin = !statePin;
  digitalWrite(ledPin, statePin);
 
  if (!playMode) {

    sequence(); 

    /* get frequency or change it */
    if (seqFreq[index] == 0 ) {
      //set to some default values
       seqFreq[index] = 440;
       seqDur[index] = 200;
    }
      
    seqFreq[index] = seqFreq[index] + 15 * X + Y; 
    X = Y = 0; // go back to zero
    CLAMP(seqFreq[index],  MIN_FREQ,  MAX_FREQ);

    /* play the sound */
    freqout(seqFreq[index], seqDur[index]);  

  } else {
    /* playmode */
    
    /* adjust index */
    index ++;
    if (index==SEQUECE_LENGHT)
      index=0;
    if (index==head)
      index = tail;
    
    /* adjust modifier */
    if (Z!=0)
      modifier += .01*Z;    
    Z=0; //back to zero
    //    CLAMP(modifier, 0.001, 2);
    freqout(seqFreq[index], seqDur[index] * modifier);
  }

/* a lot of usefull debugging information */
/*   showVal(seqFreq[index], "frequency "); */
/*   showVal(seqDur[index], "duration "); */
/*   showVal((modifier*1000), "modifier "); */
/*   showVal(head, "head "); */
/*   showVal(tail, "tail "); */
/*   showVal(index, "index "); */
/*   Serial.println(); */
  readMouse();
}

void readButtonsAndWheel() {
  char mstat = mouse_read();
  char i;
  char bit = 0x01;
  for(i = 0; i<8; i++) {
    if(mstat&bit) {
      switch(i) {
      case 0:
	/*         Serial.print("left button pressed \t"); */
	if (!playMode) {
	  seqDur[index] +=20;
	} else {
	  modifier /=2;
	}
        break;
      case 1:
	/* 	Serial.print("Right button pressed \t"); */
	if (!playMode) {
	  seqDur[index] -=20;
	} else {
	  modifier *=2;
	}
        break;
      case 2:
/*         Serial.print("Middle Button Pressed \t"); */

	if (!playMode) {
/* 	  Serial.println("Play mode \t"); */
	  playMode = 1;
	  index = tail;
	  Z=0;
	  modifier = 1.0;
	} else {
/* 	  Serial.println("Sequence mode \t"); */
	  playMode = 0;	
	  index = tail;
	  Z = index;
	}

        break;
      }
    }
    bit = bit<<1;
  } 
}


/*
  This function adjust the index if we need to
  change it, if the Z-value is different from
  index
*/
void sequence() {

  if(Z>index) {
    if (++index == SEQUECE_LENGHT) {
      index = 0; 
    }
    addToHead();
  } else if (Z<index) {
    if (--index<0) {
      index = SEQUECE_LENGHT -1;
    }
    addToTail();
      
  } 
  Z=index;
}
/*
  Enqueues at the head end
*/
void addToHead() {

  if (seqFreq[index] != 0) //not a new place
    return;

/*   Serial.println("head add"); */
  int oldHead = head;
  if (head!=tail) 
    head++; 
  if (head == SEQUECE_LENGHT ) 
    head = 0;
  if (head == tail)
    head = oldHead;
}
/*
  Enqueues at the tail end
*/
void addToTail() {

  if (seqFreq[index] != 0) //not a new place
    return;

/*   Serial.println("tail add"); */
  int oldTail = tail;
  if (tail!=head)
    tail--;
  if (tail<0)
    tail = SEQUECE_LENGHT-1;
  if (tail==head)
    tail = oldTail;
}

/* Helper function to display int values */
void showVal(int i, char * txt) {
  Serial.print(txt);
  Serial.print(i, DEC);
  Serial.print("\t");
}


/* freqout
   Paul Badger 2007
   a simple tone generation function
*/
void freqout(int freq, int t) {
  int hperiod;                               //calculate 1/2 period in us
  long cycles, i;
  pinMode(outpin, OUTPUT);                   // turn on output pin

  hperiod = (500000 / freq) - 7;             // subtract 7 us to make up for digitalWrite overhead - determined empirically
  // calculate cycles
  cycles = ((long)freq * (long)t) / 1000;    // calculate cycles
  // Serial.print(freq);
  // Serial.print((char)9);                   // ascii 9 is tab - you have to coerce it to a char to work 
  // Serial.print(hperiod);
  // Serial.print((char)9);
  // Serial.println(cycles);

  for (i=0; i<= cycles; i++){              // play note for t ms 
    digitalWrite(outpin, HIGH); 
    delayMicroseconds(hperiod);
    digitalWrite(outpin, LOW); 
    delayMicroseconds(hperiod - 1);     // - 1 to make up for fractional microsecond in digitaWrite overhead
  }
  pinMode(outpin, INPUT);                // shut off pin to avoid noise from other operations

}


void readMouse() {
  /* get a reading from the mouse */
  writeAndGetResponse(0xEB);  /* give me data! */
  readButtonsAndWheel();
  X += mouse_read();
  Y += mouse_read();
  Z += mouse_read();
}



/*
 * according to some code I saw, these functions will
 * correctly set the mouse clock and data pins for
 * various conditions.
 */
void gohi(int pin)
{
  pinMode(pin, INPUT);
  digitalWrite(pin, HIGH);
}

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


/* PRECONDITION : clock is HIGH */
/* sends a single bit and returns with clock high */
void sendBit(char bit) {

  if (bit) {
    gohi(MDATA);
  } 
  else {
    golo(MDATA);
  }
  /* wait for clock cycle */
  while (digitalRead(MCLK) == LOW)
    ;
  while (digitalRead(MCLK) == HIGH)
    ;
}

/* reads a single bit and returns with clock low */
char readBit() {
  char tmp = 0x00;
  while (digitalRead(MCLK) == HIGH)
    ;
  if (digitalRead(MDATA) == HIGH) 
    tmp = 0x01;
  while (digitalRead(MCLK) == LOW)
    ;
  return tmp;
}


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

  /* If EM84510 is not transmitting or if the system choose to */
  /* override the output, the system force CLK to an inactive  */
  /* level for a period of not less than 100μ sec while */
  /* preparing for output. When the system is ready to output  */
  /* start bit (0), it allows CLK go to active level. */

  /* put pins in output mode */
  gohi(MDATA);
  gohi(MCLK);
  delayMicroseconds(120);

  /* start bit */
  gohi(MCLK); // get the clock going
  sendBit(0);

  for (i=0; i < 8; i++) {
    char bit = (data>>i) & 0x01 ;
    sendBit(bit);
    parity = parity ^ (bit);
  }  

  /* parity */
  sendBit(parity);
  /* stop bit */
  sendBit(1);

  /* wait for mouse to switch modes */
  while ((digitalRead(MCLK) == LOW) || (digitalRead(MDATA) == LOW))
    ;
  /* put a hold on the incoming data. */
  golo(MCLK);
}

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

  /* start the clock */
  gohi(MCLK);
  gohi(MDATA);
  /*   delayMicroseconds(50); */

  readBit(); // startbit

  for (i=0; i < 8; i++) {
    data = data | (readBit()<<i);
  }

  readBit(); /* eat parity bit, which we ignore */
  readBit();   /* eat stop bit */

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


char writeAndGetResponse(char data) {
#ifdef verbose
  Serial.print("sending :  ");
  Serial.print((unsigned char)data, HEX);
  Serial.print("\t");
#endif   

  mouse_write( data );
  return response();
}

char response() {
  char data = mouse_read();
#ifdef verbose
  Serial.print("echo ");
  Serial.print((unsigned char)data, HEX);
  Serial.print(" from mouse\n");
#endif
  return data;
}

void msMode() {
  // sequence for ms mode (3 button + scroll)
  // this is a entering procedure and i guess it will
  // work with other mice/ICs as its problably stadard ps2
  // And using an array were just a quick way to handle them

  int i;
  for(i=0;i<7;i++) 
    writeAndGetResponse(msInitSequence[i]);
  if (response()==3) {
#ifdef verbose
    Serial.print(" ms mode\n"); 
#endif
  }
}

void mouse_init()
{
  gohi(MCLK);
  gohi(MDATA);
#ifdef verbose
  Serial.print("Sending reset to mouse\n");
#endif
  writeAndGetResponse(0xff);
  response();
  response();

  msMode();

  /* Remote Mode: */
  /* Data is transmitted only in response to a Read Data command. */
#ifdef verbose
  Serial.print("Sending remote mode code\n");
#endif
  writeAndGetResponse(0xF0);  /* remote mode */
  /*   delayMicroseconds(100); */
}