/*************************************************************************
 * program to use CMUcam color tracking on NXT
 * relies on the PIC16F690-based I2C-to-RS232 bridge
 *
 * modification history:
 * 2008-09-13: created
 ************************************************************************/

#include "NXCDefs.h"

//application specific defines
#define I2C_PORT         IN_1
#define CHAR_WIDTH       6
#define LINE_WIDTH       16
#define SCREEN_X_ORIG    5
#define SCREEN_Y_ORIG    13
#define SCREEN_WIDTH     90
#define SCREEN_HEIGHT    50
#define SPEED            70

//PIC16F690 specific defines
#define PIC_ADDRESS      38
#define START_BUF        128
#define GET_CHAR         129
#define BUF_SIZE         70

//CMUcam2 specific defines
#define CAM_RESET        "RS"
#define CAM_GET_VERSION  "GV"
#define CAM_TRACK_PARAMS "GT"
//TC Rmin Rmax Gmin Gmax Bmin Bmax \r
#define CAM_TRACK_COLOR  "TC 210 240 0 50 0 240"
#define CAM_LED_ON       "L0 1"
#define CAM_LED_OFF      "L0 0"

/**
 * send one byte to and receive one byte from i2c
 */
byte txrxI2C(const byte data) {
  byte recv[1];
  byte send[2];
  send[0] = PIC_ADDRESS;
  send[1] = data;
  while (I2CCheckStatus(I2C_PORT) != 0) {
  }
  I2CWrite(I2C_PORT, 1, send);
  while (I2CCheckStatus(I2C_PORT) != 0) {
  }
  while (I2CBytesReady(I2C_PORT) < 1) {
  }
  I2CRead(I2C_PORT, 1, recv);
  return recv[0];
}

/**
 * send a command byte-per-byte to i2c
 */
void sendCommandI2C(const string command) {
  int answer;
  txrxI2C(13);
  for (int i = 0; i < StrLen(command); i++) {
    answer = 0;
    do {
      answer = txrxI2C(command[i]);
    } while (answer != command[i]);
  }
  do {
    answer = txrxI2C(13);
  } while (answer != 13);
}

/**
 * send command to start buffering on the PIC16F690
 */
void startBufferI2C() {
  txrxI2C(START_BUF);
}

/**
 * send command to stop buffering on the PIC16F690
 */
void stopBufferI2C() {
  txrxI2C(GET_CHAR);
}

/**
 * read out the buffer from the PIC16F690
 */
void getResultI2C(byte &result[]) {
  for (int i = 0; i < BUF_SIZE; i++) {
    result[BUF_SIZE - i] = txrxI2C(GET_CHAR);
  }
}

/**
 * get a dummy result as expected from the PIC16F690
 */
void dummyGetResultI2C(byte &result[]) {
  for (int i = 0; i < BUF_SIZE; i++) {
    result[i] = 0;
  }
  string dummy = "T 10 20 255 4 55 6 77 128 T 10 20 30 40 abcd";
  for (int i = 0; i < StrLen(dummy); i++) {
   result[i] = dummy[i];
  }
  result[25] = 13;
}

/**
 * print the contents of the specified array
 */
void printResult(const byte result[]) {
  int i = 0;
  int j = 0;
  ClearScreen();
  for (i = 0; i < BUF_SIZE / LINE_WIDTH + 1; i++) {
    for (j = 0; j < LINE_WIDTH; j++) {
      if (j + i * LINE_WIDTH >= BUF_SIZE) {
        return;
      }
      byte help[1];
      help[0] = result[j + i * LINE_WIDTH];
      string temp;
      ByteArrayToStrEx(help, temp)
      switch (i) {
      case 0:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE1, temp, false);
        break;
      case 1:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE2, temp, false);
        break;
      case 2:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE3, temp, false);
        break;
      case 3:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE4, temp, false);
        break;
      case 4:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE5, temp, false);
        break;
      case 5:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE6, temp, false);
        break;
      case 6:
        TextOut(1 + j * CHAR_WIDTH, LCD_LINE7, temp, false);
        break;
      case 7:
        TextOut(1 +  j * CHAR_WIDTH, LCD_LINE8, temp, false);
        break;
      }
    }
  }
}

/**
 * extract the last complete answer from the given array. look for
 * the last carriage return (ASCII-code 13) in the array and return
 * the values preceding this carriage return until another carriage
 * return is found or the begin of the array is reached.
 */
void getLastAnswer(byte &result[]) {
  int i;
  int j;
  byte answer_temp[BUF_SIZE];
  answer_temp[0] = 0;
  
  //find last carriage return
  for (i = BUF_SIZE - 1; i > 0; i--) {
    if (result[i] == 13) {
      break;
    }
  }
  //no carriage return found
  if (result[i] != 13) {
    for (int j = 0; j < 8; j++) {
      return;
    }
  }

  //copy bytes until next carriage return or 0 to answer_temp
  j = 0;
  do {
    answer_temp[j++] = result[--i];
  } while (i > 0 && result[i - 1] != 13 && result[i - 1] != 0);
  for (i = 0; i <= j - 1; i++) {
    result[i] = answer_temp[j - 1 - i];
  }
  //copy bytes in reversed order to result
  for (i = j; i < BUF_SIZE; i++) {
    result[i] = 0;
  }
}

/**
 * reset the camera
 */
