component turret;
// ----Input pins---
pin in bit sensor0;
pin in bit sensor1;
pin in bit sensor2;
pin in bit sensor3;
pin in s32 target;      //target pocket from tool table
pin in bit start;       // trigger to start tool change
// ---Output pins---
pin out bit valve;      //pneumatic valve control
pin out s32 position;   //current turret pocket (0-3)
pin out bit busy;      // turret moving
pin out bit done;       //turret finished move
pin out bit error;      //turret fault
// ---Parameters---
param rw float stepTime = 2.0;      // max wait for turn
param rw float settleTime = 0.5;    // max wait for drop
param rw float sensorTimeout = 0.5; // max wait for sensor
// ---Internal variables---
variable int state = 0;
variable int stepsRemaining = 0;
variable float timer = 0;
// ---Function name---
function turret;
license "GPL";
;;
FUNCTION(turret) {

    int sensorCount = sensor0 + sensor1 + sensor2 + sensor3;

    // Determine current position
    if (sensorCount == 1){
        if (sensor0) position = 0;
        else if (sensor1) position = 1;
        else if (sensor2) position = 2;
        else if (sensor3) position = 3;
    }
    switch(state) {

        case 0: // Idle
            valve = 0;
            busy = 0;
            done = 0;
            if (start) {

                if (sensorCount != 1){
                    error =1;
                    break;
                }
                if (target == position) {
                    done = 1;
                    break;
                }
                int diff = target - position;
                if (diff < 0) diff += 4;

                stepsRemaining = diff;
                busy = 1;
                state = 1;
                timer = 0;
            }
            break;

        case 1: //Activate valve ( index step)
            valve = 1;
            timer += fperiod;
            if (timer >= stepTime) {
                timer = 0;
                state = 2;
            }
            break;
        
        case 2: // Settle after movement
            valve = 0;
            timer += fperiod;

            if (timer >= settleTime) {
                timer = 0;
                state = 3;
            }
            break;
        case 3: //Wait for sensor confirmation
        timer += fperiod;

        if (sensorCount == 1) {
            stepsRemaining--;

            if (stepsRemaining > 0) {
                state = 1;
                timer = 0;
            } else {
                state = 0;
                done = 1;
            }
            
        }

        if (timer > sensorTimeout) {
            error = 1;
            state = 0;
            valve = 0;
        }

        if (sensorCount > 1) {
            error = 1;
            state = 0;
            valve = 0;
        }
        break;
    }
}