package com.easternedgerobotics.rov.io;

import com.easternedgerobotics.rov.event.EventPublisher;
import com.easternedgerobotics.rov.value.ThrusterSensorValue;
import com.easternedgerobotics.rov.value.ThrusterSpeedValue;

import com.pi4j.io.i2c.I2CDevice;

import java.io.IOException;
import java.nio.ByteBuffer;

public class Thruster {

    private static final int BYTE_SIZE = 8;

    private static final byte WRITE_ADDRESS = 0x00;

    private static final byte READ_ADDRESS = 0x02;

    private static final int READ_BUFFER_SIZE = 9;

    private static final int VOLTAGE_INDEX = 2;

    private static final float VOLTAGE_SCALAR = 0.0004921f;

    private static final int CURRENT_INDEX = 6;

    private static final float CURRENT_SCALAR = 0.001122f;

    private static final int CURRENT_OFFSET = 32767;

    private static final int TEMPERATURE_INDEX = 4;

    private static final int SERIESRESISTOR = 3300;

    private static final int THERMISTORNOMINAL = 10000;

    private static final int BCOEFFICIENT = 3900;

    private static final int TEMPERATURENOMINAL = 25;

    private static final float CELSIUSCONVERSION = 273.15f;

    private final byte[] zeroBuffer = new byte[] {0x00, 0x00};

    private I2CDevice device;

    private ThrusterSpeedValue thrusterValue;

    private EventPublisher eventPublisher;

    private boolean initialized;

    public Thruster(final EventPublisher event, final ThrusterSpeedValue thruster, final I2CDevice device) {
        this.device = device;
        eventPublisher = event;
        thrusterValue = thruster;
        initialized = false;

        eventPublisher.valuesOfType(ThrusterSpeedValue.class)
            .filter(value -> thrusterValue.getName().equals(value.getName()))
            .subscribe(value -> thrusterValue = value);
    }

    /**
     * Write the latest {@code ThrusterSpeedValue} for this thruster to the device.
     * @throws IOException
     */
    public final void write() throws IOException {
        final short speed = (short) (thrusterValue.getSpeed() * Short.MAX_VALUE);
        final byte[] writeBuffer = ByteBuffer.allocate(2).putShort(speed).array();
        if (!initialized) {
            device.write(WRITE_ADDRESS, zeroBuffer, 0, zeroBuffer.length);
            initialized = true;
        }
        device.write(WRITE_ADDRESS, writeBuffer, 0, writeBuffer.length);
    }

    /**
     * Write zero to the thruster.
     * @throws IOException
     */
    public final void writeZero() throws IOException {
        device.write(WRITE_ADDRESS, zeroBuffer, 0, zeroBuffer.length);
    }

    public final void read() throws IOException {
        final byte[] readBuffer = new byte[READ_BUFFER_SIZE];
        device.read(READ_ADDRESS, readBuffer, 0, readBuffer.length);

        final float voltage = parseShort(readBuffer, VOLTAGE_INDEX) * VOLTAGE_SCALAR;
        final float current = (parseShort(readBuffer, CURRENT_INDEX) - CURRENT_OFFSET) * CURRENT_SCALAR;
        final float temperature = calculateTemperature(parseShort(readBuffer, TEMPERATURE_INDEX));

        eventPublisher.emit(ThrusterSensorValue.create(
            thrusterValue.getName(),
            voltage,
            current,
            temperature
        ));
    }

    /**
     * Reads a short from two consecutive bytes.
     *
     * @param bytes an array of two or more bytes, two of which represent a short value
     * @param offset the offset of the sort to read
     * @return the short representation of {@code bytes[offset]} and {@code bytes[offset + 1]}
     */
    private short parseShort(final byte[] bytes, final int offset) {
        return (short) (((bytes[offset]) << BYTE_SIZE) | bytes[offset + 1]);
    }

    /**
     * See <a href="http://docs.bluerobotics.com/bluesc/">BlueESC documentation</a>.
     *
     * @param tempRaw the raw temperature value from the thruster
     * @return the parsed temperature
     */
    private float calculateTemperature(final short tempRaw) {
        float steinhart;
        if (tempRaw == 0) {
            steinhart = 0;
        } else {
            final float resistance = SERIESRESISTOR / (65535 / tempRaw - 1);

            steinhart = resistance / THERMISTORNOMINAL;
            steinhart = (float) Math.log(steinhart);
            steinhart /= BCOEFFICIENT;
            steinhart += 1.0f / (TEMPERATURENOMINAL + CELSIUSCONVERSION);
            steinhart = 1.0f / steinhart;
            steinhart -= CELSIUSCONVERSION;
        }
        return steinhart;
    }
}

