/**
 * @file robot.h
 * @brief Robot Class for the Arduino Mega 2650
 * @author David Such
 */

#ifndef robot_h
#define robot_h

#include <Arduino.h>

class Robot
{
public:
    /*
     * @brief Class constructor.
     */
    Robot():
    leftMotor(pin_LW_Servo), rightMotor(pin_RW_Servo),
    frontSonar(pin_FrontPing_Sensor, pin_FrontPing_Sensor, MAX_DISTANCE),
    distanceAverage(MAX_DISTANCE), frontServo(pin_FrontPing_Servo),
    synthesizer(pin_EMIC_SerialRx, pin_EMIC_SerialTx), realTimeClock()
    {

    }

    /*
     * @brief Initialize the robot state.
     */
    void begin()
    {
        leftMotor.begin();
        rightMotor.begin();
        frontServo.begin();
        synthesizer.begin();
        compass.begin();

        Wire.begin();           // Join I2C bus (address optional for master)

        synthesizer.speakMessage("AEVA one, start up sequence in progress.");

        delay(HOLD_OFF_TIME);
        startTime = millis();
        endTime = millis() + RUN_TIME * 1000;

        frontServo.scan();
        move(AHEAD_SLOW);
        delay(2000);

        sendMessage(address_I2C_Logging, "Master I2C bus is up.");
        synthesizer.speakMessage("Start up sequence complete. Awaiting command.");

        if (realTimeClock.available()) {
            sendMessage(address_I2C_Logging, "RTC connected.");
            realTimeClock.begin();
            realTimeClock.updateMsgArrayWithTime(_msgArray);
            sendMessage(address_I2C_Logging, _msgArray);
            realTimeClock.updateMsgArrayWithDate(_msgArray);
            sendMessage(address_I2C_Logging, _msgArray);
        } else {
            sendMessage(address_I2C_Logging, "RTC unavailable.");
        }

        if (compass.available()) {
            compass.updateMsgArrayWithHeading(_msgArray);
            sendMessage(address_I2C_Logging, _msgArray);
        } else {
            sendMessage(address_I2C_Logging, "Compass unavailable.");
        }
    }

    /*
     * @brief Move left motor at speed (-500 to 500).
     */
    void moveLeft(int speed) {
        leftMotor.moveAtSpeed(speed);
    }

    /*
     * @brief Move right motor at speed (-500 to 500).
     */
    void moveRight(int speed) {
        rightMotor.moveAtSpeed(speed);
    }

    /*
     * @brief Move both motors at speed (-500 to 500).
     */
    void move(int speed) {
        leftMotor.moveAtSpeed(speed);
        rightMotor.moveAtSpeed(speed);
        state = stateMoving;
    }

    /*
     * @brief Stop both motors.
     */
    void stop() {
        leftMotor.stop();
        rightMotor.stop();
        state = stateStopped;
    }

    bool moving()
    {
        return (state == stateMoving);
    }

    bool turning()
    {
        return (state == stateTurning);
    }

    bool stopped()
    {
        return (state == stateStopped);
    }

    bool doneRunning(unsigned long currentTime)
    {
        return (currentTime >= endTime);
    }

    bool obstacleAhead(unsigned int distance)
    {
        return (distance <= MIN_DISTANCE);
    }

    bool turn(unsigned long currentTime)
    {
        if (random(2) == 0) {
            moveLeft(BACK_SLOW);
            moveRight(AHEAD_SLOW);
        }
        else {
            moveLeft(AHEAD_SLOW);
            moveRight(BACK_SLOW);
        }
        state = stateTurning;
        endStateTime = currentTime + random(500, 1000);
    }

    bool doneTurning(unsigned long currentTime, unsigned int distance)
    {
        if (currentTime >= endStateTime)
            return (distance > MIN_DISTANCE);

        return false;
    }

    /*
     * @brief Display text message on the logging OLED.
     *  Maximum message length is 32 chars including terminating char.
     *  Flash LED on pin 13 when sending message.
     */
    void sendMessage(byte address, char *msg)
    {
        digitalWrite(pin_LED, HIGH);
        Wire.beginTransmission(address);
        Wire.write(msg);
        Wire.endTransmission();
        digitalWrite(pin_LED, LOW);
    }

    /*
     * @brief Update the state of the robot based on input from sensor and remote control.
     *  Called repeatedly while the robot is in operation.
     */
    void run()
    {
        unsigned long currentTime = millis();
        unsigned long elapsedTime = currentTime - startTime;

        if (stopped())
            return;

        int distance = distanceAverage.add(frontSonar.getDistance());

        log("state: %d, currentTime: %ul, ", state, currentTime);
        log("distance: %d\n", distance);

        if (doneRunning(currentTime)) {
            stop();
            log("End of run time");
        }
        else if (moving()) {
            log("Moving, ");
            if (obstacleAhead(distance)) {
                turn(currentTime);
                log("Commencing turn, ");
            }
        }
        else if (turning()) {
            if (doneTurning(currentTime, distance)) {
                move(AHEAD_SLOW);
                log("Turn complete, ");
            }
        }
    }

private:
    HB25MotorControl leftMotor, rightMotor;
    Synthesizer synthesizer;
    DistanceSensor frontSonar;
    TurretServo frontServo;
    MovingAverage <unsigned int, 3> distanceAverage;
    RealTimeClock realTimeClock;
    Compass compass;

    enum state_t { stateStopped, stateMoving, stateTurning };
    state_t state;
    unsigned long startTime, endTime, endStateTime;
    char _msgArray[32];
};

#endif