#include <Servo.h>

byte elevation_bytes[2];
byte azimuth_bytes[2];

Servo elevation_servo;

void setup() {

    elevation_servo.attach(9);  // Attach servo to pin 9
    elevation_servo.write(180);

    Serial.begin(115200);
    // Serial.write("Arduino Connected :)");
}

void loop() {
    read_serial();
    control_all_servos(byteArrayToInt(elevation_bytes),
                       flip_spin_axis(byteArrayToInt(azimuth_bytes)));
}

// Handles both the read and write to serial functions
void read_serial() {
    byte serial_input[4];

    while (Serial.available()) {
        Serial.readBytes(serial_input, 4);
        parse_serial(serial_input);
        write_serial();
    }
}

// Write the stored values back to the serial
void write_serial() {
    int16_t elevation_val = byteArrayToInt(elevation_bytes);
    int16_t azimuth_val = byteArrayToInt(azimuth_bytes);

    Serial.write(true);
    splitByteWrite16bit(elevation_val);
    splitByteWrite16bit(azimuth_val);
}

// Parse incoming serial message into the elevation and azimuth
void parse_serial(byte inbytes[4]) {

    elevation_bytes[0] = inbytes[0];
    elevation_bytes[1] = inbytes[1];
    azimuth_bytes[0] = inbytes[2];
    azimuth_bytes[1] = inbytes[3];
}

// Flip spin axis because servos are flipped in real life
uint16_t flip_spin_axis(uint16_t original_angle) {

    uint16_t flipped_angle = 360 - original_angle;

    return flipped_angle;
}

// Convert incoming byte data to an int data type
uint32_t byteArrayToInt(byte byteArray[]) {
    uint32_t value = 0;

    size_t length = sizeof(byteArray) / sizeof(byteArray[0]);

    for (uint8_t i = 0; i < length; i++) {
        value = (value << 8) | byteArray[length - 1 - i];
    }

    return value;
}

void control_elevation_servo(int16_t elevation_angle) {
    if ((elevation_angle <= 180) & (elevation_angle >= 0)) {

        elevation_servo.write(elevation_angle);
    }
}