# m500-2.py
#
# Run the ping-pong ball writing and scaffold picking
# operations.  Reads commands from bool_pv table.
#
# RLS 20250430

import mecademicpy.robot as mdr
import numpy as np
import psycopg2
from meca500 import *
#from pg_mods import read_bit, write_bit, open_db
from db import DB
from statemachine2 import StateMachine

def main():

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

    db = DB()           # open database

    do_ball = StateMachine()

    move_from_file(r,"home2")

    while True:

        do_ball.set_inputs(read_inputs(db))

        do_ball.state_change()
        #print("state=", do_ball.sm.state)
        #print("inputs=", do_ball.inputs)

        # do_scaff - remove the finished scaffold
        if do_ball.sm.state == "do_scaff":
                #move_from_file(r, "scaff_pick")
                pass

        # do_ball - proceed to write on a ball
        if do_ball.sm.state == "do_ball":
            # execute moves in ball_pick.mxprog
            move_from_file(r, "ball_pick")

            # be sure it is caught up
            checkpoint_1 = r.SetCheckpoint(1)
            checkpoint_1.wait(timeout=60)

            # execute moves in ball_pen_approach.mxprog
            move_from_file(r, "ball_pen_approach")

            checkpoint_2 = r.SetCheckpoint(2)
            checkpoint_2.wait(timeout=60)

            #### Do Writing ####

            arr = np.array([[0,  -10.5, 2.5],    # starting point
                    [1.1,    0,   0],
                    [  0,    2,  -5],    # first W
                    [  0,  1.5,   3],
                    [  0,  1.5,  -3],
                    [  0,    2,   5],
                    [-1.1,   0,   0],
                    [   0,   1,   0],    # second W
                    [ 1.1,   0,   0],
                    [   0,   2,  -5],
                    [   0, 1.5,   3],
                    [   0, 1.5,  -3],
                    [   0,   2,   5],
                    [-1.1,   0,   0],
                    [   0,   1,   0],    # U
                    [ 1.1,   0,   0],
                    [   0,   0,  -3],
                    [   0,   1,  -2],
                    [   0,   2,   0],
                    [   0,   1,   2],
                    [   0,   0,   3],
                    [-1.1,   0,   0],
                    [   0,   2,   2],
                    [ 1.1,   0,   0],   # box
                    [   0,   0,  -9],
                    [   0, -25,   0],
                    [   0,   0,   9],
                    [   0,  25,   0],
                    [-1.1,   0,   0],
                    ])

            l = len(arr)-1

            r.SetBlending(0)

            r.SetCartLinVel(10)
            for i in range(l):
                #print(i, arr[i,0], arr[i,1]*2.9, arr[i,2]*2.9)
                r.MoveLinRelTrf(0, 0, arr[i,0], arr[i,1]*2.9, arr[i,2]*2.9, 0)

            r.SetCartLinVel(20)

            # back off from pen
            move_from_file(r,"ball_pen_backoff")

            checkpoint_3 = r.SetCheckpoint(3)
            checkpoint_3.wait(timeout=60)

            #### End Writing ####

            # move to tube and drop ball
            move_from_file(r,"ball_drop")

        # update pick_done bit in database
        write_outputs(db, do_ball.get_outputs())

    # shouldn't reach here
    move_home(r)

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

def read_inputs(db):
    inputs = {}

    inputs["ball_rdy"] = db.read_bit("ball")

    return inputs

def write_outputs(db, b):
    db.write_bit("pick_done", b)

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