PROGRAM MAIN

' --- Variables generales ---
C = 0
N = 0
REM = 0
ADVROT = 0      ' 0: no se avanzó cinta en rotación; 1: ya se avanzó (debounce)

X1 = 0
Y1 = 0
A1 = 0
X2 = 0
Y2 = 0
A2 = 0
X3 = 0
Y3 = 0
A3 = 0
X4 = 0
Y4 = 0
A4 = 0
X5 = 0
Y5 = 0
A5 = 0
X6 = 0
Y6 = 0
A6 = 0
X7 = 0
Y7 = 0
A7 = 0
X8 = 0
Y8 = 0
A8 = 0
X9 = 0
Y9 = 0
A9 = 0
X10 = 0
Y10 = 0
A10 = 0

' --- Parámetros de movimiento ---
ZS = -53         ' bajada sobre pieza
Zcuna = -15      ' bajada en cuna

' --- Rotativa (3 cunas) ---
CAP = 3
CUNAIDX = 1
TimeAvance = 5

' --- Cinta transportadora ---
TAKEN = 0
TimeCinta = 3

' --- Selección temporal de pickup ---
PX = 0
PY = 0
PA = 0

' --- Prefetch (sostener pieza durante rotación) ---
HOLDING = 0
NPX = 0
NPY = 0
NPA = 0
J = 0

M = 0
I = 0

' --- Inicio ---
MOVE ESPERA

MAINLOOP:
PRINT IP2, "1", CR
INPUT IP2, N, X1, Y1, A1, X2, Y2, A2, X3, Y3, A3, X4, Y4, A4, X5, Y5, A5, X6, Y6, A6, X7, Y7, A7, X8, Y8, A8, X9, Y9, A9, X10, Y10, A10
delay 0.1
PRINT IP2, "0", CR

PRINT TP,"N=",N
IF N <= 0 THEN GOTO NOPIECES
GOTO HAVEPIECES

NOPIECES:
IF ADVROT > 0 THEN GOTO CLEARADVROT ELSE GOTO AVANZACINTA

CLEARADVROT:
ADVROT = 0
GOTO MAINLOOP

HAVEPIECES:
TAKEN = 0
M = N
IF M > 10 THEN M = 10

FOR I = 1 TO M

PX = 0
PY = 0
PA = 0

S1:
IF I > 1 THEN GOTO S2
PX = -Y1
PY =  X1
PA =  A1
GOTO PICKGUARD

S2:
IF I > 2 THEN GOTO S3
PX = -Y2
PY =  X2
PA =  A2
GOTO PICKGUARD

S3:
IF I > 3 THEN GOTO S4
PX = -Y3
PY =  X3
PA =  A3
GOTO PICKGUARD

S4:
IF I > 4 THEN GOTO S5
PX = -Y4
PY =  X4
PA =  A4
GOTO PICKGUARD

S5:
IF I > 5 THEN GOTO S6
PX = -Y5
PY =  X5
PA =  A5
GOTO PICKGUARD

S6:
IF I > 6 THEN GOTO S7
PX = -Y6
PY =  X6
PA =  A6
GOTO PICKGUARD

S7:
IF I > 7 THEN GOTO S8
PX = -Y7
PY =  X7
PA =  A7
GOTO PICKGUARD

S8:
IF I > 8 THEN GOTO S9
PX = -Y8
PY =  X8
PA =  A8
GOTO PICKGUARD

S9:
IF I > 9 THEN GOTO S10
PX = -Y9
PY =  X9
PA =  A9
GOTO PICKGUARD

S10:
PX = -Y10
PY =  X10
PA =  A10
' cae a PICKGUARD

' --- PICK con guardia de HOLDING ---
PICKGUARD:
IF HOLDING > 0 THEN GOTO PICKSKIP

PRINT TP,"PICK I=",I," PX=",PX," PY=",PY," PA=",PA
MOVE ORIGEN + POINT(PX, PY, 0, PA-90)
DOUT(2)
MOVE ORIGEN + POINT(PX, PY, ZS, PA-90)
MOVE ORIGEN + POINT(PX, PY, 0, PA-90)
GOTO PICKEND

PICKSKIP:
PRINT TP,"PICK SALTADO (sostengo prefeteada)"

PICKEND:

PRINT TP,"CUNAIDX=",CUNAIDX
IF CUNAIDX > 2 THEN GOTO TCUNA3
IF CUNAIDX > 1 THEN GOTO TCUNA2
GOTO TCUNA1

TCUNA1:
MOVE CUNA1
MOVE CUNA1 + POINT(0, 0, Zcuna)
DOUT(-2)
MOVE CUNA1
HOLDING = 0
GOTO AFTERCUNA

TCUNA2:
MOVE CUNA2
MOVE CUNA2 + POINT(0, 0, Zcuna)
DOUT(-2)
MOVE CUNA2
HOLDING = 0
GOTO AFTERCUNA

