/** 
 * @file
 * @brief KU Corobots API implementation
 * @author Chaiporn Jaikaeo <chaiporn.j@ku.ac.th>
 *
 * @mainpage
 * This site provides documentation and basic information about the KU Corobots.
 *
 * <h2>The KU Corobots</h2>
 * \image html corobots.jpg
 * \image latex corobots.jpg "The KU Corobots" width=4in
 * Each Corobot consists of
 * <ul>
 *   <li>Main controller:
 *   <a href="https://learn.sparkfun.com/tutorials/pro-micro--fio-v3-hookup-guide">
 *     Pro Micro board</a>, which is compatible with 
 *     <a href="https://www.arduino.cc/en/Main/ArduinoBoardMicro">Arduino Micro</a>
 *   </li>
 *   <li>Two front wheels attached to two DC motors whose powers and
 *     directions are independently controllable</li>
 *   <li>One basic I/O board with 3 LEDs and a push switch</li>
 *   <li>Three infrared line trackers</li>
 *   <li>One color sensor: 
 *     <a href="https://www.adafruit.com/products/1356">Adafruit Flora Color
 *     Sensor TCS34725</a></li>
 *   <li>One accelerometer:
 *     <a href="http://www.arduino.in.th/product/140/3-axis-accelerometer-module-adxl335">ADXL335</a></li>
 *   <li>One Bluetooth communication module: 
 *     <a href="https://www.fasttech.com/product/1129200-jy-mcu-hc-05-bluetooth-wireless-serial-port-module">HC-05</a>,
 *     already paired with the other
 *     Corobot in the same set</li>
 *   <li>One power bank</li>
 * </ul>
 * \image html top.png
 * \image latex top.png "Components viewed from top" width=4in
 * <br/>
 * \image html bottom.png
 * \image latex bottom.png "Components viewed from bottom" width=4in
 *
 * <h2>The Library</h2>
 * The KU Corobot Arduino library can be downloaded from 
 * https://ecourse.cpe.ku.ac.th/corobot/Corobot.zip
 *
 */
#include <limits.h>
#include <avr/pgmspace.h>
#include "Corobot.h"

#define PWM_LT_PIN    6  ///< PWM output pin for the left motor
#define DIR_LT_PIN    7  ///< Direction control pin for the left motor
#define PWM_RT_PIN    9  ///< PWM output pin for the right motor
#define DIR_RT_PIN    8  ///< Direction control pin for the right motor

#define TRACK_LT_PIN  4  ///< Input pin for the left line tracker
#define TRACK_RT_PIN  5  ///< Input pin for the right line tracker
#define TRACK_CT_PIN  21 ///< Input pin for the center line tracker

#define ACCEL_X_PIN   A0 ///< Analog input for accelerometer's x value
#define ACCEL_Y_PIN   A1 ///< Analog input for accelerometer's y value
#define ACCEL_Z_PIN   A2 ///< Analog input for accelerometer's z value

#define LED_RED_PIN     15 ///< Output pin for the red LED on the basic I/O board
#define LED_YELLOW_PIN  14 ///< Output pin for the yellow LED on the basic I/O board
#define LED_GREEN_PIN   16 ///< Output pin for the green LED on the basic I/O board
#define SWITCH_PIN      10 ///< Input pin for the push switch on the basic I/O board

/// Default raw color sensor readings on black floor
#define DEFAULT_BLACK_RAW   383,323,226,1011

/// Default raw color sensor readings on white floor
#define DEFAULT_WHITE_RAW   3743,3647,2429,10866

/// Default raw color sensor readings on red floor
#define DEFAULT_RED_RAW     2360,541,443,3377

/// Default raw color sensor readings on green floor
#define DEFAULT_GREEN_RAW   741,1482,714,3228

/// Default raw color sensor readings on blue floor
#define DEFAULT_BLUE_RAW    511,906,1254,2883

/// Default raw color sensor readings on yellow floor
#define DEFAULT_YELLOW_RAW  4521,3597,1291,10385

/// Default raw color sensor readings on magenta floor
#define DEFAULT_MAGENTA_RAW 1974,642,798,3506

/// Default x-axis acceleration value when the robot is leveled
#define DEFAULT_ACCEL_CENTER_X  349

/// Default y-axis acceleration value when the robot is leveled
#define DEFAULT_ACCEL_CENTER_Y  345

