// Robot utility
// (c) 1999 Erik Hougaard & Lau Noergaard

__CONFIG(FOSC0|PWRTE|0x10);

#define DLY   3         /* cycles per null loop */
#define TX_OHEAD  13    /* overhead cycles per loop */
#define RX_OHEAD  12    /* receiver overhead per loop */
#define DELAY(ohead)  (((XTAL/4/BRATE)-(ohead))/DLY)


//unsigned char LastI2CData  = 1;
//unsigned char LastI2CClock = 1;

static bit  LED           @ (unsigned)&PORTA*8+0; 
static bit  ChipAck       @ (unsigned)&PORTA*8+1;    // Upgraded
static bit  ChipSelect    @ (unsigned)&PORTA*8+2;    // Upgraded
static bit  RxData        @ (unsigned)&PORTA*8+3;    // Upgraded
static bit  TxData        @ (unsigned)&PORTA*8+4;    // Upgraded
// static bit  I2Cclock      @ (unsigned)&PORTA*8+3;   
// static bit  I2Cdata       @ (unsigned)&PORTA*8+4;
static bit  Motor1OnOffx     @ (unsigned)&PORTB*8+2;
static bit  Motor2OnOffx     @ (unsigned)&PORTB*8+5;


// Global variables
unsigned char I2Cbuffer[I2Cbuffersize + 1];
unsigned char Active = 0;

// Serial Functions

// OK 60000 baud #define WHB asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop")
// OK 74000 baud 
#define WHB asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");asm("nop")

void putch2(char c)
{
	unsigned char bi = 8;
	TxData = 0; // Start Bit
	WHB;
	WHB;
	WHB;
	WHB;
	do
	{
		TxData = c & 1;
		WHB;
		WHB;
		WHB;
		WHB;
  	c = c >> 1;
	} while(--bi);
	TxData = 1; // Stop Bit
	WHB;
	WHB;
	WHB;
	WHB;
}

char getch2()
{
	unsigned char c = 0;
	unsigned char bi = 9;

	while(RxData); // Wait for start bit
	WHB; // Wait half bit
	WHB;
	do
	{
		c = (c >> 1) | (RxData << 7);
		WHB;
		WHB;
		WHB;
		WHB;
	} while(--bi);
	return c;
}

void putch(char c)
{
  unsigned char dly, bitno;

  TxData = 0;     /* start bit */
  bitno = 11;
  do {
    dly =  DELAY(TX_OHEAD);  /* wait one bit time */
    do
      /* nix */ ;
    while(--dly);
    if(c & 1)
      TxData = 1;
    if(!(c & 1))
      TxData = 0;
    // TxData = c & 1;
    c = (c >> 1) | 0x80;
  } while(--bitno);
}

char getch(void)
{
  unsigned char c, bitno, dly;

  for(;;) {
    while(RxData)
      continue; /* wait for start bit */
    dly = DELAY(3)/2;
    do
      /* nix */;
    while(--dly);
    if(RxData)
      continue; /* twas just noise */
    bitno = 8;
    c = 0;
    do 
    {
      dly = DELAY(RX_OHEAD);
      do
        /* nix */;
      while(--dly);
      c = (c >> 1) | (RxData << 7);
    } while(--bitno);
    // while(!RxData); // wait for stop bit to arrive
    return c;
  }
}


unsigned CheckIO()
{
	if (ChipSelect)
	{
		int            i;
		unsigned       CommandByte;
		unsigned char  CheckSum;
		
		// Haha, we must communicate!
		// Motor1OnOffx = 0;
		// Motor2OnOffx = 0;
		TRISA = 0b00001100;
		ChipAck = 1;

		CommandByte = getch2();
		switch(CommandByte)
		{
			case 10: // Enable Chip 
			  Active = 1;
			  break;
			case 11: // Disable Chip (Psudo power save)
			  Active = 0;
			  break;  
      case 100: // Send Data
        CheckSum = 0;
        for (i = 0; i < I2Cbuffersize; i++)
        {
          putch2(I2Cbuffer[i]);
          CheckSum = CheckSum + I2Cbuffer[i];
        }
        putch2(CheckSum);
        break;
      case 200: // Receive Data
        for (i = 0; i < I2Cbuffersize; i++)
          I2Cbuffer[i] = getch2();
        break;
		}

		ChipAck = 0;
		// Turn TRISA Back to normal
		TRISA = 0b00011110;
		return 1;
	}
	else
	  return 0;
}

void init()
{
  int i;
  // Startup Pause
  for (i = 0; i < 10; i++)
  {
  	DelayMs(100);
  	LED = !LED;
  }
  TRISA = 0b00011110;
  for (i = 0; i < I2Cbuffersize; i++)
    // I2Cbuffer[i] = 100 + i;
    I2Cbuffer[i] = 0;

  LED = 1;  
}