import socket
import json
from pprint import pprint

from func import *

s = socket.socket()
host = MODBUS_SERVER_HOST
port = 9760

s.connect((host, port))
# print(s)
def send_data(data):    
    s.send(str.encode(json.dumps(data)))
    response_data = s.recv(1024)
    response = json.loads(response_data)
    
    if data["reqType"] == 'query':
        # pprint({"request":data["queryAddr"], "response":response["queryData"]})
        return response["queryData"]
    elif data["reqType"] == 'command':
        # pprint({"request":data["cmdData"], "response":response["cmdReply"]})
        return response["cmdReply"]
    else:
        pprint(response)

# send_data({
#     "dsID": "www.hc-system.com.RemoteMonitor",
#     "reqType": "query",
#     "packID": "0",
#     "queryAddr": ["RemoteCmdLen"],
# })

# send_data({
#     "dsID": "www.hc-system.com.HCRemoteCommand",
#     "reqType": "AddRCC",
#     "emptyList": "1",
#     "packID": "0",
#     "instructions": [{"oneshot": "1", "action": "4", 
#                       "m0":"79.287", 
#                       "m1":"1.639",
#                       "m2":"-31.667",  
#                       "m3":"3.380",  
#                       "m4":"7.986",  
#                       "m5":"-25.592",  
#                       "m6":"0",  
#                       "m7":"0",  
#                       "ckStatus":"0xFF",  
#                       "speed":"11.0",
#                       "delay":"1.0",
#                       "coord":"0",
#                       "tool":"0",
#                       "smooth":"0",
#                       }],
# })
def make_addrcc_data(q, e="1"):
   return {"dsID": "www.hc-system.com.HCRemoteCommand","reqType":"AddRCC","emptyList":e,"packID":"0","instructions":q} 
def make_command_data(q):
    return {"dsID": "www.hc-system.com.RemoteMonitor","reqType":"command","packID":"0","cmdData":q}
def make_query_data(q):
    return {"dsID": "www.hc-system.com.RemoteMonitor","reqType":"query","packID":"0","queryAddr":q}

axis_coord = send_data(make_query_data(['axis-0','axis-1','axis-2','axis-3','axis-4','axis-5']))
world_coord = send_data(make_query_data(['world-0','world-1','world-2','world-3','world-4','world-5']))

print('axis', axis_coord)
print('world',  world_coord)
print('remote command count', send_data(make_query_data(['RemoteCmdLen'])))

def make_world_step(p):
    return {
        "oneshot": "1", "action": "10",
        "m0": str(float(world_coord[0]) + p[0]), "m1":str(float(world_coord[1]) + p[1]), "m2": str(float(world_coord[2]) + p[2]),
        "m3":world_coord[3], "m4":world_coord[4], "m5": world_coord[5],
        "m6":"0", "m7":"0",
        "delay":"0.0","speed":"20.0",
        "coord":"0","tool":"3",
        "smooth":"0", "ckStatus":"0x3F",
    }
k = 5
data = [
    (10*k, 10*k, 0),
    (0, 20*k, 0),
    (-10*k, 10*k, 0),
    (0, 0, 0),
    (0, 0, 10*k),
]
send_data(make_addrcc_data([make_world_step(p) for p in data]))
print('remote command count', send_data(make_query_data(['RemoteCmdLen'])))
send_data(make_command_data(['actionSingleCycle']))