@@ -95,70 +95,8 @@
 er;%0A
-import com.easternedgerobotics.rov.value.ThrusterSensorValue;%0A
 impo
@@ -152,16 +152,16 @@
 dValue;%0A
+
 %0Aimport 
@@ -273,54 +273,8 @@
 r %7B%0A
-%0A    private static final int BYTE_SIZE = 8;%0A%0A
     
@@ -326,715 +326,8 @@
 0;%0A%0A
-    private static final byte READ_ADDRESS = 0x02;%0A%0A    private static final int READ_BUFFER_SIZE = 9;%0A%0A    private static final int VOLTAGE_INDEX = 2;%0A%0A    private static final float VOLTAGE_SCALAR = 0.0004921f;%0A%0A    private static final int CURRENT_INDEX = 6;%0A%0A    private static final float CURRENT_SCALAR = 0.001122f;%0A%0A    private static final int CURRENT_OFFSET = 32767;%0A%0A    private static final int TEMPERATURE_INDEX = 4;%0A%0A    private static final int SERIESRESISTOR = 3300;%0A%0A    private static final int THERMISTORNOMINAL = 10000;%0A%0A    private static final int BCOEFFICIENT = 3900;%0A%0A    private static final int TEMPERATURENOMINAL = 25;%0A%0A    private static final float CELSIUSCONVERSION = 273.15f;%0A%0A
     
@@ -468,52 +468,8 @@
 e;%0A%0A
-    private EventPublisher eventPublisher;%0A%0A
     
@@ -641,40 +641,8 @@
 ce;%0A
-        eventPublisher = event;%0A
     
@@ -714,25 +714,16 @@
    event
-Publisher
 .valuesO
@@ -1588,32 +1588,32 @@
 s IOException %7B%0A
+
         device.w
@@ -1676,1918 +1676,7 @@
    %7D
-%0A%0A    public final void read() throws IOException %7B%0A        final byte%5B%5D readBuffer = new byte%5BREAD_BUFFER_SIZE%5D;%0A        device.read(READ_ADDRESS, readBuffer, 0, readBuffer.length);%0A%0A        final float voltage = parseShort(readBuffer, VOLTAGE_INDEX) * VOLTAGE_SCALAR;%0A        final float current = (parseShort(readBuffer, CURRENT_INDEX) - CURRENT_OFFSET) * CURRENT_SCALAR;%0A        final float temperature = calculateTemperature(parseShort(readBuffer, TEMPERATURE_INDEX));%0A%0A        eventPublisher.emit(ThrusterSensorValue.create(%0A            thrusterValue.getName(),%0A            voltage,%0A            current,%0A            temperature%0A        ));%0A    %7D%0A%0A    /**%0A     * Reads a short from two consecutive bytes.%0A     *%0A     * @param bytes an array of two or more bytes, two of which represent a short value%0A     * @param offset the offset of the sort to read%0A     * @return the short representation of %7B@code bytes%5Boffset%5D%7D and %7B@code bytes%5Boffset + 1%5D%7D%0A     */%0A    private short parseShort(final byte%5B%5D bytes, final int offset) %7B%0A        return (short) (((bytes%5Boffset%5D) %3C%3C BYTE_SIZE) %7C bytes%5Boffset + 1%5D);%0A    %7D%0A%0A    /**%0A     * See %3Ca href=%22http://docs.bluerobotics.com/bluesc/%22%3EBlueESC documentation%3C/a%3E.%0A     *%0A     * @param tempRaw the raw temperature value from the thruster%0A     * @return the parsed temperature%0A     */%0A    private float calculateTemperature(final short tempRaw) %7B%0A        float steinhart;%0A        if (tempRaw == 0) %7B%0A            steinhart = 0;%0A        %7D else %7B%0A            final float resistance = SERIESRESISTOR / (65535 / tempRaw - 1);%0A%0A            steinhart = resistance / THERMISTORNOMINAL;%0A            steinhart = (float) Math.log(steinhart);%0A            steinhart /= BCOEFFICIENT;%0A            steinhart += 1.0f / (TEMPERATURENOMINAL + CELSIUSCONVERSION);%0A            steinhart = 1.0f / steinhart;%0A            steinhart -= CELSIUSCONVERSION;%0A        %7D%0A        return steinhart;%0A    %7D
 %0A%7D%0A

Remove unused code from io.Thruster