#pragma config(Sensor, S1,     BRAKEMOTORS,         sensorTouch)
#pragma config(Sensor, S4,     CAPTURE,             sensorTouch)
#pragma config(Motor,  motorA,          POWER,         tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          TURN,          tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//
/////////////////////////////////////////////////////////////
//
// lsvccontrol.c
//
// Controller program for remote that drives LSVC2
//
// Copyright October 2011, Mark Crosbie
// mark@mastincrosbie.com
// http://www.mastincrosbie.com/Marks_LEGO_projects/LEGO_Projects.html
//
// Requires a Bluetooth to the LSVC2 car on line 1. The controller must
// initiate the BT connection as it is the master
//
// Connections
//	S1: Emergency Brake button
//	S4: Take photo button
//	A:  Speed
//	C:  Turn direction

#include "BTlibrary.h"

#include "lsvccommon.h"

// Bluetooth mailbox data
const int kMaxSizeOfMessage = 5;

void initialise() {
    bNxtLCDStatusDisplay = true;
	ConnectPort(LSVCNAME);
	nMotorEncoder[POWER] = 0;  //clear the  encoders
	nMotorEncoder[TURN] = 0;
}

void sendDataMsg(byte cmd, byte power, byte turn) {
	ubyte nXmitBuffer[kMaxSizeOfMessage] = {0x00, 0x00, 0x00, 0x00, 0x00}; // For NXT-G compatability, last byte of message must be zero because of string messsages.
	TFileIOResult nBTCmdErrorStatus;

	nXmitBuffer[0] = cmd;
	nXmitBuffer[1] = power;
	nXmitBuffer[2] = turn;

	if (bBTBusy)
	{
		return;
	}

	nBTCmdErrorStatus = cCmdMessageWriteToBluetooth(nXmitBuffer, kMaxSizeOfMessage, kQueueID);
	switch (nBTCmdErrorStatus)
	{
		case ioRsltSuccess:
		case ioRsltCommPending:
		break;

		case ioRsltCommChannelBad:
		default:
		break;
	}
	return;
}

int clip(int a, int min, int max, int deadzone) {

	if( a < min) {
		a = min;
	}
	if(a > max) {
		a = max;
	}

	if(abs(a) < deadzone)
	    a = 0;

	return a;
}

void centerSteering() {

	eraseDisplay();
	nxtDisplayCenteredTextLine(0, "Controller");

	nxtDisplayTextLine(1, "Center steering");
	nxtDisplayTextLine(2, "and power level");
    nxtDisplayTextLine(3, "Press orange");

    while(nNxtButtonPressed != ORANGEBUTTON) wait10Msec(1);

    nMotorEncoder[POWER] = 0;
    nMotorEncoder[TURN] = 0;

    wait10Msec(100);
    eraseDisplay();
}

//
// Returns 1 if wifi
// Returns 2 if cable
// Returns 3 if resume
int checkIfWifi() {

    eraseDisplay();

    nxtDisplayTextLine(1, "Choose connect");
    nxtDisplayTextLine(2, " <  Wifi");
    nxtDisplayTextLine(3, " >  Cable");
    nxtDisplayTextLine(4, "Orange: Resume");

    until(nNxtButtonPressed == -1) wait10Msec(1);

    while(true) {

        if(nNxtButtonPressed == LEFTARROW) {
            until(nNxtButtonPressed == -1) wait10Msec(1);
            return 1;
        }

        if(nNxtButtonPressed == RIGHTARROW) {
            until(nNxtButtonPressed == -1) wait10Msec(1);
            return 2;
        }

        if(nNxtButtonPressed == ORANGEBUTTON) {
            until(nNxtButtonPressed == -1) wait10Msec(1);
            return 3;
        }
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Read a message from the Bluetooth link. A message from the remote is composed of 4 bytes
// command-power-turn-zero
// Returns -1 if an error occurred
// Returns 0 if no message is waiting
// Returns >0 if a message was read
////////////////////////////////////////////////////////////////////////////////////////////////////////
int readDataMsg(int queueID, long &val) {

	const bool bWaitForReply = false;
	TFileIOResult nBTCmdRdErrorStatus;
	int nSizeOfMessage;
	ubyte nRcvBuffer[kMaxSizeOfMessage];
    int ret = -1;

	// Check to see if a message is available
	nSizeOfMessage = cCmdMessageGetSize(kQueueID);
	if (nSizeOfMessage <= 0) {
		wait1Msec(1);    // Give other tasks a chance to run
		ret=0;           // No message this time
	}

	if (nSizeOfMessage > kMaxSizeOfMessage)
        nSizeOfMessage = kMaxSizeOfMessage;

    nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, queueID);

    if (nBTCmdRdErrorStatus == ioRsltSuccess) {
  	    ret = 1;
  	    val = (long)nRcvBuffer[3] + ((long)nRcvBuffer[2] << 8) + ((long)nRcvBuffer[1] << 16) + ((long)nRcvBuffer[0] << 24);
	} else {
	    ret = -1;
    }
	return ret;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        Main Task
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

task main() {

    byte power, turn;
    long pictureIndex, longitude, latitude, utc;

    initialise();

	wait1Msec(1000);

	power = turn = 0;

	eraseDisplay();

	while(true) {
    	nxtDisplayCenteredTextLine(0, "Controller");
        nxtDisplayCenteredTextLine(1, "Orange: end");

        power = clip(nMotorEncoder[POWER] * 2, -100, 100, 20);
	    turn = clip(nMotorEncoder[TURN] * 2, -100, 100, 20);

	    if(nNxtButtonPressed == RIGHTARROW) {
	        centerSteering();
	    }

	    if(SensorValue[CAPTURE] == 1) {
	        sendDataMsg(SNAPSHOT, power, turn);
	        nxtDisplayCenteredTextLine(5, "CAPTURE");
	        wait10Msec(100);
	        nxtDisplayClearTextLine(5);

	        // debounce
	        while(SensorValue[CAPTURE] == 1) wait10Msec(1);
	    }

	    if(SensorValue[BRAKEMOTORS] == 1) {
	        sendDataMsg(BRAKE, power, turn);
	        nxtDisplayCenteredTextLine(5, "BRAKE MOTORS");
	        wait10Msec(100);
	        nxtDisplayClearTextLine(5);
	        power = 0;
	        // debounce
	        while(SensorValue[BRAKEMOTORS] == 1) wait10Msec(1);
	    }

	    if(nNxtButtonPressed == ORANGEBUTTON) {
	        // we are done with the capture session, so tell the car to finish up

	        int r = checkIfWifi();

	        if(r == 3) {
	            wait1Msec(250);
	            eraseDisplay();
	            continue;
	        }

	        if(r == 1) {
	            sendDataMsg(STARTWIFI, 0, 0);
	            wait1Msec(500);
            }

	        sendDataMsg(STOPCAPTURE, 0, 0);
	        nxtDisplayCenteredTextLine(5, "STOP CAPTURE");
	        wait10Msec(100);
	        nxtDisplayClearTextLine(5);
	        break;
	    }


	    nxtDisplayTextLine(2, "Power: %d   ", power);
	    nxtDisplayTextLine(3, "Turn: %d   ", turn);

	    if(readDataMsg(kPictureCountID, pictureIndex) > 0) {
	        nxtDisplayTextLine(4, "Index: %d   ", pictureIndex);
	    } else {
	        nxtDisplayTextLine(4, "Index: ---");
	    }

	    sendDataMsg(NOP, power, turn);

	    wait10Msec(10);
    }

    wait10Msec(10);

}