/// Default z-axis acceleration value when the robot is leveled
#define DEFAULT_ACCEL_CENTER_Z  419

#define ACCEL_READ_REPEATS   5 ///< Number of readings for each acceleration inquiry

#define DEBOUNCING_DELAY    10 ///< Delay (in ms) for switch debouncing

static const char* COLOR_NAMES[] = {
  "BLACK"  ,
  "WHITE"  ,
  "RED"    ,
  "GREEN"  ,
  "BLUE"   ,
  "YELLOW" ,
  "MAGENTA",
  "UNKNOWN",
};

CorobotC Corobot; ///< Main object for accessing and controlling the robot

/******************************************************//**
 * Construct a Corobot object.
 */
CorobotC::CorobotC():
  _tcs(TCS34725_INTEGRATIONTIME_50MS,TCS34725_GAIN_1X)
{
}

/******************************************************//**
 * Initialize Corobot.
 */
void CorobotC::init()
{
  //////////////////////////////////
  // Initialize LEDs and switch
  //////////////////////////////////
  pinMode(LED_RED_PIN,OUTPUT);
  pinMode(LED_YELLOW_PIN,OUTPUT);
  pinMode(LED_GREEN_PIN,OUTPUT);
  pinMode(SWITCH_PIN,INPUT_PULLUP);
  setRightLed(false);
  setLeftLed(false);

  //////////////////////////////////
  // Initialize bluetooth
  //////////////////////////////////
  Serial1.begin(9600);

  //////////////////////////////////
  // Initialize line trackers
  //////////////////////////////////
  pinMode(TRACK_LT_PIN,INPUT);
  pinMode(TRACK_RT_PIN,INPUT);
  pinMode(TRACK_CT_PIN,INPUT);

  //////////////////////////////////
  // Initialize motors
  //////////////////////////////////
  pinMode(PWM_LT_PIN,OUTPUT);
  pinMode(PWM_RT_PIN,OUTPUT);
  pinMode(DIR_LT_PIN,OUTPUT);
  pinMode(DIR_RT_PIN,OUTPUT);
  calibrateMotors(100,100);
  setLeftMotor(FORWARD,0);
  setRightMotor(FORWARD,0);

  //////////////////////////////////
  // Initialize color sensor 
  //////////////////////////////////
  calibrateColor(
    DEFAULT_BLACK_RAW,
    DEFAULT_WHITE_RAW,
    DEFAULT_RED_RAW,
    DEFAULT_GREEN_RAW,
    DEFAULT_BLUE_RAW,
    DEFAULT_YELLOW_RAW,
    DEFAULT_MAGENTA_RAW);
  // Perform dummy reading (somehow first reading always wrong)
  uint16_t rr,rg,rb,rc;
  getRawColor(&rr,&rg,&rb,&rc);

  //////////////////////////////////
  // Initialize accelerometer
  //////////////////////////////////
  pinMode(ACCEL_X_PIN,INPUT);
  pinMode(ACCEL_Y_PIN,INPUT);
  pinMode(ACCEL_Z_PIN,INPUT);
  calibrateAccel(
      DEFAULT_ACCEL_CENTER_X,
      DEFAULT_ACCEL_CENTER_Y,
      DEFAULT_ACCEL_CENTER_Z);
}

/******************************************************//**
 * Calibrate motors.
 * The power percentages will be used to adjust the actual motors' powers when
 * calling setLeftMotor(), setRightMotor(), and setMotors()
 *
 * @param leftMotorPercent  Power percentage adjustment to be applied to the
 *                          left motor
 *
 * @param rightMotorPercent Power percentage adjustment to be applied to the
 *                          right motor
 */
void CorobotC::calibrateMotors(uint8_t leftMotorPercent,uint8_t rightMotorPercent)
{
  _leftMotorPercent = min(leftMotorPercent,100);
  _rightMotorPercent = min(rightMotorPercent,100);
}

/******************************************************//**
 * Set powers and directions for both motors.
 * If calibrateMotors() has been previously called, the actual power of each
 * motor will be normalized by the corresponding power percentage.
 *
 * @param dir_l FORWARD|BACKWARD Direction of the left motor
 * @param dir_r FORWARD|BACKWARD Direction of the right motor
 * @param power_l 0-100 Power of the left motor
 * @param power_r 0-100 Power of the right motor
 *
 */
