////////////////////////////////////////////////////////////////////////////////
// ROBOTC EV3 remote control demonstration program.
// This program receives messages from the host computer: Forward, Backward, Left and Right.
// It sends back the distance in cm measured by the ultrasonic sensor and the angle in degrees
// measured by the gyro sensor.
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Connection scheme:
// motorA: medium motor front lever
// motorB: large motor right wheel
// motorC: large motor left wheel
// motorD: -
// Sensor S1: Touch sensor
// Sensor S2: Gyro sensor
// Sensor S3: Color sensor
// Sensor S4: Ultrasonic sensor
////////////////////////////////////////////////////////////////////////////////





#include "EV3Mailbox.c"

task main()
{

    displayBigTextLine(0, "Started!");

    char msgBufIn[MAX_MSG_LENGTH];  // To contain the incoming message.
    char msgBufOut[MAX_MSG_LENGTH];  // To contain the outgoing message

    openMailboxIn("EV3_INBOX0");
    openMailboxOut("EV3_OUTBOX0");

    while (true)
    {
        // Read input message.
        // readMailboxIn() is non-blocking and returns "" if there is no message.
        readMailboxIn("EV3_INBOX0", msgBufIn);
        if (strcmp(msgBufIn, "Forward") == 0)
        {
            displayBigTextLine(0, "Forward");
            setMotorSync(motorB, motorC, 0, 20);

        }
        else if  (strcmp(msgBufIn, "Backward") == 0)
        {
            displayBigTextLine(0, "Backward");
            setMotorSync(motorB, motorC, 0, -20);
        }
        else if (strcmp(msgBufIn, "Left") == 0)
        {
            displayBigTextLine(0, "Left");
            setMotorSpeed(motorC, 0);
            setMotorSpeed(motorB, 20);
        }
        else if (strcmp(msgBufIn, "Right") == 0)
        {
            displayBigTextLine(0, "Right");
            setMotorSpeed(motorB, 0);
            setMotorSpeed(motorC, 20);
        }


        // Here fill your outgoing message handling.
        // This means sending the distance in cm measured by the ultrasonic sensor
        // and the angle in degrees measured by the gyro sensor.
        // Put both values in msgBufOut.
        // Use the format ?<distance> <angle>" (space between distance and angle) so that it can be read by the host program.
        // Create message string to store the message received.
        char msgBufIn[20];

        // Receive message from host computer.
        // readMailboxIn is a library function in EV3Mailbox.c.
        readMailboxIn("EV3_INBOX0", msgBufIn);

        // Drive forward if “Forward” is received.
        if (strcmp(msgBufIn, "Forward") == 0)
        {
            displayBigTextLine(0, "Forward");
            setMotorSync(motorB, motorC, 0, 20);

        }
        else if  (strcmp(msgBufIn, "Backward") == 0)
        {
            displayBigTextLine(0, "Backward");
            setMotorSync(motorB, motorC, 0, -20);
        }
        else if (strcmp(msgBufIn, "Left") == 0)
        {
            displayBigTextLine(0, "Left");
            setMotorSpeed(motorC, 0);
            setMotorSpeed(motorB, 20);
        }
        else if (strcmp(msgBufIn, "Right") == 0)
        {
            displayBigTextLine(0, "Right");
            setMotorSpeed(motorB, 0);
            setMotorSpeed(motorC, 20);
        }
        // Create message string to store the message to send.
        char msgBufOut[20];

        // Measure distance and angle.
        float dist = getUSDistance(S4);
        float angle = getGyroDegrees(S2);

        // Fill msgBufOut[20] with the two numbers and space in between.
        // %.1f means 1 digit after the decimal point.
        sprintf(msgBufOut, "%.1f %.1f", dist, angle);

        // Send message to host computer.
        // writeMailboxOut is a library function in EV3Mailbox.c.
        writeMailboxOut("EV3_OUTBOX0", msgBufOut);



        writeMailboxOut("EV3_OUTBOX0", msgBufOut);

        delay(100);  // Wait 100 ms to give host computer time to react.
    }
    closeMailboxIn("EV3_INBOX0");
    closeMailboxOut("EV3_OUTBOX0");
    return;

}