//Jesse Chand
//Team WU TANG: Tam Henry Le Nguyen, Denzel Alexander, Adrian Mendoza
//Dr. Sheila Tejada
//11 November 2012

//Urban Search and Rescue
//-----------------------

#include <iostream>
#include <cstdlib>
#include "Myro.h"
#include "math.h"
#include "stdio.h"
#include <string.h>

using namespace std;

//Inch forward by a bit
void crawlForward()
{
    robot.motors(1,1);
    wait(1);
    robot.motors(0,0);
}

//Inch backward by a bit
void crawlBackward()
{
    robot.motors(-1,-1);
    wait(1);
    robot.motors(0,0);
}

//Tell the user all the instructions.
void Intro() 
{
    cout << "Remote controlling robot. Press WASD to navigate, F to brake." << endl;
    cout << "Press P to take a picture." << endl;
    cout << "Press Q to alert the user with a beep." << endl;
    cout << "When you are done searching, press C to complete and view the slideshow." << endl;    
    
    cout << "Robot statistics:" << endl;
    cout << "Robot Obstacle Sensor Check: " << robot.getObstacle("center") << endl;
    cout << "Robot Light Sensor Check: " << robot.getLight("center") << endl;
    cout << "Robot IR Check: " << robot.getIR("left") << endl;
}

//Movement Control
void remoteControl()
{
    //This character stores the last command the user inputted.
    //WASD to move, Q to brake.
    
    char index = 'n';
    char response = 'z'; 
    
    double timer = currentTime();
    
    PicturePtr pLibrary[100];  //Original pictures
    PicturePtr Found[100];     //Modified pictures
    int imgindex = 0;          //Number of pictures taken
    int foundIndex = 0;        //Number of found robots
    
    const int WIDTH = 256;
    const int HEIGHT = 192;
    
    int x1, x2, y1, y2;        //Four points define the user-inputted rectangle
    
    Intro();
    
    while(true) {
        
    cin >> index;
    
    //Move Forward
    if (index == 'w') {
        crawlForward();
        index = 'n';
    }
    //Alert Driver: Location
    if (index == 'q') {
        robot.beep(0.25,4000);
        robot.beep(0.25,4000,5000);
        robot.beep(0.25,4000);
        robot.beep(0.25,4000,5000);
        robot.beep(0.25,4000);
        robot.beep(0.25,4000,5000);
        robot.beep(0.25,4000);
        robot.beep(0.25,4000,5000);
    }
    //Turn Left 90 degrees
    else if (index == 'a') {
        robot.motors(-1,1);
        wait(0.78);
        robot.motors(0,0);
        index = 'n';
    }
    //Move Backward
    else if (index == 's') {
        crawlBackward();
        index = 'n';
    }
    //Turn Right 90 degrees
    else if (index == 'd') {
        robot.motors(1,-1);
        wait(0.78);
        robot.motors(0,0);
        index = 'n';
    }
    //Brake 
    else if (index == 'f') {
        robot.motors(0,0);
        wait(1);
        robot.motors(0,0);
        index = 'n';
    }
    //Take Picture 
    else if (index == 'p') {
        cout << "Taking Picture ..." << endl;
        robot.motors(0,0);
        timer = currentTime();
        
        pLibrary[imgindex] = robot.takePicture();
        
        //For convenience, draw a grid over the picture.
        //Create a blue grid over the image.
        for (int xx = 0; xx < WIDTH; xx += WIDTH/4) {
            for (int yy = 0; yy < HEIGHT; yy++) {
                setPixelColor(pLibrary[imgindex],xx,yy,0,0,255);
            }
        }
        for (int yy = 0; yy < HEIGHT; yy += HEIGHT/4) {
            for (int xx = 0; xx < WIDTH; xx++) {
                setPixelColor(pLibrary[imgindex],xx,yy,0,0,255);
            }
        }
        
        cout << "Displaying picture # " << (imgindex+1) << ". Rendering time: " << (currentTime() - timer) << "s." << endl;
        
        show(pLibrary[imgindex]);
        cout << "Is there a robot in this image? Press 'y' or 'n'." << endl;
        cin >> response;
        
        if (response == 'y') {
            cout << "Locate the robot. Enter x1, y1, x2, y2." << endl;
            Found[foundIndex] = pLibrary[imgindex]->clone();
            
            cin >> x1 >> y1 >> x2 >> y2;
            
            //Draw the rectangle in red.
            for (int xx = x1; xx < x2; xx++) {
                setPixelColor(Found[foundIndex],xx,y1,255,0,0);    //Upper edge
                setPixelColor(Found[foundIndex],xx,y2,255,0,0);    //Lower edge
            }
            for (int yy = y1; yy < y2; yy++) {
                setPixelColor(Found[foundIndex],x1,yy,255,0,0);    //Upper edge
                setPixelColor(Found[foundIndex],x2,yy,255,0,0);    //Lower edge
            }
            
            cout << "Displaying located robot ..." << endl;
            show(Found[foundIndex]);
            foundIndex++;
        }
        response = 'z';
        
        imgindex++;
        index = 'n';
        cout << "Picture has been edited. Returning to movement control." << endl;
    }
    //Slideshow
    else if (index == 'c') {
        cout << "Showing slideshow of images taken." << endl;
        
        for (int i = 0; i < imgindex; i++) {
            char name[9] = "picx.jpg";
            name[3] = i;
            savePicture(pLibrary[i], name);
            show(pLibrary[0]);
        }
        
        cout << "Showing slideshow of modified images." << endl;
        
        for (int i = 0; i < foundIndex; i++) {
            char name[11] = "foundx.jpg";
            name[5] = i;
            savePicture(Found[i], name);
            show(Found[i]);
        }
        
        cout << "Slideshow complete. Returning to movement control." << endl;
        
        index = 'n';
    }
        
    }
}

int main()
{
	connect("/dev/tty.StarvingArtist");
    cout << "Battery Level: " << (robot.getBattery() / 9 * 100) << "%." << endl;
    
    remoteControl();
    
    disconnect();
    return 0;
}