# m500-1.py
#
# Run the scaffold printing robot.
# Reads commands from bool_pv table.
#
# RLS 20250430

import mecademicpy.robot as mdr
import numpy as np
import psycopg2
from meca500 import *
from db import DB
#from transitions import Machine
from statemachine1 import StateMachine

def main():

    r = open_robot(1)   # get connection to meca500-2 robot

    db = DB()           # open database

    extrude = StateMachine()  # start statemachine

    move_from_file(r,"home1")

    while True:

        extrude.set_inputs(read_inputs(db))

        extrude.state_change()
        #print("state=",extrude.sm.state)
        #print("inputs=",extrude.inputs)
        match extrude.sm.state:
            case "idle":
                pass
            case "heat":
                start_heating(db,extrude.inputs)
            case "print":
                start_printing(db,extrude.inputs)
                mew_printlayer(r,db)
            case "wait":
                move_from_file(r,"home1")
                pass

        # 'scaff_rdy' is only output at present

        write_outputs(db,extrude.get_outputs())

    # shouldn't exit...

    move_from_file(r,"home1")

    r.WaitIdle()
    r.DeactivateRobot()
    r.Disconnect()

def mew_printlayer(robot,db):
    # spirograph pattern

    R = 1
    r = 0.91
    p = 0.075
    scale = 20
    
    k = r/R
    L = p/r

    t = np.arange(0, 100 * 6.28, 0.1)

    x = ((1-k)*np.cos(t) + L*k*np.cos(((1-k)/k)*t))
    y = ((1-k)*np.sin(t) - L*k*np.sin(((1-k)/k)*t))

    s = 0.5 * scale / np.max(x)
    x = s * x
    y = s * y
    x0 = x[0]
    y0 = y[0]
    dx = np.diff(x)
    dy = np.diff(y)
    nx = np.concatenate(([x0],dx))
    ny = np.concatenate(([y0],dy))

    # get close
    robot.MoveJoints(0,25,20,0,45,0)

    # starting zoff
    zoff = float(db.read_val("run_store", "zoff"))
    v = float(db.read_val("run_store", "vel"))
    new_zoff = zoff
    dz = 0.0

    check = 1

    #for idx in range(500):
    for idx in range(len(nx)):

        # check current parameters

        print("nx=",nx[idx]," ny=",ny[idx], " dz=",dz, "v=", v)

        if idx % 10 == 0:
            checkpoint = robot.SetCheckpoint(check)
            checkpoint.wait(timeout=60)
            check += 1
            new_zoff = float(db.read_val("run_store", "zoff"))
            v = float(db.read_val("run_store", "vel"))
            dz = new_zoff - zoff
            
        # update print velocity
        try:
            robot.SetCartLinVel(v)
        except Exception as e:
            print("setcartlinvel exception: ", e)

        try:
            robot.MoveLinRelWrf(nx[idx],ny[idx],dz,0,0,0)
        except Exception as e:
            print("movelinrelwrf exception: ", e)

#        zoff = new_zoff

    while True:
        pass

def read_inputs(db):
    inputs = {}
    inputs["pick_done"] = db.read_bit("pick_done")

    # read setpoints for:
    #   temp, hv, zoff, nlayers, print vel, and extrusion rate

    inputs["te_sp"] = te_sp = float(db.read_val("run_store", "te"))
    inputs["tb_sp"] = tb_sp = float(db.read_val("run_store", "tb"))

    # are we warmed up for printing?

    inputs["te_pv"] = te_pv = float(db.read_avg_temp("te_pv"))
    inputs["tb_pv"] = tb_pv = float(db.read_avg_temp("tb_pv"))
    # check if temp is within 10% of setpoint
    if te_sp > 0.0 and tb_sp > 0.0:
        tep = abs(te_sp - te_pv)/te_sp  # can't div by zero
        tbp = abs(tb_sp - tb_pv)/tb_sp  # can't div by zero
        if tep < 0.1 and tbp < 0.1:
            inputs["heated"] = True
            db.write_bit("heated", True)
            #print("avg te=",tep, " avg tb=",tbp)
    else:
        inputs["heated"] = False
        db.write_bit("heated", False)
    #print("heated=", inputs["heated"])


    inputs["hv"] = db.read_val("run_store", "hv")
    inputs["zoff"] = db.read_val("run_store", "zoff")
    inputs["nlayers"] = db.read_val("run_store", "nlayers")
    inputs["vel"] = db.read_val("run_store", "vel")
    inputs["rate"] = db.read_val("run_store", "rate")
    inputs["te_p"] = db.read_val("run_store", "te_p")
    inputs["te_i"] = db.read_val("run_store", "te_i")
    inputs["te_d"] = db.read_val("run_store", "te_d")
    inputs["tb_p"] = db.read_val("run_store", "tb_p")
    inputs["tb_i"] = db.read_val("run_store", "tb_i")
    inputs["tb_d"] = db.read_val("run_store", "tb_d")

    #print(inputs)
    return inputs

# get heating started
def start_heating(db, inputs):
    db.write_val("run_store", "te",inputs["te_sp"])
    db.write_val("run_store", "tb",inputs["tb_sp"])
    db.write_val("run_store", "te_p",inputs["te_p"])
    db.write_val("run_store", "te_i",inputs["te_i"])
    db.write_val("run_store", "te_d",inputs["te_d"])
    db.write_val("run_store", "tb_p",inputs["tb_p"])
    db.write_val("run_store", "tb_i",inputs["tb_i"])
    db.write_val("run_store", "tb_d",inputs["tb_d"])

# get printing started
def start_printing(db, inputs):
    db.write_val("run_store", "hv", inputs["hv"])
    db.write_val("run_store", "vel", inputs["vel"])
    db.write_val("run_store", "rate", inputs["rate"])
    db.write_val("run_store", "nlayers", inputs["nlayers"])
    db.write_val("run_store", "zoff", inputs["zoff"])

def write_outputs(db,outputs):
    db.write_bit("scaff_rdy", outputs["scaff_rdy"])

if __name__ == '__main__':
    sys.exit(main())