void resetCamera() {
  byte result[BUF_SIZE];
  do {
    sendCommandI2C(CAM_LED_ON);
    startBufferI2C();
    sendCommandI2C(CAM_RESET);
    sendCommandI2C(CAM_LED_OFF);
    getResultI2C(result);
    getLastAnswer(result);
  } while (result[0] != ':' || result[1] != 'A' ||
           result[2] != 'C' || result[3] != 'K')
}

/**
 * extract coordinates from a received result where a result is of the
 * form .* c0 c1 c2 c3 c4 c5 c6 c7\r.*, that is, a sequence of
 * integers separated by spaces, and terminated bya carriage return
 * (ASCII-code 13)
 */
void getCoordinates(const byte result[], byte &coords[]) {
  int i;
  //find last carriage return
  for (i = BUF_SIZE - 1; i > 0; i--) {
    if (result[i] == 13) {
      break;
    }
  }
  //no carriage return found
  if (result[i] != 13) {
    for (int k = 0; k < 8; k++) {
      coords[k] = 0;
      return;
    }
  }
  //process 8 integers (each integer has max. 3 characters)
  for (int j = 7; j >= 0; j--) {
    byte temp[3];
    int k;
    int l;
    string help_string = "";
    do {
      i--;
    } while (result[i] != ' ' && i > 0);
    if (i == 0) {
      for (int k = 0; k < 8; k++) {
        coords[k] = 0;
        return;
      }
    }
    l = i;
    k = 0;
    do {
      temp[k++] = result[++l];
    } while (result[l + 1] != 13 && result[l + 1] != ' ');
    ByteArrayToStrEx(temp, help_string)
    coords[j] = StrToNum(help_string);
  }
}

/**
 * print an array of 8 integers at the bottom two rows of the display
 */
void printCoordinates(const byte coords[]) {
  for (int i = 0; i < 8; i++) {
    NumOut(1,  LCD_LINE7, coords[0]);
    NumOut(25, LCD_LINE7, coords[1]);
    NumOut(50, LCD_LINE7, coords[2]);
    NumOut(75, LCD_LINE7, coords[3]);
    NumOut(1,  LCD_LINE8, coords[4]);
    NumOut(25, LCD_LINE8, coords[5]);
    NumOut(50, LCD_LINE8, coords[6]);
    NumOut(75, LCD_LINE8, coords[7]);
  }
}

/**
 * main. reset the camera and drive towards a colored object.
 */
task main() {
  int button_count;
  int counter = 0;
  byte result[BUF_SIZE];
  byte coords[8];
  bool tracking_enabled = false;

  SetSensorType(I2C_PORT, SENSOR_TYPE_LOWSPEED);
  SetSensorMode(I2C_PORT, IN_MODE_RAW);
  ResetSensor(I2C_PORT);
  TextOut(1, LCD_LINE1, "Trying to reset");
  TextOut(1, LCD_LINE2, "CMUcam2.");

  resetCamera();
  TextOut(1, LCD_LINE8, "Camera reset.");

  while(true) {
    if (ButtonCount(BTNLEFT, true) >= 1) {
      ClearScreen();
      TextOut(1, LCD_LINE1, "Left button.");
      startBufferI2C();
      sendCommandI2C(CAM_GET_VERSION);
      //sendCommandI2C(CAM_TRACK_PARAMS);
      Wait(200);
      getResultI2C(result);
      printResult(result);
      tracking_enabled = false;
    } else if (ButtonCount(BTNRIGHT, true) >= 1) {
      ClearScreen();
      TextOut(1, LCD_LINE1, "Right button.");
      resetCamera();
      TextOut(1, LCD_LINE8, "Camera reset.");
      tracking_enabled = false;
    } else if (ButtonCount(BTNCENTER, true) >= 0) {
      if (!tracking_enabled) {
        startBufferI2C();
        sendCommandI2C(CAM_TRACK_COLOR);
        Wait(200);
        tracking_enabled = true;
      }
      getResultI2C(result);
      startBufferI2C();

      ClearScreen();
      Wait(70);
      getCoordinates(result, coords);

      //draw a screen showing the position of the tracked object
      printCoordinates(coords);
      RectOut(SCREEN_X_ORIG, SCREEN_Y_ORIG, SCREEN_WIDTH, SCREEN_HEIGHT);
      CircleOut(SCREEN_X_ORIG + SCREEN_WIDTH - coords[0],
                SCREEN_Y_ORIG + SCREEN_HEIGHT - coords[1]/3, 2);
      RectOut(SCREEN_X_ORIG + SCREEN_WIDTH - coords[4],
              SCREEN_Y_ORIG + SCREEN_HEIGHT - coords[5]/3,
              (coords[4] - coords[2]), (coords[5] - coords[3])/3);

      //depending on the coordinates switch the motors on and off
      Off(OUT_AB);
      if (coords[0] == 0 && coords[3] != 0) {
        continue;
      }
      int x = coords[0];
      if (x < 40) {
        RotateMotor(OUT_A, SPEED, 3 * (x - 45));
        Off(OUT_A);
        RotateMotor(OUT_B, SPEED, 3 * (45 - x));
        Off(OUT_B);
      }
      else if (coords[0] > 50) {
        RotateMotor(OUT_A, SPEED, 3 * (x - 45));
        Off(OUT_A);
        RotateMotor(OUT_B, SPEED, 3 * (45 - x));
        Off(OUT_B);
      }
      OnRev(OUT_AB, 45 - abs(x - 45));
    }
  }
}