/*
 * Serial Communication with Arduino
 * Author: Maximilian Schraysshuen and Nils Landmark
 */

// Anleitung um Arduino in VS Code einzurichten
// https://blog.devgenius.io/visual-studio-code-arduino-configuration-and-import-solution-a5f188a0cfd0

/*
 * Input parameter: 6-digit int <right.direction><right.speed><left.direction><left.speed>
 * Direction:
 * 1 = Release (off)
 * 2 = Forward
 * 3 = Backward
 * 00 to 99 Speeds, speed+1=speed in percentage
 */

// Includes =============================================================================
#include <Adafruit_MotorShield.h>

// Variables ============================================================================
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor* motor_left = AFMS.getMotor(1);
Adafruit_DCMotor* motor_right = AFMS.getMotor(2);
long inpt; // Input from computer parsed to long
int left_dir; // left motor direction
int left_speed_inpt; // left motor input value
int right_dir; // right motor direction
int right_speed_inpt; // right motor input value

int left_speed; // left motor speed value
int right_speed; // right motor speed value

int min_speed = 40; // minimal motor speed
int max_speed = 255; // maximum motor speed

bool motor_dir_left_reversed = true; // reversed left motor directions to ajust for cable direction
bool motor_dir_right_reversed = true; // reversed right motor directions to ajust for cable direction

unsigned long last_command_time; // timestamp in milli seconds of last input command
unsigned long max_time_diff = 400; // time in milli seconds without a new command to stop

// Functions ============================================================================
// setup ======================================================================
void setup()
{
    Serial.begin(115200); // Initializing the Serial Port to Baud rate 115200
    Serial.setTimeout(2);

    AFMS.begin(); // create with the default frequency 1.6KHz

    // Set the speed to start, from 0 (off) to 255 (max speed)
    motor_left->setSpeed(150); // set left motor speed
    motor_left->run(FORWARD); // turn on left motor
    motor_left->run(RELEASE); // turn of left motor

    motor_right->setSpeed(150); // set right motor speed
    motor_right->run(FORWARD); // turn on right motor
    motor_right->run(RELEASE); // turn off right motor

    last_command_time = millis();
}

// loop =======================================================================
void loop()
{

    // if serial is available parse input, else set speed to 0
    if (Serial.available()) {
        inpt = Serial.parseInt(); // Input as int
        last_command_time = millis();
        // inpt = Serial.readString();  // Input as string
    }

    if ((millis() - max_time_diff) > last_command_time) {
        inpt = 100100; // both motors stop, speed 0
    }

    // Parse input to values from int
    left_dir = inpt / 100000; // 1. input digit
    left_speed_inpt = (inpt % 100000) / 1000; // 2. and 3. input digit
    right_dir = (inpt % 1000) / 100; // 4. input digit
    right_speed_inpt = inpt % 100; // 5. and 6. input digit

    // Calculate motor speed values from input and motor speed range:
    left_speed = min_speed + (left_speed_inpt + 1) / 100. * (max_speed - min_speed);
    right_speed = min_speed + (right_speed_inpt + 1) / 100. * (max_speed - min_speed);
    // Serial.print(String(left_speed));
    // Set motor speeds
    motor_left->setSpeed(left_speed);
    motor_right->setSpeed(right_speed);

    // Run Motors
    // Left Motor
    if (motor_dir_left_reversed == false) {
        if (left_dir == 2) {
            motor_left->run(FORWARD);
        } else if (left_dir == 3) {
            motor_left->run(BACKWARD);
        } else if (left_dir == 1) {
            motor_left->run(RELEASE);
        } else {
            motor_left->run(RELEASE);
        }
    } else {
        if (left_dir == 2) {
            motor_left->run(BACKWARD);
        } else if (left_dir == 3) {
            motor_left->run(FORWARD);
        } else if (left_dir == 1) {
            motor_left->run(RELEASE);
        } else {
            motor_left->run(RELEASE);
        }
    }

    // Right Motor
    if (motor_dir_right_reversed == false) {
        if (right_dir == 2) {
            motor_right->run(FORWARD);
        } else if (right_dir == 3) {
            motor_right->run(BACKWARD);
        } else if (right_dir == 1) {
            motor_right->run(RELEASE);
        } else {
            motor_right->run(RELEASE);
        }
    } else {
        if (right_dir == 2) {
            motor_right->run(BACKWARD);
        } else if (right_dir == 3) {
            motor_right->run(FORWARD);
        } else if (right_dir == 1) {
            motor_right->run(RELEASE);
        } else {
            motor_right->run(RELEASE);
        }
    }
}