/*
A "Blink Bot" is a robot controlled by the blinking of areas on an LCD screen,
e.g. the screen of a cheap old cell phone. After years of looking, that seems to
be the best way to interface the amazing power per cost of those devices to robotics.
https://hackaday.io/project/184720-web-smart-phone-screen-blink-bot
seriously, read that page before you scream? 

The idea here is to use a simple RasPi Pico to interface a cell phone to a roomba
making a telepresense robot out of scrap. The phone can load a web page (or cashe it)
for autonomous control (with help from web apis) 
or connect to a video meeting service so a human can drive the robot around with a
set of black and white cards held up to the camera. 

Another (probably better) option is to use this project:
https://github.com/pkyanam/ArduRoomba
and add the light sensing to that. 
*/

#include <Adafruit_NeoPixel.h>
#define LED 4
/* Roomba interface. 
https://cdn.hackaday.io/files/1747287475562752/Roomba_SCI_manual.pdf
Pin   Name  Description
 1    Vpwr  Roomba battery + (unregulated)
 2    Vpwr  Roomba battery + (unregulated)
 3    RXD   0 – 5V Serial input to Roomba from Pico pin 1 GP0 UART0 Serial1
 4    TXD   0 – 5V Serial output from Roomba to Pico pin 2 GP1 UART0 Serial1
 5    DD    Device Detect input: Active low 500ms to wake up Roomba
 6    GND   Roomba battery ground
 7    GND   Roomba battery ground
*/
#define ROOMBA_DD 3 //DIN pin 4 io pin for Device Detect output
// https://datasheets.raspberrypi.com/pico/Pico-2-Pinout.pdf

//Modes: off, passive, safe, full. Wait 20ms after a mode change
#define ROOMBA_CMD_START 128 //moves from off to passive mode
#define ROOMBA_CMD_CTRL 130 //move from passive to safe: cliff and wheel-drop safety
#define ROOMBA_CMD_FULL 132 //moves from safe to full mode: NO cliff or wheel-drop safety
#define ROOMBA_CMD_SAFE 131 //moves from full to safe mode
#define ROOMBA_CMD_POWER 133 //moves from safe or full to passive, goes to sleep
#define ROOMBA_CMD_DRIVE 137 //plus 2 byte speed and 2 byte turn radius. Safe or full mode only
#define ROOMBA_CMD_DOCK 143 //find the power dock and re-charge. Any mode

void roomba_drive(char * seq, int16_t vel, int16_t rad) {
/*
Serial sequence: [137] [Velhi] [Vello] [Radhi] [Radlo]
Drive data bytes 1 and 2: Velocity (-500 – 500 mm/s)
Drive data bytes 3 and 4: Radius (-2000 – 2000 mm)
Special cases: Straight = 32768 = hex 8000
Turn in place clockwise = -1
Turn in place counter-clockwise = 1
Example:
To drive in reverse at a velocity of -200 mm/s while turning at
a radius of 500mm, you would send the serial byte sequence
[137] [255] [56] [1] [244].
Velocity = -200 = hex FF38 = [hex FF] [hex 38] = [255] [56]
Radius = 500 = hex 01F4 = [hex 01] [hex F4] = [1] [244]
*/
  constrain(vel, -500, 500); //def max and min velocity in mm/s
  constrain(rad, -2000, 2000); //def max and min radius in mm
  if (0==rad) rad = 0x8000; //special case for straight
  seq[0] = ROOMBA_CMD_DRIVE;
  seq[1] = vel >> 8;
  seq[2] = vel & 0xFF;
  seq[3] = rad >> 8;
  seq[4] = rad & 0xFF;
}

//https://www.instructables.com/Controlling-a-Roomba-Robot-With-Arduino-and-Androi/

void roomba_init() {
  pinMode(ROOMBA_DD, OUTPUT);
  digitalWrite(ROOMBA_DD, LOW);
  delay(500+100); //wakey, wakey!

}
char buf[5];
//Adafruit_NeoPixel pixels(1, 25, NEO_GRB + NEO_KHZ800);

void setup() {
  //pixels.begin();
  //pixels.clear();
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // wait for serial port to connect. Needed for native USB
  }
  Serial.println("Ready");
  //Serial1.setRX(ROOMBA_TXD); Only on Earl Powell 
  //Serial1.setTX(ROOMBA_RXD);
  Serial1.begin(57600); //default speed for roomba serial
  roomba_init();
}


#define DEAD_ZONE 100 //don't move if we are close to not moving
#define LITE_MID 250 //ADC reading with lite midrange

void loop() {
  //the ADC reading goes down as lite level goes up
  int lite_right = 1024 - analogRead(A0);
  int lite_left = 1024 - analogRead(A1);

  int left_track = lite_left - LITE_MID;
  if (left_track < DEAD_ZONE) left_track = 0;
  int right_track = lite_right - LITE_MID;
  if (right_track < DEAD_ZONE) right_track = 0;
  int vel = left_track + right_track;
  int rad = left_track - right_track;

  roomba_drive(buf, vel, rad);
  Serial1.write(buf,5);

  Serial.print(lite_right);
  Serial.print(",");
  Serial.print(lite_left);
  Serial.print(",");
  Serial.print(buf[1]*256+buf[2]);
  Serial.print(",");
  Serial.print(buf[3]*256+buf[4]);
  Serial.print("\n");

  delay(500); 
  digitalWrite(LED, HIGH);
  digitalWrite(LED_BUILTIN, LOW);
  //pixels.setPixelColor( 0, pixels.Color(150, 150, 150));
  //pixels.show();
  delay(500); 
  digitalWrite(LED, LOW);
  digitalWrite(LED_BUILTIN, HIGH);
  //pixels.setPixelColor( 0, pixels.Color(0, 0, 0));
  //pixels.show();

}