package com.ubtrobot.jimu.robotapi;

import android.util.Log;
import com.ubtrobot.jimu.bluetooth.base.ProtocolPacket;
import com.ubtrobot.jimu.connection.RequestException;
import com.ubtrobot.jimu.connection.RobotConnection;
import java.util.ArrayList;
import java.util.Arrays;

/* loaded from: classes2.dex */
public class MotorManager {
    protected RobotConnection a;

    public MotorManager(RobotConnection robotConnection) {
        this.a = robotConnection;
    }

    private static byte[] b(int[] iArr, int[] iArr2, int[] iArr3) {
        if (iArr.length != iArr2.length || iArr2.length != iArr3.length) {
            Log.e("MotorManager", "Invalid arguments!");
            return null;
        }
        int length = iArr.length;
        byte[] a = a(iArr);
        byte[] bArr = new byte[a.length + (length * 4)];
        System.arraycopy(a, 0, bArr, 0, a.length);
        int length2 = a.length + 0;
        for (int i = 1; i <= length; i++) {
            int i2 = length - i;
            int i3 = iArr2[i2];
            int i4 = length2 + 1;
            bArr[length2] = (byte) ((i3 >> 8) & 255);
            int i5 = i4 + 1;
            bArr[i4] = (byte) (i3 & 255);
            int i6 = iArr3[i2];
            int i7 = i5 + 1;
            bArr[i5] = (byte) ((i6 >> 8) & 255);
            length2 = i7 + 1;
            bArr[i7] = (byte) (i6 & 255);
        }
        return bArr;
    }

    public void a(int[] iArr, int[] iArr2, int[] iArr3) throws MotorException {
        int[] iArr4 = new int[iArr3.length];
        for (int i = 0; i < iArr3.length; i++) {
            iArr4[i] = iArr3[i] / 100;
        }
        try {
            byte[] g = this.a.a(new ProtocolPacket(144, b(iArr, iArr2, iArr4))).g();
            if (g[0] == 0) {
                return;
            }
            throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]), a(Arrays.copyOfRange(g, 1, g.length)));
        } catch (RequestException e) {
            throw new MotorException(e.getCode(), e.getMessage());
        }
    }

    public int a(int i, int i2) throws JimuException {
        ProtocolPacket protocolPacket = new ProtocolPacket(116, new byte[]{10, (byte) i, (byte) i2});
        try {
            byte[] g = this.a.a(protocolPacket).g();
            if (g.length == 3) {
                return g[2] & 255;
            }
            throw new JimuException(-22, "res argument is err!");
        } catch (RequestException e) {
            Log.e("MotorManager", "Call cmd fail! cmd:" + ((int) protocolPacket.f()), e);
            throw new JimuException(e.getCode(), e.getMessage());
        }
    }

    public void a() throws JimuException {
        try {
            byte[] g = this.a.a(new ProtocolPacket(146, new byte[0])).g();
            if (g[0] == 0) {
                return;
            }
            throw new JimuException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
        } catch (RequestException e) {
            throw new JimuException(e.getCode(), e.getMessage());
        }
    }

    private static byte[] a(int[] iArr) {
        int i;
        byte[] bArr = new byte[8];
        int i2 = 0;
        byte b = 0;
        while (true) {
            i = 1;
            if (i2 >= iArr.length) {
                break;
            }
            if (iArr[i2] > 64 || iArr[i2] < 1) {
                Log.e("MotorManager", "Unsupport id! The support motor id range is 1~64");
            }
            int i3 = (iArr[i2] - 1) / 8;
            b = (byte) (b | (1 << i3));
            bArr[i3] = (byte) (((byte) (1 << ((iArr[i2] - 1) % 8))) | bArr[i3]);
            i2++;
        }
        int i4 = 0;
        for (byte b2 : bArr) {
            if (b2 != 0) {
                i4++;
            }
        }
        byte[] bArr2 = new byte[i4 + 1];
        bArr2[0] = b;
        for (int length = bArr.length - 1; length >= 0; length--) {
            if (bArr[length] != 0) {
                bArr2[i] = bArr[length];
                i++;
            }
        }
        return bArr2;
    }

    private static ArrayList<Integer> a(byte[] bArr) {
        ArrayList<Integer> arrayList = new ArrayList<>();
        int i = 0;
        byte b = bArr[0];
        ArrayList arrayList2 = new ArrayList();
        for (int i2 = 7; i2 >= 0; i2--) {
            if (((1 << i2) & b & 255) != 0) {
                arrayList2.add(Integer.valueOf(i2));
            }
        }
        if (arrayList2.size() != bArr.length - 1) {
            return arrayList;
        }
        while (i < arrayList2.size()) {
            int intValue = ((Integer) arrayList2.get(i)).intValue();
            i++;
            arrayList.addAll(a(bArr[i], intValue * 8));
        }
        return arrayList;
    }

    private static ArrayList<Integer> a(byte b, int i) {
        ArrayList<Integer> arrayList = new ArrayList<>();
        for (int i2 = 0; i2 < 8; i2++) {
            if ((b & 255 & (1 << i2)) != 0) {
                arrayList.add(Integer.valueOf(i2 + 1 + i));
            }
        }
        return arrayList;
    }
}