#pragma config(CircuitBoardType, typeCktBoardMega)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl20, SDATA,          sensorDigitalOut)
#pragma config(Sensor, dgtl21, SCLK,           sensorDigitalOut)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/** \file NXShieldTest1.c
 * \brief 1st test with ArduinoMega and NXShield-M
 *
 * Changelog:
 * - 0.1: Initial release
 *
 * License: You may use this code as you wish, provided you give credit where its due.
 *
 * Credit: Adapted from http://www.keil.com/download/docs/64.asp
 *
 * \author Xander Soldaat (mightor_at_gmail.com)
 * \date 20 May 2011
 * \version 0.1
 */

typedef ubyte array[16];
#define SETSCL(X)  SensorValue[SCLK] = X

//Was a startcond condition sent most recently? Used to do double-startcond
ubyte startcond = 0;

//==== Lowest level functions to manage the pins -- All LPC-specific code is here

//Set SDA line to read and return the value on it.
//Since the line has a pull-up on it, this pulls the
//SDA line high if no one else is driving it
int READSDA() {
  SensorType[SDATA] = sensorDigitalIn ;
  return SensorValue[SDATA];
}

//Set SDA line to write and pull it low
void CLRSDA(void) {
  SensorType[SDATA] = sensorDigitalOut;
  //wait1Msec(1);
  SensorValue[SDATA] = 0;
}



void i2c_stop() {
  SensorType[SDATA]   = sensorDigitalOut;
  SensorValue[SCLK]   = 0;              // Set clock line low
  SensorValue[SDATA]  = 0;              // Set data line low
  SensorValue[SCLK]   = 1;             // Set clock line high
  SensorValue[SDATA]  = 1;             // Set data line high (STOP SIGNAL)
  SensorType[SDATA]   = sensorDigitalIn;  // Put port pin into HiZ
}

//==== Bit-level functions
//Translated from Wikipedia pseudocode - http://en.wikipedia.org/wiki/I2C

ubyte read_bit(void) {
   ubyte bit;

   // Let the slave drive data
   READSDA();
   SETSCL(1);
   bit = READSDA();
   SETSCL(0);
   return bit;
}

void write_bit(ubyte bit) {
	//Put the bit on SDA by either letting it rise or pulling it down
	if (bit) {
		READSDA();
	} else {
		CLRSDA();
	}
	SETSCL(1);
	SETSCL(0);
}

void startcond_cond(void) {
   READSDA();
   SETSCL(1);
   CLRSDA();
  // Now pull SCL down
   SETSCL(0);
}


//==== _byte-level protocol
//Translated from Wikipedia pseudocode - http://en.wikipedia.org/wiki/I2C

ubyte tx(int send_startcond, int send_stop, ubyte _byte) {
   ubyte bit;
   ubyte nack;
   if (send_startcond) {
      startcond_cond();
   }
   for (bit = 0; bit < 8; bit++) {
      write_bit(_byte & 0x80);
      _byte <<= 1;
   }
   nack = read_bit();
   if (send_stop) {
      i2c_stop();
      //stop_cond();
   }
   return nack;
}

ubyte rx (int nak, int send_stop) {
   ubyte _byte = 0;
   ubyte bit;

   for(bit = 0; bit < 8; bit++) {
    _byte <<= 1;
    _byte |= read_bit();
   }
   write_bit(nak);
   if (send_stop) {
      i2c_stop();
      //stop_cond();
   }
   return _byte;
}

//==== Multi-_byte protocol - Public interface to I2C bus

int i2c_tx_string(ubyte addr, array &buf, int len) {
  tx(1,0,addr << 1 | 0);
  for(int i = 0; i < len - 1; i++)
    tx(0,0,buf[i]);
  tx(0,1,buf[len-1]);
  return 0;
}

int i2c_rx_string(ubyte addr, array &buf, int len) {
  tx(1,0,addr << 1 | 1);
  for(int i = 0; i < len - 1; i++)
    buf[i]=rx(0,0);
  buf[len-1]=rx(1,1);
  return 0;
}

void setupTouch()
{
  array buffer;
	buffer[0] = 0x8A;
	buffer[1] = 0x01;
  i2c_tx_string(0x03, buffer, 2);
}

bool readTouch()
{
  array buffer;
	buffer[0] = 0x8C;
  i2c_tx_string(0x03, buffer, 1);
  i2c_rx_string(0x03, buffer, 2);
  return ((buffer[1] << 8) + buffer[0]) < 512;
}

void turnMotor(ubyte speed)
{
  array buffer;
  buffer[0] = 0x46;
  buffer[1] = speed;
  buffer[2] = 0x02;
  buffer[3] = 0x00;
  buffer[4] = 0x81;
  i2c_tx_string(0x03, buffer, 5);
}

task main () {
  setupTouch();
  while (true)
  {
    if (readTouch())
      turnMotor(50);
    else
      turnMotor(0);
    wait1Msec(50);
  }
}