void CorobotC::setMotors(
    Direction dir_l,uint8_t power_l,
    Direction dir_r,uint8_t power_r) 
{
  setLeftMotor(dir_l,power_l);
  setRightMotor(dir_r,power_r);
}

/******************************************************//**
 * Set power and direction for the left motor.
 * @param dir FORWARD|BACKWARD Direction of the left motor
 * @param power 0-100 Power of the left motor
 *
 * If calibrateMotors() has been used,the actual power of the motor will be
 * adjusted by the previously specified power percentage.
 */
void CorobotC::setLeftMotor(Direction dir,uint8_t power) const 
{
  digitalWrite(DIR_LT_PIN,dir);
  power = (255L*power*_leftMotorPercent)/100/100;
  analogWrite(PWM_LT_PIN,(dir==FORWARD)?255-power:power);
}

/******************************************************//**
 * Set power and direction for the right motor.
 * @param dir FORWARD|BACKWARD Direction of the right motor
 * @param power 0-100 Power of the right motor
 *
 * If calibrateMotors() has been used,the actual power of the motor will be
 * adjusted by the previously specified power percentage.
 */
void CorobotC::setRightMotor(Direction dir,uint8_t power) const 
{
  digitalWrite(DIR_RT_PIN,dir);
  power = (255L*power*_rightMotorPercent)/100/100;
  analogWrite(PWM_RT_PIN,(dir==FORWARD)?255-power:power);
}

/******************************************************//**
 * Check whether the left eye (the left line tracker) is seeing non-black color.
 * \image html eye-left.png
 * \image latex eye-left.png "Left eye position" width=4in
 * @retval true  the left eye is on top of a non-black color floor
 * @retval false the left eye is on top of a black floor
 */
bool CorobotC::getLeftEye() const 
{
  return digitalRead(TRACK_LT_PIN) == HIGH;
}

/******************************************************//**
 * Check whether the right eye (the right line tracker) is seeing non-black
 * color.
 * \image html eye-right.png
 * \image latex eye-right.png "Right eye position" width=4in
 * @retval true  the right eye is on top of a non-black color floor
 * @retval false the right eye is on top of a black floor
 */
bool CorobotC::getRightEye() const 
{
  return digitalRead(TRACK_RT_PIN) == HIGH;
}

/******************************************************//**
 * Check whether the center eye (the center line tracker) is seeing non-black color.
 * \image html eye-center.png
 * \image latex eye-right.png "Center eye position" width=4in
 * @retval true  the center eye is on top of a non-black color floor
 * @retval false the center eye is on top of a black floor
 */
bool CorobotC::getCenterEye() const 
{
  return digitalRead(TRACK_CT_PIN) == HIGH;
}

/******************************************************//**
 * Calibrate the color sensor.
 * These values will be used by the getColor() method for computing closest
 * matching color.
 *
 * @param black_r,black_g,black_b,black_c 
 *        Raw color sensor readings when the sensor is on black color
 * @param white_r,white_g,white_b,white_c
 *        Raw color sensor readings when the sensor is on white color
 * @param red_r,red_g,red_b,red_c
 *        Raw color sensor readings when the sensor is on red color
 * @param green_r,green_g,green_b,green_c
 *        Raw color sensor readings when the sensor is on green color
 * @param blue_r,blue_g,blue_b,blue_c
 *        Raw color sensor readings when the sensor is on blue color
 * @param yellow_r,yellow_g,yellow_b,yellow_c
 *        Raw color sensor readings when the sensor is on yellow color
 * @param magenta_r,magenta_g,magenta_b,magenta_c
 *        Raw color sensor readings when the sensor is on magenta color
 */
