package yaokong;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

public class SteeringCommandSender extends WaitForAck {

    public SteeringCommandSender(InputStream inputStream, OutputStream outputStream) {
        super(inputStream, outputStream);
    }

    /**
     * 修正后的转向指令发送方法
     */
    public boolean sendSteeringCommand(byte steeringSpeed, byte forwardSpeed) throws IOException {
        // 使用修正后的方法构建指令
        byte[] commandBytes = buildSteeringCommandBytes(steeringSpeed, forwardSpeed);
        
        // 调试：打印发送的数据
        System.out.println("发送修正后的转向指令: " + bytesToHex(commandBytes));
        
        // 发送数据
        outputStream.write(commandBytes);
        outputStream.flush();
        
        return waitForAck(BluetoothProtocol.CMD_STEERING);
    }
    
    /**
     * 构建修正后的转向指令字节数组（按照协议1.docx格式）
     * 数据部分：转向速度(1) + 行进速度(1) + 保留字段(2) = 4字节
     */
    private byte[] buildSteeringCommandBytes(byte steeringSpeed, byte forwardSpeed) {
        // 验证速度范围
        if (steeringSpeed < -100 || steeringSpeed > 100) {
            throw new IllegalArgumentException("转向速度必须在-100到100之间");
        }
        if (forwardSpeed < -100 || forwardSpeed > 100) {
            throw new IllegalArgumentException("行进速度必须在-100到100之间");
        }
        
        // 数据长度：4字节
        int dataLength = 4;
        
        ByteBuffer buffer = ByteBuffer.allocate(14);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        
        // 帧头
        buffer.put(BluetoothProtocol.FRAME_HEADER);
        // 指令类型
        buffer.put(BluetoothProtocol.CMD_STEERING);
        // 数据长度
        buffer.putShort((short) dataLength);
        // 序列号
        short sequence = (short) BluetoothProtocol.getNextSequence();
        buffer.putShort(sequence);
        // 转向速度
        buffer.put(steeringSpeed);
        // 行进速度
        buffer.put(forwardSpeed);
        // 保留字段
        buffer.put((byte) 0x00);
        buffer.put((byte) 0x00);
        
        // 计算CRC16
        byte[] dataForCRC = new byte[1 + 2 + 2 + dataLength];
        System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
        int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
        buffer.putShort((short) crc);
        
        // 帧尾
        buffer.put(BluetoothProtocol.FRAME_FOOTER);
        
        return buffer.array();
    }
    
    /**
     * 字节数组转换为HEX字符串（用于调试）
     */
    private static String bytesToHex(byte[] bytes) {
        StringBuilder hexString = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) {
            String hex = Integer.toHexString(bytes[i] & 0xFF);
            if (hex.length() == 1) {
                hexString.append('0');
            }
            hexString.append(hex);
            if (i < bytes.length - 1) {
                hexString.append(' ');
            }
        }
        return hexString.toString().toUpperCase();
    }
    
    /**
     * 原始协议格式的发送方法（如果需要向后兼容）
     * 注意：这个方法不符合协议1.docx的规范，仅在特殊情况下使用
     */
    public boolean sendSteeringCommandOriginal(byte steerMode, short steerAngle,
                                       byte speedLevel, short durationMs) throws IOException {
        // 这个方法构建的是9字节数据格式，不符合协议规范
        ByteBuffer buffer = ByteBuffer.allocate(2 + 1 + 2 + 2 + 9 + 2 + 1);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        
        // 帧头
        buffer.put(BluetoothProtocol.FRAME_HEADER);
        // 指令类型
        buffer.put(BluetoothProtocol.CMD_STEERING);
        // 数据长度
        buffer.putShort((short) 9);
        // 序列号
        buffer.putShort((short) BluetoothProtocol.getNextSequence());
        // 转向模式
        buffer.put(steerMode);
        // 转向角度
        buffer.putShort(steerAngle);
        // 速度等级
        buffer.put(speedLevel);
        // 持续时间
        buffer.putShort(durationMs);
        // 保留字段
        buffer.put((byte) 0x00);
        buffer.put((byte) 0x00);
        
        // 计算CRC16
        byte[] dataForCRC = new byte[1 + 2 + 2 + 9];
        System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
        int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
        buffer.putShort((short) crc);
        
        // 帧尾
        buffer.put(BluetoothProtocol.FRAME_FOOTER);
        
        outputStream.write(buffer.array());
        outputStream.flush();
        
        return waitForAck(BluetoothProtocol.CMD_STEERING);
    }
}