package yaokong;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

import chuankou.SerialPortService;
import chuankou.sendmessage;

public class Control03 {

    private static final int MAX_SPEED = 100;
    private static final int MIN_SPEED = -100;
    private static final int SPEED_STEP = 10;

    private static int currentForwardSpeed = 0;
    private static int currentSteeringSpeed = 0;
    
    /**
     * 构建控制转向指令（指令类型0x03）的HEX格式字符串
     * 
     * @param steeringSpeedStr 转向速度字符串，范围-100到100，负数左转
     * @param forwardSpeedStr  行进速度字符串，范围-100到100，负数倒退
     * @return 控制转向指令的HEX格式字符串
     */
    public static String buildSteeringCommandHex(String steeringSpeedStr, String forwardSpeedStr) {
        // 解析速度值
        int steeringSpeed = parseSpeed(steeringSpeedStr, "转向速度");
        int forwardSpeed = parseSpeed(forwardSpeedStr, "行进速度");
        
        byte[] commandBytes = buildSteeringCommandBytes((byte) steeringSpeed, (byte) forwardSpeed);
        return bytesToHex(commandBytes);
    }
    
    /**
     * 构建控制转向指令的字节数组（修正版本，按照协议1.docx格式）
     * 协议要求：转向速度(1) + 行进速度(1) + 保留字段(2) = 4字节
     */
    public static 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之间");
        }
        
        // 数据长度：转向速度1 + 行进速度1 + 保留字段2 = 4字节
        int dataLength = 4;
        
        // 总长度：帧头2 + 指令类型1 + 数据长度2 + 序列号2 + 数据内容4 + CRC16 2 + 帧尾1 = 14字节
        ByteBuffer buffer = ByteBuffer.allocate(14);
        buffer.order(ByteOrder.LITTLE_ENDIAN);
        
        // 帧头
        buffer.put(BluetoothProtocol.FRAME_HEADER);
        
        // 指令类型
        buffer.put((byte) 0x03);
        
        // 数据长度 (4字节)
        buffer.putShort((short) dataLength);
        
        // 序列号
        short sequence = (short) BluetoothProtocol.getNextSequence();
        buffer.putShort(sequence);
        
        // 1. 转向速度 (1字节, int8_t, -100~100, 负数左转)
        buffer.put(steeringSpeed);
        
        // 2. 行进速度 (1字节, int8_t, -100~100, 负数倒退)
        buffer.put(forwardSpeed);
        
        // 3. 保留字段 (2字节，全部填0)
        buffer.put((byte) 0x00);
        buffer.put((byte) 0x00);
        
        // 计算CRC16（从指令类型开始到数据内容结束）
        // 范围：指令类型1 + 数据长度2 + 序列号2 + 数据4 = 9字节
        byte[] dataForCRC = new byte[9];
        
        // 临时构建CRC计算数据
        ByteBuffer tempBuffer = ByteBuffer.allocate(9);
        tempBuffer.order(ByteOrder.LITTLE_ENDIAN);
        tempBuffer.put((byte) 0x03); // 指令类型
        tempBuffer.putShort((short) dataLength); // 数据长度
        tempBuffer.putShort(sequence); // 序列号
        tempBuffer.put(steeringSpeed); // 转向速度
        tempBuffer.put(forwardSpeed); // 行进速度
        tempBuffer.put((byte) 0x00); // 保留字段1
        tempBuffer.put((byte) 0x00); // 保留字段2
        
        dataForCRC = tempBuffer.array();
        
        int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
        buffer.putShort((short) crc);
        
        // 帧尾
        buffer.put((byte) 0x0D);
        
        return buffer.array();
    }

    // 以下方法保持不变...
    public static synchronized boolean increaseForwardSpeedIfDebugSerialOpen() {
        return adjustForwardSpeed(SPEED_STEP);
    }

    public static synchronized boolean decreaseForwardSpeedIfDebugSerialOpen() {
        return adjustForwardSpeed(-SPEED_STEP);
    }

    public static synchronized boolean turnLeftIfDebugSerialOpen() {
        return adjustSteeringSpeed(-SPEED_STEP);
    }

    public static synchronized boolean turnRightIfDebugSerialOpen() {
        return adjustSteeringSpeed(SPEED_STEP);
    }

    public static synchronized boolean sendNeutralCommandIfDebugSerialOpen() {
        return sendSpeedsIfDebugSerialOpen(0, 0);
    }

    public static synchronized boolean adjustForwardSpeed(int delta) {
        if (delta == 0) {
            return true;
        }
        int next;
        if (delta > 0 && currentForwardSpeed < 0) {
            next = Math.min(currentForwardSpeed + delta, 0);
        } else if (delta < 0 && currentForwardSpeed > 0) {
            next = Math.max(currentForwardSpeed + delta, 0);
        } else {
            next = clampSpeed(currentForwardSpeed + delta);
        }
        if (next == currentForwardSpeed) {
            return true;
        }
        return sendSpeedsIfDebugSerialOpen(currentSteeringSpeed, next);
    }

    public static synchronized boolean adjustSteeringSpeed(int delta) {
        if (delta == 0) {
            return true;
        }
        int next;
        if (delta > 0 && currentSteeringSpeed < 0) {
            next = Math.min(currentSteeringSpeed + delta, 0);
        } else if (delta < 0 && currentSteeringSpeed > 0) {
            next = Math.max(currentSteeringSpeed + delta, 0);
        } else {
            next = clampSpeed(currentSteeringSpeed + delta);
        }
        if (next == currentSteeringSpeed) {
            return true;
        }
        return sendSpeedsIfDebugSerialOpen(next, currentForwardSpeed);
    }

    public static synchronized boolean approachForwardSpeedToZero(int step) {
        int magnitude = Math.abs(step);
        if (magnitude == 0 || currentForwardSpeed == 0) {
            return true;
        }
        int next = currentForwardSpeed > 0
                ? Math.max(currentForwardSpeed - magnitude, 0)
                : Math.min(currentForwardSpeed + magnitude, 0);
        if (next == currentForwardSpeed) {
            next = 0;
        }
        return sendSpeedsIfDebugSerialOpen(currentSteeringSpeed, next);
    }

    public static synchronized boolean approachSteeringSpeedToZero(int step) {
        int magnitude = Math.abs(step);
        if (magnitude == 0 || currentSteeringSpeed == 0) {
            return true;
        }
        int next = currentSteeringSpeed > 0
                ? Math.max(currentSteeringSpeed - magnitude, 0)
                : Math.min(currentSteeringSpeed + magnitude, 0);
        if (next == currentSteeringSpeed) {
            next = 0;
        }
        return sendSpeedsIfDebugSerialOpen(next, currentForwardSpeed);
    }

    public static synchronized void resetSpeeds() {
        currentForwardSpeed = 0;
        currentSteeringSpeed = 0;
    }

    public static synchronized int getCurrentForwardSpeed() {
        return currentForwardSpeed;
    }

    public static synchronized int getCurrentSteeringSpeed() {
        return currentSteeringSpeed;
    }

    /**
     * 直接设置并发送转向和前进速度（用于持续发送控制指令）
     * @param steeringSpeed 转向速度，范围-100到100
     * @param forwardSpeed 前进速度，范围-100到100
     * @return 是否发送成功
     */
    public static synchronized boolean setAndSendSpeeds(int steeringSpeed, int forwardSpeed) {
        return sendSpeedsIfDebugSerialOpen(steeringSpeed, forwardSpeed);
    }

    private static boolean sendSpeedsIfDebugSerialOpen(int nextSteering, int nextForward) {
        SerialPortService service = sendmessage.getActiveService();
        if (service == null || !service.isOpen()) {
            return false;
        }
        nextSteering = clampSpeed(nextSteering);
        nextForward = clampSpeed(nextForward);
        byte[] payload = buildSteeringCommandBytes((byte) nextSteering, (byte) nextForward);
        
        // 调试：打印发送的数据
        System.out.println("发送转向指令: " + bytesToHex(payload));
        
        boolean success = sendmessage.sendViaActive(payload);
        if (success) {
            currentSteeringSpeed = nextSteering;
            currentForwardSpeed = nextForward;
        }
        return success;
    }

    private static int clampSpeed(int value) {
        return Math.max(MIN_SPEED, Math.min(MAX_SPEED, value));
    }
    
    /**
     * 解析速度字符串
     */
    private static int parseSpeed(String speedStr, String paramName) {
        try {
            int speed = Integer.parseInt(speedStr.trim());
            if (speed < -100 || speed > 100) {
                throw new IllegalArgumentException(paramName + "必须在-100到100之间");
            }
            return speed;
        } catch (NumberFormatException e) {
            throw new IllegalArgumentException(paramName + "必须是整数: " + speedStr);
        }
    }
    
    /**
     * 构建常用转向指令的快捷方法
     */
    
    // 前进
    public static String forward(int speed) {
        speed = Math.min(speed, 100);
        return buildSteeringCommandHex("0", String.valueOf(speed));
    }
    
    // 后退
    public static String backward(int speed) {
        speed = Math.min(speed, 100);
        return buildSteeringCommandHex("0", String.valueOf(-speed));
    }
    
    // 左转前进
    public static String turnLeftForward(int steeringSpeed, int forwardSpeed) {
        steeringSpeed = Math.max(-steeringSpeed, -100);
        forwardSpeed = Math.min(forwardSpeed, 100);
        return buildSteeringCommandHex(String.valueOf(steeringSpeed), String.valueOf(forwardSpeed));
    }
    
    // 右转前进
    public static String turnRightForward(int steeringSpeed, int forwardSpeed) {
        steeringSpeed = Math.min(steeringSpeed, 100);
        forwardSpeed = Math.min(forwardSpeed, 100);
        return buildSteeringCommandHex(String.valueOf(steeringSpeed), String.valueOf(forwardSpeed));
    }
    
    /**
     * 字节数组转换为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();
    }
    
    /**
     * 测试函数：验证转向指令构建
     */
    public static void testBuildSteeringCommand() {
        System.out.println("=== 测试修正后的转向指令构建 ===");
        
        // 测试1：直行前进
        byte[] cmd1 = buildSteeringCommandBytes((byte)0, (byte)50);
        System.out.println("直行前进50: " + bytesToHex(cmd1));
        
        // 测试2：左转前进
        byte[] cmd2 = buildSteeringCommandBytes((byte)-30, (byte)40);
        System.out.println("左转前进: " + bytesToHex(cmd2));
        
        // 测试3：右转后退
        byte[] cmd3 = buildSteeringCommandBytes((byte)20, (byte)-60);
        System.out.println("右转后退: " + bytesToHex(cmd3));
        
        // 测试4：停止
        byte[] cmd4 = buildSteeringCommandBytes((byte)0, (byte)0);
        System.out.println("停止: " + bytesToHex(cmd4));
    }
      
}