#include "Arduino.h"
#include <stdint.h>
#include "Syren50.h"

void Syren50::init() {
    Serial1.begin(9600);
    delay(1000); // Give about 1 second to boot up, per datasheet
    Serial.write(0b10101010);
}

void Syren50::command(uint8_t command, uint8_t data) {
    Serial1.write(0x80); // Address
    Serial1.write(command); // Command
    Serial1.write(data);
    Serial1.write((0x80 + command + data) & 0b01111111); // Checksum
    Serial1.flush();
}

void Syren50::forwards(uint8_t speed) {
    command(0x00, speed);
}

void Syren50::reverse(uint8_t speed) {
    command(0x01, speed);
}

void Syren50::move(int8_t direction, uint8_t speed) {
    if (direction == 1) {
        forwards(speed);
    } else if (direction == -1) {
        reverse(speed);
    } else {
        forwards(0);
    }
}

void Syren50::minInputVoltage(float voltage) {
    command(0x03, (uint8_t)((voltage - 6.0) * 5.0));
}