# -*- coding: utf-8 -*-
"""
SM-NK17 시리즈 RS485(Modbus-RTU) 파이썬 샘플 코드 — MotorBank
필요: pip install pyserial
통신: 115200bps, 8 데이터비트, 1 정지비트, 패리티 없음 (출하 기본값)
주소: DIP 스위치로 설정, 기본 1번

실행 전 PORT만 환경에 맞게 바꾸세요.
  Windows: "COM3" 등 / Linux: "/dev/ttyUSB0" / Mac: "/dev/tty.usbserial-XXXX"
"""
import serial
import struct
import time

PORT = "COM3"        # ← 사용 환경에 맞게 변경
BAUD = 115200
SLAVE = 1            # 드라이버 주소 (DIP 스위치 설정값)

# ---------- Modbus-RTU 기본 도구 ----------

def crc16(data: bytes) -> bytes:
    """Modbus CRC16 (하위 바이트 먼저 붙여 전송)"""
    crc = 0xFFFF
    for b in data:
        crc ^= b
        for _ in range(8):
            if crc & 1:
                crc = (crc >> 1) ^ 0xA001
            else:
                crc >>= 1
    return struct.pack("<H", crc)

def _txrx(ser, frame: bytes, resp_len: int) -> bytes:
    ser.reset_input_buffer()
    ser.write(frame + crc16(frame))
    time.sleep(0.05)
    return ser.read(resp_len)

def write_u16(ser, reg: int, value: int):
    """기능코드 0x06 — 16비트 레지스터 1개 쓰기"""
    frame = struct.pack(">BBHH", SLAVE, 0x06, reg, value & 0xFFFF)
    return _txrx(ser, frame, 8)

def write_i32(ser, reg: int, value: int):
    """기능코드 0x10 — 32비트 값 쓰기.
    ※ SM-NK17은 하위 워드를 먼저 보냅니다 (매뉴얼 예제 기준)."""
    v = value & 0xFFFFFFFF
    lo, hi = v & 0xFFFF, (v >> 16) & 0xFFFF
    frame = struct.pack(">BBHHBHH", SLAVE, 0x10, reg, 2, 4, lo, hi)
    return _txrx(ser, frame, 8)