void CorobotC::calibrateColor(
          uint16_t   black_r, uint16_t   black_g, uint16_t   black_b, uint16_t   black_c,
          uint16_t   white_r, uint16_t   white_g, uint16_t   white_b, uint16_t   white_c,
          uint16_t     red_r, uint16_t     red_g, uint16_t     red_b, uint16_t     red_c,
          uint16_t   green_r, uint16_t   green_g, uint16_t   green_b, uint16_t   green_c,
          uint16_t    blue_r, uint16_t    blue_g, uint16_t    blue_b, uint16_t    blue_c,
          uint16_t  yellow_r, uint16_t  yellow_g, uint16_t  yellow_b, uint16_t  yellow_c,
          uint16_t magenta_r, uint16_t magenta_g, uint16_t magenta_b, uint16_t magenta_c)
{
  uint16_t tmp[UNKNOWN][4];
  tmp[0][0]=black_r;   tmp[0][1]=black_g;   tmp[0][2]=black_b;   tmp[0][3]=black_c;
  tmp[1][0]=white_r;   tmp[1][1]=white_g;   tmp[1][2]=white_b;   tmp[1][3]=white_c;
  tmp[2][0]=red_r;     tmp[2][1]=red_g;     tmp[2][2]=red_b;     tmp[2][3]=red_c;
  tmp[3][0]=green_r;   tmp[3][1]=green_g;   tmp[3][2]=green_b;   tmp[3][3]=green_c;
  tmp[4][0]=blue_r;    tmp[4][1]=blue_g;    tmp[4][2]=blue_b;    tmp[4][3]=blue_c;
  tmp[5][0]=yellow_r;  tmp[5][1]=yellow_g;  tmp[5][2]=yellow_b;  tmp[5][3]=yellow_c;
  tmp[6][0]=magenta_r; tmp[6][1]=magenta_g; tmp[6][2]=magenta_b; tmp[6][3]=magenta_c;

  // Compute range of each color channel: r,g,b,c
  _color_range[0][0] = USHRT_MAX;
  _color_range[1][0] = USHRT_MAX;
  _color_range[2][0] = USHRT_MAX;
  _color_range[3][0] = USHRT_MAX;
  _color_range[0][1] = 0;
  _color_range[1][1] = 0;
  _color_range[2][1] = 0;
  _color_range[3][1] = 0;
  for (uint8_t i = 0; i < UNKNOWN; i++) {
    for (uint8_t j = 0; j < 4; j++) {
      //_color_range[j][0] = min(_color_range[j][0],tmp[i][j]);
      //_color_range[j][1] = max(_color_range[j][1],tmp[i][j]);
      _color_range[j][0] = tmp[0][j];
      _color_range[j][1] = tmp[1][j];
    }
  }

  // Normalize color values based on channel's range
  for (uint8_t i = 0; i < UNKNOWN; i++) {
    for (uint8_t j = 0; j < 4; j++) {
      uint16_t minVal = _color_range[j][0];
      uint16_t maxVal = _color_range[j][1];
      _color_map[i][j] = constrain(map(tmp[i][j],minVal,maxVal,0,255),0,255);
    }
  }
}

/******************************************************//**
 * Get raw color readings from the color sensor.
 * @param r Pointer to 16-bit unsigned integer to store raw red intensity
 *          reading
 * @param g Pointer to 16-bit unsigned integer to store raw green intensity
 *          reading
 * @param b Pointer to 16-bit unsigned integer to store raw blue intensity
 *          reading
 * @param c Pointer to 16-bit unsigned integer to store raw clear intensity
 *          reading
 */
void CorobotC::getRawColor(uint16_t *r,uint16_t *g,uint16_t *b,uint16_t *c)
{
  _tcs.getRawData(r,g,b,c);
}

/******************************************************//**
 * Return the current color.
 * Color recognition is done by comparing the current raw color readings with
 * the calibrated values indicated by the most recent call to
 * calibrateColor().  The closest color is returned.
 * \image html color-sensor.png
 * \image latex color-sensor.png "Color sensor position" width=4in
 *
 * @return Color code representing the recognized color on the floor.
 *
 */
Color CorobotC::getColor()
{
  uint16_t rr,rg,rb,rc;
  getRawColor(&rr,&rg,&rb,&rc);
  return getColor(rr,rg,rb,rc);
}

/******************************************************//**
 * Estimate the color from the given raw values.
 * @param rr Raw red intensity value
 * @param rg Raw green intensity value
 * @param rb Raw blue intensity value
 * @param rc Raw clear intensity value
 * @return Corresponding color code recognized from the specified raw values
 */
