package message

import (
	"encoding/binary"
	"math"
)

// 速度转转速计算公式：
// 转速=车速 ÷ 车轮周长
// 车的轮径为d，车速为v1，电机转速为v2
// 公式为：V2=V1÷（πd）×60
type ElectricMachinery struct {
	Speed         float32 // 车速,m/s
	WheelDiameter int32   // 轮径,mm
	IsBack        bool    // 是否倒车
}

func (t *ElectricMachinery) Encode() []byte {
	b := []byte{0x01, 0x10, 0x60, 0xff, 0x00, 0x02, 0x04}
	rotarySpeed := t.Speed * 60 / (float32(math.Pi) * float32(t.WheelDiameter) / 1000)
	if t.IsBack {
		b = binary.BigEndian.AppendUint32(b, uint32(0-rotarySpeed))
	} else {
		b = binary.BigEndian.AppendUint32(b, uint32(rotarySpeed))
	}
	crc := chkcrc(b) // crc校验码
	b = binary.BigEndian.AppendUint16(b, crc)
	return b
}

// 生成crc
func chkcrc(data []byte) uint16 {
	var crc uint16 = 0xFFFF
	for i := 0; i < len(data); i++ {
		crc = calccrc(data[i], crc)
	}
	hi := crc % 256
	lo := crc / 256
	crc = (hi << 8) | lo
	return crc
}

// 计算crc
func calccrc(crcbuf byte, crc uint16) uint16 {
	crc = crc ^ uint16(crcbuf)
	for i := 0; i < 8; i++ {
		chk := crc & 1
		crc = crc >> 1
		crc = crc & 0x7fff
		if chk == 1 {
			crc = crc ^ 0xa001
		}
		crc = crc & 0xffff
	}
	return crc
}