package com.example.upbot;



import com.example.upbot.MSG.Head;
import com.example.upbot.MSG.MsgId;

import java.util.zip.CRC32;
import java.util.zip.Checksum;

public class SendMsg {
    public static byte[] sendCmdMsg(byte value){
        Head head = new Head();
        head.setMsg_id(MsgId.MSG_CMD);
        head.setLength((byte) 0x02);
        byte[] result = new byte[]{head.getFlag(), head.getMsg_id(), head.getLength(), value,0x00};
        calculateAndSetChecksum(result);
        return result;
    }

    public static byte[] sendMapCorrectMsg(float x_offset, float y_offset, float rotation){
        Head head = new Head();
        head.setMsg_id(MsgId.MSG_CONF);
        byte[] x_bytes = getBytes(x_offset);
        byte[] y_bytes = getBytes(y_offset);
        byte[] rotation_bytes = getBytes(rotation);
        int length = x_bytes.length+y_bytes.length+rotation_bytes.length+1;
        head.setLength((byte) length);
        byte[] result = new byte[4+length];
        result[0] = head.getFlag();
        result[1] = head.getMsg_id();
        result[2] = head.getLength();
        System.arraycopy(x_bytes, 0, result, 3, x_bytes.length);
        System.arraycopy(y_bytes, 0, result, 3+x_bytes.length, y_bytes.length);
        System.arraycopy(rotation_bytes, 0, result, 3+x_bytes.length+y_bytes.length, rotation_bytes.length);
        result[result.length-1] = 0x00;
        calculateAndSetChecksum(result);
        return result;
    }

    public static byte[] sendChargingMsg(byte charge){
        Head head = new Head();
        head.setMsg_id(MsgId.MSG_CONF);
        head.setLength((byte) 0x01);
        byte[] result = new byte[]{head.getFlag(), head.getMsg_id(), head.getLength(), charge,0x00};
        calculateAndSetChecksum(result);
        return result;
    }

    public static byte[] sendFrequencyMsg(float frequency){
        Head head = new Head();
        head.setMsg_id(MsgId.MSG_CONF);
        head.setLength((byte) 0x01);
        byte[] frequency_byte = getBytes(frequency);
        byte[] result = new byte[frequency_byte.length+4];
        result[0] = head.getFlag();
        result[1] = head.getMsg_id();
        result[2] = head.getLength();
        System.arraycopy(frequency_byte, 0, result, 3, frequency_byte.length);
        result[result.length-1] = 0x00;
        calculateAndSetChecksum(result);
        return result;
    }

    public static byte[] sendCoordinateMsg(byte base_num, float[] x_offset, float[] y_offset, float[] z_offset){
        Head head = new Head();
        head.setMsg_id(MsgId.MSG_CONF);
        byte[][] x_bytes = new byte[base_num][4];
        byte[][] y_bytes = new byte[base_num][4];
        byte[][] z_bytes = new byte[base_num][4];
        for (int i = 0; i < base_num; i++){
            x_bytes[i] = getBytes(x_offset[i]);
            y_bytes[i] = getBytes(y_offset[i]);
            z_bytes[i] = getBytes(z_offset[i]);
        }
        int totalLength = (x_bytes.length)*4+(y_bytes.length)*4+(z_bytes.length)*4+1;
        head.setLength((byte) totalLength);
        int l = 4 + totalLength;
        byte[] result = new byte[l];
        result[0] = head.getFlag();
        result[1] = head.getMsg_id();
        result[2] = head.getLength();
        for (int i = 0; i < base_num ; i++){
            System.arraycopy(x_bytes[i], 0, result, 3+4*i, x_bytes[i].length);
        }
        for (int i = 0; i < base_num; i++){
            System.arraycopy(y_bytes[i], 0, result, (x_bytes.length)*4+3+4*i, y_bytes.length);
        }
        for (int i = 0; i < base_num; i++){
            System.arraycopy(z_bytes[i], 0, result, (x_bytes.length)*4+(y_bytes.length)*4+3+4*i, y_bytes.length);
        }
        result[result.length-1] = 0x00;
        calculateAndSetChecksum(result);
        return result;
    }
    public static void calculateAndSetChecksum(byte[] bytes) {
        int sum = 0;
        for (int i = 0; i < bytes.length - 1; i++) {
            sum += bytes[i] & 0xFF; // 将字节转换为无符号值
        }
        byte checksum = (byte) (sum & 0xFF); // 取结果的低 8 位
        bytes[bytes.length - 1] = checksum; // 将校验和写入最后一位
    }

    public static byte[] getBytes(float data)
    {
        int intBits = Float.floatToIntBits(data);
        return getBytes(intBits);
    }

    public static byte[] getBytes(int data)
    {
        byte[] bytes = new byte[4];
        bytes[0] = (byte) (data & 0xff);
        bytes[1] = (byte) ((data & 0xff00) >> 8);
        bytes[2] = (byte) ((data & 0xff0000) >> 16);
        bytes[3] = (byte) ((data & 0xff000000) >> 24);
        return bytes;
    }
}