Color CorobotC::getColor(uint16_t rr,uint16_t rg,uint16_t rb,uint16_t rc) const
{
  uint8_t color = UNKNOWN;
  uint16_t mindist = USHRT_MAX;

  for (uint8_t i = 0; i < UNKNOWN; i++) {
    int16_t mr = _color_map[i][0];
    int16_t mg = _color_map[i][1];
    int16_t mb = _color_map[i][2];
    int16_t mc = _color_map[i][3];
    int16_t nr = constrain(map(rr,_color_range[0][0],_color_range[0][1],0,255),0,255);
    int16_t ng = constrain(map(rg,_color_range[1][0],_color_range[1][1],0,255),0,255);
    int16_t nb = constrain(map(rb,_color_range[2][0],_color_range[2][1],0,255),0,255);
    int16_t nc = constrain(map(rc,_color_range[3][0],_color_range[3][1],0,255),0,255);
    uint16_t dist;

    dist  = (nr-mr)*(nr-mr);
    dist += (ng-mg)*(ng-mg);
    dist += (nb-mb)*(nb-mb);
    //dist += (nc-mc)*(nc-mc);

    if (dist < mindist) {
      color = i;
      mindist = dist;
    }
  }
  return (Color) color;
}

/******************************************************//**
 * Translate the given color code into a textual name.
 * @param c Color code
 * @return String containing the corresponding textual color name
 */
const char* CorobotC::getColorName(Color c) const
{
  if (c > UNKNOWN)
    return NULL;
  else
    return COLOR_NAMES[c];
}

/******************************************************//**
 * Calibrate the accelerometer as "leveled" using the specified acceleration values.
 * @param ax Acceleration value on the x-axis
 * @param ay Acceleration value on the y-axis
 * @param az Acceleration value on the z-axis
 */
void CorobotC::calibrateAccel(uint16_t ax,uint16_t ay,uint16_t az)
{
  _accel_center_x = ax;
  _accel_center_y = ay;
  _accel_center_z = az;
}

/******************************************************//**
 * Calibrate the accelerometer as "leveled" using the current readings.
 */
void CorobotC::calibrateAccel() 
{
  getRawAccel(&_accel_center_x,&_accel_center_y,&_accel_center_z);
}

/******************************************************//**
 * Obtain the raw accelerometer's readings.
 * @param ax Pointer to a location for storing x-axis acceleration
 * @param ay Pointer to a location for storing y-axis acceleration
 * @param az Pointer to a location for storing z-axis acceleration
 */
void CorobotC::getRawAccel(uint16_t *ax,uint16_t *ay,uint16_t *az) 
{
  uint16_t sx,sy,sz;

  sx = sy = sz = 0;
  for (uint8_t i = 0; i < ACCEL_READ_REPEATS; i++) {
    sx += analogRead(ACCEL_X_PIN);
    sy += analogRead(ACCEL_Y_PIN);
    sz += analogRead(ACCEL_Z_PIN);
  }
  *ax = sx/ACCEL_READ_REPEATS;
  *ay = sy/ACCEL_READ_REPEATS;
  *az = sz/ACCEL_READ_REPEATS;
}

/******************************************************//**
 * Calculate the rotation angle of the Corobot along the pitch axis using the
 * specified raw acceleration values.
 * @param ax,ay,az Accelerometer reading on the X, Y and Z axes, respectively
 * @return Pitch angle in degrees, similar to getRoll()
 */
int8_t CorobotC::getPitch(uint16_t ax,uint16_t ay,uint16_t az) const 
{
  return constrain((int)ay-(int)_accel_center_y,-20,20);
}

/******************************************************//**
 * Return the current rotation angle of the Corobot along the pitch axis.
 * \image html pitch.png
 * \image latex pitch.png "Pitch angle interpretation" width=4in
 * @return Pitch angle in degrees between -20 and 20
 * @retval 0 The Corobot is leveled with respect to the pitch axis
 * @retval >0 The Corobot's nose is pointing downward
 * @retval <0 The Corobot's nose is pointing upward
 */
int8_t CorobotC::getPitch() const 
{
  return getPitch(0,analogRead(ACCEL_Y_PIN),0);
}

/******************************************************//**
 * Calculate the rotation angle of the Corobot along the roll axis using the
 * specified raw acceleration values.
 * @param ax,ay,az Accelerometer reading on the X, Y and Z axes, respectively
 * @return Roll angle in degrees, similar to getPitch()
 */
int8_t CorobotC::getRoll(uint16_t ax,uint16_t ay,uint16_t az) const 
{
  return constrain((int)ax-(int)_accel_center_x,-20,20);
}

