package com.ubtrobot.jimu.robotapi;

import android.util.Log;
import com.ubtrobot.jimu.bluetooth.base.IPacket;
import com.ubtrobot.jimu.bluetooth.base.ProtocolPacket;
import com.ubtrobot.jimu.connection.RequestException;
import com.ubtrobot.jimu.connection.ResponseCallback;
import com.ubtrobot.jimu.connection.RobotConnection;
import com.ubtrobot.log.ALog;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/* loaded from: classes2.dex */
public class ServoManager extends MotorManager {
    public ServoManager(RobotConnection robotConnection) {
        super(robotConnection);
    }

    public void a(int[] iArr, float[] fArr, int i, int i2) throws MotorException {
        int length = fArr.length;
        byte[] bArr = new byte[length + 4 + 1 + 2];
        byte[] a = a(iArr);
        byte[] bArr2 = new byte[length];
        for (int i3 = 0; i3 < length; i3++) {
            bArr2[i3] = (byte) fArr[i3];
        }
        System.arraycopy(a, 0, bArr, 0, a.length);
        int length2 = a.length + 0;
        System.arraycopy(bArr2, 0, bArr, length2, bArr2.length);
        int length3 = length2 + bArr2.length;
        bArr[length3] = (byte) (i / 20);
        int i4 = length3 + 1;
        int i5 = i + i2;
        bArr[i4] = (byte) ((65280 & i5) >> 8);
        bArr[i4 + 1] = (byte) (i5 & 255);
        try {
            byte[] g = this.a.a(new ProtocolPacket(9, bArr)).g();
            if (g.length == 1 && g[0] == 0) {
                return;
            }
            if (g.length != 5) {
                throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
            }
            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 void b() throws JimuException {
        b(0, false);
    }

    public ServoAngleReadInfo b(int i, boolean z) throws JimuException {
        new ArrayList();
        byte[] bArr = new byte[2];
        bArr[0] = (byte) i;
        boolean z2 = true;
        if (z) {
            bArr[1] = 1;
        } else {
            bArr[1] = 0;
        }
        ProtocolPacket protocolPacket = new ProtocolPacket(11, bArr);
        try {
            byte[] g = this.a.a(protocolPacket).g();
            if (g.length == 1 || g.length != 6) {
                throw new JimuException(-22, "Control fail!");
            }
            int i2 = g[0] & 255;
            if ((g[1] & 255) != 170) {
                z2 = false;
            }
            return new ServoAngleReadInfo(i2, z2, ((g[2] & 255) << 8) | (g[3] & 255), (g[5] & 255) | ((g[4] & 255) << 8));
        } catch (RequestException e) {
            Log.e("ServoManager", "Call cmd fail! cmd:" + ((int) protocolPacket.f()), e);
            throw new JimuException(e.getCode(), e.getMessage());
        }
    }

    public void a(int[] iArr, int i) throws MotorException {
        int length = iArr.length;
        byte[] bArr = new byte[length + 4];
        bArr[0] = (byte) (length & 255);
        int i2 = 0;
        while (i2 < length) {
            int i3 = i2 + 1;
            bArr[i3] = (byte) iArr[i2];
            i2 = i3;
        }
        if (i > 0) {
            bArr[length + 1] = 1;
        } else if (i < 0) {
            bArr[length + 1] = 2;
        } else {
            bArr[length + 1] = 0;
        }
        int abs = Math.abs(i);
        bArr[length + 2] = (byte) ((65280 & abs) >> 8);
        bArr[length + 3] = (byte) (abs & 255);
        try {
            byte[] g = this.a.a(new ProtocolPacket(7, bArr)).g();
            if (g.length == 1 && g[0] == 0) {
                return;
            }
            if (g.length == 5) {
                throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]), a(Arrays.copyOfRange(g, 1, g.length)));
            }
            throw new MotorException(-21, "cmd result is fail! ErrCode:" + ((int) g[0]));
        } catch (RequestException e) {
            throw new MotorException(e.getCode(), e.getMessage());
        }
    }

    @Override // com.ubtrobot.jimu.robotapi.MotorManager
    public void a() throws JimuException {
        try {
            byte[] g = this.a.a(new ProtocolPacket(59, 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());
        }
    }

    public List<ServoAngleReadInfo> a(final int i, boolean z) throws JimuException {
        final boolean[] zArr = new boolean[1];
        final boolean[] zArr2 = new boolean[1];
        final JimuException[] jimuExceptionArr = new JimuException[1];
        final ArrayList arrayList = new ArrayList();
        byte[] bArr = new byte[2];
        bArr[0] = 0;
        if (z) {
            bArr[1] = 1;
        } else {
            bArr[1] = 0;
        }
        ProtocolPacket protocolPacket = new ProtocolPacket(11, bArr);
        this.a.a(protocolPacket, new ResponseCallback(this) { // from class: com.ubtrobot.jimu.robotapi.ServoManager.1
            @Override // com.ubtrobot.jimu.connection.ResponseCallback
            public void a(IPacket iPacket, IPacket iPacket2) {
                byte[] g = iPacket2.g();
                if (g.length == 1 || g.length != 6) {
                    synchronized (zArr) {
                        jimuExceptionArr[0] = new JimuException(-22, "Control fail!");
                        zArr[0] = true;
                        ALog.a("ServoManager").d("responded notify");
                        zArr.notifyAll();
                    }
                    return;
                }
                ServoAngleReadInfo servoAngleReadInfo = new ServoAngleReadInfo(g[0] & 255, (g[1] & 255) == 170, ((g[2] & 255) << 8) | (g[3] & 255), (g[5] & 255) | ((g[4] & 255) << 8));
                synchronized (zArr) {
                    arrayList.add(servoAngleReadInfo);
                    if (arrayList.size() == i) {
                        zArr2[0] = true;
                        zArr[0] = true;
                        Log.d("ServoManager", "responded notify");
                        zArr.notifyAll();
                    }
                }
            }

            @Override // com.ubtrobot.jimu.connection.ResponseCallback
            public void a(IPacket iPacket, RequestException requestException) {
                synchronized (zArr) {
                    jimuExceptionArr[0] = new JimuException(requestException.getCode(), requestException.getMessage());
                    zArr[0] = true;
                    Log.d("ServoManager", "responded notify");
                    zArr.notifyAll();
                }
            }
        }, i);
        synchronized (zArr) {
            while (!zArr[0]) {
                try {
                    Log.d("ServoManager", "responded wait");
                    zArr.wait();
                    break;
                } catch (InterruptedException e) {
                    Log.w("ServoManager", "Interrupted by someone when sync doRequest. req=" + protocolPacket + "Ignore it.", e);
                    Thread.currentThread().interrupt();
                }
            }
            if (jimuExceptionArr[0] != null) {
                throw jimuExceptionArr[0];
            }
        }
        return arrayList;
    }

    @Override // com.ubtrobot.jimu.robotapi.MotorManager
    public int a(int i, int i2) throws JimuException {
        ProtocolPacket protocolPacket = new ProtocolPacket(12, new byte[]{(byte) i, (byte) i2});
        try {
            byte[] g = this.a.a(protocolPacket).g();
            if (g.length == 1) {
                return g[0] & 255;
            }
            throw new JimuException(-22, "res argument is err!");
        } catch (RequestException e) {
            Log.e("ServoManager", "Call cmd fail! cmd:" + ((int) protocolPacket.f()), e);
            throw new JimuException(e.getCode(), e.getMessage());
        }
    }

    private byte[] a(int[] iArr) {
        byte[] bArr = new byte[4];
        for (int i : iArr) {
            if (i > 0 && i <= 8) {
                bArr[3] = (byte) (bArr[3] + (1 << (i - 1)));
            } else if (i > 8 && i <= 16) {
                bArr[2] = (byte) (bArr[2] + (1 << ((i - 1) - 8)));
            } else if (i > 16 && i <= 24) {
                bArr[1] = (byte) (bArr[1] + (1 << ((i - 1) - 16)));
            } else if (i >= 24 && i <= 32) {
                bArr[0] = (byte) (bArr[0] + (1 << ((i - 1) - 24)));
            }
        }
        return bArr;
    }

    private ArrayList<Integer> a(byte[] bArr) {
        ArrayList<Integer> arrayList = new ArrayList<>();
        if (bArr.length != 4) {
            return arrayList;
        }
        for (int i = 0; i < bArr.length; i++) {
            int i2 = 24 - (i * 8);
            byte b = bArr[i];
            for (int i3 = 0; i3 < 8; i3++) {
                if ((b & 255 & (1 << i3)) != 0) {
                    arrayList.add(Integer.valueOf(i3 + 1 + i2));
                }
            }
        }
        return arrayList;
    }
}