def read_regs(ser, reg: int, count: int):
    """기능코드 0x03 — 레지스터 읽기. 워드 리스트 반환"""
    frame = struct.pack(">BBHH", SLAVE, 0x03, reg, count)
    resp = _txrx(ser, frame, 5 + 2 * count)
    if len(resp) < 5 + 2 * count:
        raise IOError("응답 없음 — 배선(A/B), 주소, 통신 설정을 확인하세요")
    n = resp[2]
    return list(struct.unpack(">%dH" % (n // 2), resp[3:3 + n]))

def read_i32(ser, reg: int) -> int:
    """32비트 읽기 — 하위 워드 먼저 온다고 가정 (쓰기와 동일 순서).
    값이 이상하게 크게 읽히면 lo/hi를 바꿔보세요."""
    lo, hi = read_regs(ser, reg, 2)
    v = (hi << 16) | lo
    return v - 0x100000000 if v >= 0x80000000 else v

# ---------- 자주 쓰는 레지스터 ----------
REG_POS_ACTUAL = 0x0004   # 실시간 위치 (INT32, 읽기, 엔코더 기준)
REG_SPEED_RPM  = 0x009A   # 운전 속도 rpm (기본 300)
REG_ACC_MS     = 0x0098   # 가속 시간 ms
REG_DEC_MS     = 0x0099   # 감속 시간 ms
REG_RUN_STOP   = 0x00C8   # 1 정방향 / 257 역방향 / 0 감속정지 / 256 비상정지
REG_MOVE_REL   = 0x00CE   # 상대 이동 펄스 수 (INT32, 정지 시)
REG_MOVE_ABS   = 0x00D0   # 절대 위치 이동 (INT32, 정지 시)
REG_MOVE_ABS_R = 0x00E8   # 절대 위치 이동 (운전 중에도 실행 가능)
REG_ALARM_READ = 0x00A3   # 경보 상태 읽기
REG_ALARM_CLR  = 0x00A4   # 경보 해제

PULSES_PER_REV = 4000     # 기본 세분화 (0x0024에서 변경 가능)

# ---------- 데모 ----------
if __name__ == "__main__":
    with serial.Serial(PORT, BAUD, bytesize=8, parity="N", stopbits=1, timeout=0.5) as ser:

        # 1) 운전 속도 60rpm 설정
        write_u16(ser, REG_SPEED_RPM, 60)
        print("속도 60rpm 설정 완료")

        # 2) 정방향으로 1회전 (상대 이동 +4000펄스)
        write_i32(ser, REG_MOVE_REL, PULSES_PER_REV)
        print("1회전 이동 명령 전송")
        time.sleep(2)

        # 3) 실제 위치 확인 (엔코더 기준)
        pos = read_i32(ser, REG_POS_ACTUAL)
        print("현재 위치: %d 펄스 (약 %.2f회전)" % (pos, pos / PULSES_PER_REV))

        # 4) 역방향으로 원위치 (절대 위치 0으로)
        write_i32(ser, REG_MOVE_ABS, 0)
        print("원위치 복귀 명령 전송")
        time.sleep(2)
        print("현재 위치:", read_i32(ser, REG_POS_ACTUAL))

        # 참고) 정속 회전 시작/정지
        # write_u16(ser, REG_RUN_STOP, 1)    # 정방향 연속 회전
        # time.sleep(3)
        # write_u16(ser, REG_RUN_STOP, 0)    # 감속 정지

/*
 * SM-NK17 시리즈 RS485(Modbus-RTU) 아두이노 샘플 코드 — MotorBank
 *
 * 배선 (MAX485 모듈 사용 기준):
 *   Arduino D8  -> MAX485 RO      (수신)
 *   Arduino D9  -> MAX485 DI      (송신)
 *   Arduino D2  -> MAX485 DE + RE (송수신 전환, 묶어서 연결)
 *   MAX485 A/B  -> 모터 485A / 485B
 *   전원: 모터 V+/V- 에 DC 8~48V (24V 권장), 극성 주의!
 *
 * 통신: 115200bps, 8N1 (출하 기본값) / 드라이버 주소: DIP 스위치, 기본 1
 * 동작: 3초 정방향 회전 -> 정지 -> 3초 역방향 회전 -> 정지 반복
 */
#include <SoftwareSerial.h>

SoftwareSerial rs485(8, 9);   // RX, TX
const int DE_RE = 2;
const byte SLAVE = 1;

uint16_t crc16(byte *buf, int len) {
  uint16_t crc = 0xFFFF;
  for (int i = 0; i < len; i++) {
    crc ^= buf[i];
    for (int b = 0; b < 8; b++)
      crc = (crc & 1) ? (crc >> 1) ^ 0xA001 : crc >> 1;
  }
  return crc;
}

void sendFrame(byte *frame, int len) {
  uint16_t crc = crc16(frame, len);
  digitalWrite(DE_RE, HIGH);          // 송신 모드
  rs485.write(frame, len);
  rs485.write(crc & 0xFF);            // CRC 하위 바이트 먼저
  rs485.write(crc >> 8);
  rs485.flush();
  digitalWrite(DE_RE, LOW);           // 수신 모드
  delay(50);
  while (rs485.available()) rs485.read();  // 응답 버퍼 비우기 (단순 예제)
}

// 16비트 레지스터 쓰기 (기능코드 0x06)
void writeU16(uint16_t reg, uint16_t val) {
  byte f[6] = { SLAVE, 0x06,
                (byte)(reg >> 8), (byte)reg,
                (byte)(val >> 8), (byte)val };
  sendFrame(f, 6);
}

// 32비트 레지스터 쓰기 (기능코드 0x10)
// ※ SM-NK17은 하위 워드를 먼저 보냅니다
void writeI32(uint16_t reg, long val) {
  uint16_t lo = val & 0xFFFF;
  uint16_t hi = ((unsigned long)val >> 16) & 0xFFFF;
  byte f[11] = { SLAVE, 0x10,
                 (byte)(reg >> 8), (byte)reg,
                 0x00, 0x02, 0x04,
                 (byte)(lo >> 8), (byte)lo,
                 (byte)(hi >> 8), (byte)hi };
  sendFrame(f, 11);
}

void setup() {
  pinMode(DE_RE, OUTPUT);
  digitalWrite(DE_RE, LOW);
  rs485.begin(115200);
  delay(500);

  writeU16(0x009A, 60);       // 운전 속도 60rpm
  writeU16(0x0098, 200);      // 가속 200ms
  writeU16(0x0099, 200);      // 감속 200ms
}

void loop() {
  writeU16(0x00C8, 1);        // 정방향 회전 시작
  delay(3000);
  writeU16(0x00C8, 0);        // 감속 정지
  delay(1000);

  writeU16(0x00C8, 257);      // 역방향 회전 시작
  delay(3000);
  writeU16(0x00C8, 0);        // 감속 정지
  delay(1000);

  // 참고) 위치 이동 예:
  // writeI32(0x00CE, 4000);   // 현재 위치에서 +1회전 (기본 4000펄스/회전)
  // writeI32(0x00D0, 0);      // 절대 위치 0으로 복귀
}