/******************************************************//**
 * Return the current rotation angle of the Corobot along the roll axis.
 * \image html roll.png
 * \image latex roll.png "Roll angle interpretation" width=4in
 * @return Roll angle in degrees between -20 and 20
 * @retval 0 The Corobot is leveled with respect to the roll axis
 * @retval >0 The Corobot is banking right
 * @retval <0 The Corobot is banking left
 */
int8_t CorobotC::getRoll() const 
{
  return getRoll(analogRead(ACCEL_X_PIN),0,0);
}

/******************************************************//**
 * Set the state of the specified LED on the basic I/O board.
 * \image html leds.jpg
 * \image latex leds.jpg "LEDs position on the basic I/O board" width=2in
 * @param c Color code of the LED, which can be either RED, YELLOW, or GREEN.
 * Requests to set other colors will be silently ignored.
 * @param state <b>true</b> to turn the specified LED on; <b>false</b> to turn off
 *
 */
void CorobotC::setLed(Color c,bool state) 
{
  if (c == RED)
    digitalWrite(LED_RED_PIN,state);
  else if (c == YELLOW)
    digitalWrite(LED_YELLOW_PIN,state);
  else if (c == GREEN)
    digitalWrite(LED_GREEN_PIN,state);
}

/******************************************************//**
 * Set the LEDs on the basic I/O board to represent the binary of the specified
 * value.
 * \image html leds.jpg
 * @param value Value to be interpreted as binary.  Only the 3 least
 * significant bits will be used.
 */
void CorobotC::setLedValue(uint8_t value) 
{
  setLed(RED,(value&1)>>0);
  setLed(YELLOW,(value&2)>>1);
  setLed(GREEN,(value&4)>>2);
}

/******************************************************//**
 * Set the state of the right LED (the green TX LED) on the controller board.
 * \image html led-right.jpg
 * \image latex led-right.jpg "Right LED position on the controller board" width=2in
 * @param state <b>true</b> to turn the LED on; <b>false</b> to turn off
 */
void CorobotC::setRightLed(bool state)
{
  if (state)
    TXLED0;
  else
    TXLED1;
}

/******************************************************//**
 * Set the state of the left LED (the yellow RX LED) on the controller board.
 * \image html led-left.jpg
 * \image latex led-left.jpg "Left LED position on the controller board" width=2in
 * @param state <b>true</b> to turn the LED on; <b>false</b> to turn off
 */
void CorobotC::setLeftLed(bool state)
{
  if (state)
    RXLED0;
  else
    RXLED1;
}

/******************************************************//**
 * Wait until the switch on the basic I/O board is pressed.
 * If the switch is currently pressed, this function will return immediately
 * \image html switch.jpg
 * \image latex switch.jpg "Switch position on the basic I/O board" width=2in
 */
void CorobotC::waitSwitchPress() const
{
  while (digitalRead(SWITCH_PIN) != LOW)
    ;
  delay(DEBOUNCING_DELAY);
}

/******************************************************//**
 * Wait until the switch on the basic I/O board is released.
 * If the switch is currently released, this function will return immediately
 * \image html switch.jpg
 */
void CorobotC::waitSwitchRelease() const
{
  while (digitalRead(SWITCH_PIN) != HIGH)
    ;
  delay(DEBOUNCING_DELAY);
}

/******************************************************//**
 * Check whether the switch on the basic I/O board is currently pressed.
 * \image html switch.jpg
 * @retval true   the switch is pressed.
 * @retval false  the switch is not pressed.
 */
bool CorobotC::switchPressed() const
{
  return digitalRead(SWITCH_PIN) == LOW;
}

/******************************************************//**
 * Wait until the remote Corobot is connected via Bluetooth.
 * Make sure that <b>both Corobots</b> call this same function during setup().
 * While calling this function, the special character '#' will be sent
 * continuously to the remote Corobot.  At the same time, the local Corobot is
 * also expecting a '#' from the remote.
 */
void CorobotC::waitRemote() const
{
  // Wait until receiving '#' from the friend
  while (Corobot.remote.read() != '#') {
    // Keep sending '#' to the friend
    Corobot.remote.print('#');
    delay(100);
  }

  // Read until no longer '#' is found
  while (Corobot.remote.peek() == '#') {
    Corobot.remote.read();

    // Still keep sending '#' to the friend
    Corobot.remote.print('#');
  }
}
