//************************************************************************************************************
// The following code uses a modified version of Nathan Labott's code from his senior capstone of Spring 2019.
// This includes functions, comments, and their respective implementation.
// To see the original version of this implementation, use the following URL link:
// http://compsci02.snc.edu/cs460/2019/labonw/uploads/1/2/4/1/124126046/maincsspring2019.zip
//************************************************************************************************************

// manual.c

#include "abdrive.h"
#include "ping.h"
#include "servo.h"
#include "simpletools.h"
#include "wifi.h"

void manual();

char cmd, direction;
int * cog;
int event, id, handler, navId;
int dist, ticksLeft, ticksRight = 0;

char grid[ROWS][COLUMNS];

int main() {  
  wifi_start(31, 30, 115200, WX_ALL_COM); 	// communication to bot is through WX Wi-Fi Module
  navId = wifi_listen(HTTP, "/path");     	// bot will listen on the same path created by the webpage
  
  cog = cog_run(manual, 128);				// begins manual function
}


void manual() {
  while (1) {
    wifi_poll(&event, &id, &handler);     // grab data from path
    
    if(event == 'P') { // POST request
	
      if (id == navId) {
        print("Incoming POST request\r");
        wifi_scan(POST, handler, "go%c", &cmd);
        print("go=%c\n", cmd);
      }
	  
    } else if (event == 'G') {  // GET REQUEST
        dist = ping_cm(8);  // check to see if the bot is okay to move
        
        drive_getTicks(&ticksLeft, &ticksRight);
       
        if(dist > 20 || direction == 'B') {	// distance > 20cm or the bot is stopped
          wifi_print(GET, handler, "%d%c%d%c%d%c%c", dist, ':', ticksLeft, ':', ticksRight, ':', 't');  //response string seperated by ':' delimiter
        } else {
            wifi_print(GET, handler, "%d%c%d%c%d%c%c", dist, ':', ticksLeft, ':', ticksRight, ':', 'f');
          }    
          
      }
	  
    if (dist > 20 || cmd == 'D') { // distance > 20cm or the bot is going backwards (if the bot cannot move forward)
		switch (cmd) {
			case 'F':
			drive_ramp(64, 64);		// drive forward at 1/2 the max speed
			break;
			
			case 'L':
			drive_ramp(0, 64);		// turn left (right wheel moves only)
			break;
			
			case 'R':
			drive_ramp(64, 0);		// turn right (left wheel moves only)
			break;
			
			case 'D':
			drive_speed(-32, -32);	// reverse at 1/4 the max speed
			break;
			
			case 'S':
			drive_ramp(0, 0);		// slow the bot to a stop (button release)
			break;
		}
    } else {
		drive_speed(0, 0);			// keeps the bot from moving w/o input
	}
  }	
}