// Robot Module
// (c) 1999 Erik Hougaard & Lau Nørgaard

// PIC Layout

// PORTA Communication / Service Port

// BIT 0 - Activity LED 

// PORTB Feature Port

// ANSI C Includes
#include <stdio.h>
#include <pic.h>

// PIC Includes
#include "delay.h"

// Robot Includes

#define I2CADDR                     80  // I2C Address for this feature
#define I2Cbuffersize               10  // Size of I2C Buffer
#define XTAL                  10000000  // PIC Speed
#define BRATE                    76800  // Serial Port Speed

#include "util.c"
// Function list in "util.c"
// I2C (I2C Communication handler)
// I2Cbuffer[] (Data segment for I2C)
// putch (RS232)
// getch (RS232)

// A/D Converter
static bit  ADcs          @ (unsigned)&PORTB*8+1;    // bit1 in port B
static bit  ADsclk        @ (unsigned)&PORTB*8+0;    // bit0 in port B
static bit  ADdin         @ (unsigned)&PORTB*8+2;    // bit2 in port B
static bit  ADdout        @ (unsigned)&PORTB*8+3;    // bit3 in port B
static bit  ADLED         @ (unsigned)&PORTB*8+4;    // bit4 in port B


void ReadLED(unsigned char Channel)
{
  unsigned int d;
  unsigned int x;
  unsigned char xPos = Channel * 2;

  // for (i = 0;i < 5; i++)
  //{
    switch(Channel)
    {
      case 0:
        d = 0b10000000;
        break;
      case 1:
        d = 0b10010000;
        break;
      case 2:
        d = 0b10100000;
        break;
      case 3:
        d = 0b10110000;
        break;
      case 4:
        d = 0b11000000;
        break;
      case 5:
        d = 0b11010000;
        break;
      case 6:
        d = 0b11100000;
        break;
      case 7:
        d = 0b11110000;
        break;
    }         
    ADcs = 0;
    for (x = 0;x < 8;x++)
    {
      if (d & 0b10000000)
        ADdin = 1;
      else
        ADdin = 0;
      ADsclk = 1;
      ADsclk = 0;
      d = d << 1;
      if (x == 4)
        ADLED = 1;
    }
    ADcs = 1;
    DelayUs(3);
    ADLED = 0;
    DelayUs(17);
    ADcs = 0;

    d = 0;    
    for (x = 0; x < 12; x++)
    {
      if (ADdout)
        d = d + 1;
      d = d << 1;
      ADsclk = 1;
      ADsclk = 0;
    }
    d = d >> 1;
    ADcs = 1;

    I2Cbuffer[xPos] = (d & 0b0000111100000000) >> 8;
    I2Cbuffer[xPos+1] = (d & 0b0000000011111111);
    xPos += 2;
  //}
}


void main()
{
	unsigned char i;
	unsigned char i2;
	init(); // Initialization of PORTA and I2C Buffer
	PORTB = 0;
	TRISB = 0b00001000;

  //for (i = 0; i < I2Cbuffersize; i++)
  //  I2Cbuffer[i] = 100 + i;
	 
	for(;;)
	{
		CheckIO();
		if (Active)
  		ReadLED(0);
		CheckIO();
		if (Active)
  		ReadLED(1);
		CheckIO();
		if (Active)
  		ReadLED(2);
		CheckIO();
		if (Active)
  		ReadLED(3);
		CheckIO();
		if (Active)
  		ReadLED(4);
  		
		if (i == 0)
		{
			i2++;
			if (i2 == 0)
    		LED = !LED;
		}
	}
}