TCUNA3:
MOVE CUNA3
MOVE CUNA3 + POINT(0, 0, Zcuna)
DOUT(-2)
MOVE CUNA3
HOLDING = 0
' cae a AFTERCUNA

AFTERCUNA:
TAKEN = TAKEN + 1

CUNAIDX = CUNAIDX + 1
PRINT TP,"CUNAIDX->",CUNAIDX

IF CUNAIDX <= CAP THEN GOTO CONT1

' --- Fase de rotación ---
' Si quedan piezas en el lote (TAKEN < M) => PREFETCH: tomar la siguiente y esperar en ESPERA con la pieza.
IF TAKEN < M THEN GOTO ROTWAITPREF
' Si NO quedan piezas => avanzar cinta durante la rotación.
GOTO ROTWAITADV

' ===== PREFETCH DURANTE ROTACIÓN (espera en ESPERA) =====
ROTWAITPREF:
J = I + 1
IF J <= M THEN GOTO PREFSEL ELSE GOTO ROTWAITONLY

PREFSEL:
NPX = 0
NPY = 0
NPA = 0

J1:
IF J > 1 THEN GOTO J2
NPX = -Y1
NPY =  X1
NPA =  A1
GOTO PREFDO

J2:
IF J > 2 THEN GOTO J3
NPX = -Y2
NPY =  X2
NPA =  A2
GOTO PREFDO

J3:
IF J > 3 THEN GOTO J4
NPX = -Y3
NPY =  X3
NPA =  A3
GOTO PREFDO

J4:
IF J > 4 THEN GOTO J5
NPX = -Y4
NPY =  X4
NPA =  A4
GOTO PREFDO

J5:
IF J > 5 THEN GOTO J6
NPX = -Y5
NPY =  X5
NPA =  A5
GOTO PREFDO

J6:
IF J > 6 THEN GOTO J7
NPX = -Y6
NPY =  X6
NPA =  A6
GOTO PREFDO

J7:
IF J > 7 THEN GOTO J8
NPX = -Y7
NPY =  X7
NPA =  A7
GOTO PREFDO

J8:
IF J > 8 THEN GOTO J9
NPX = -Y8
NPY =  X8
NPA =  A8
GOTO PREFDO

J9:
IF J > 9 THEN GOTO J10
NPX = -Y9
NPY =  X9
NPA =  A9
GOTO PREFDO

J10:
NPX = -Y10
NPY =  X10
NPA =  A10
' cae a PREFDO

PREFDO:
PRINT TP,"PREFETCH J=",J," NPX=",NPX," NPY=",NPY," NPA=",NPA
MOVE ORIGEN + POINT(NPX, NPY, 0, NPA)
DOUT(2)
MOVE ORIGEN + POINT(NPX, NPY, ZS, NPA)
MOVE ORIGEN + POINT(NPX, NPY, 0, NPA)
HOLDING = 1
PRINT TP,"ROTATIVA: ESPERAR EN ESPERA CON PIEZA"
MOVE ESPERA2
DELAY TimeAvance
CUNAIDX = 1
ADVROT = 0
PRINT TP,"CUNAIDX->",CUNAIDX
GOTO CONT1

' ===== AVANCE DE CINTA DURANTE ROTACIÓN (sin piezas restantes) =====
ROTWAITADV:
PRINT TP,"ROTATIVA: ESPERA CON AVANCE DE CINTA (FIN DE LOTE)"
MOVE ESPERA
DOUT(1)
DELAY TimeCinta
DOUT(-1)
REM = TimeAvance - TimeCinta
IF REM > 0 THEN GOTO ROTEXTRA ELSE GOTO ROTDONE

ROTEXTRA:
DELAY REM
GOTO ROTDONE

' ===== ESPERA SIMPLE (fallback) =====
ROTWAITONLY:
PRINT TP,"ROTATIVA: ESPERA SIN AVANCE"
MOVE ESPERA2
DELAY TimeAvance
GOTO ROTDONE

ROTDONE:
CUNAIDX = 1
ADVROT = 1
PRINT TP,"CUNAIDX->",CUNAIDX
GOTO CONT1

CONT1:
NEXT I

GOTO FINCHK

FINCHK:
IF TAKEN >= M THEN GOTO FINCHKDECIDE
GOTO MAINLOOP

FINCHKDECIDE:
IF ADVROT > 0 THEN GOTO CLEAADVROT2 ELSE GOTO AVANZACINTA

CLEAADVROT2:
ADVROT = 0
GOTO MAINLOOP

AVANZACINTA:
PRINT TP,"CINTA: AVANZANDO ",TimeCinta," s"
DOUT(1)
DELAY TimeCinta
DOUT(-1)
GOTO MAINLOOP

END