// setp lcec.0.0.drivecontrol-1 1
// setp lcec.0.0.drivecontrol-2 1
// net state-op    <= lcec.state-op
// net X-amp       => lcec.0.0.drivecontrol-3 => lcec.0.0.drivecontrol-0
// net X-poscmd    => lcec.0.0.poscommand
// net X-posfb     <= lcec.0.0.pos
// net X-fault     <= lcec.0.0.drivestatus-3

// net X-stat-pos	<= lcec.0.0.modestatus-3
// net X-stat-home	<= lcec.0.0.modestatus-2 
// net X-homingok   <= lcec.0.0.drivestatus-12
// net X-athome	    <= lcec.0.0.drivestatus-10

// net X-ctrl-home	=> lcec.0.0.modecontrol-1 => lcec.0.0.modecontrol-2
// net X-homestart	=> lcec.0.0.drivecontrol-4
// net X-ctrl-pos	=> lcec.0.0.modecontrol-3

// net X-PoT       <= lcec.0.0.inputs-1
// net X-NoT       <= lcec.0.0.inputs-0

component lcec_simu "ESTUN drive simulator";
author "Electrolab";

pin out bit state_op;

pin in bit drivecontrol_0;
pin in bit drivecontrol_1;
pin in bit drivecontrol_2;
pin in bit drivecontrol_3;
pin in bit drivecontrol_4;

pin in float poscommand;
pin out float pos;

pin out bit drivestatus_3;
pin out bit drivestatus_10;
pin out bit drivestatus_12;

pin out bit modestatus_2;
pin out bit modestatus_3;

pin in bit modecontrol_0;
pin in bit modecontrol_1;
pin in bit modecontrol_2;
pin in bit modecontrol_3;
pin in bit modecontrol_4;

pin out bit inputs_0;
pin out bit inputs_1;

pin out float physical_pos = 50;
pin out s32 state;
pin out float home_to;
pin out bit home_dir;

function _;
license "GPL"; // indicates GPL v2 or later
;;

#include <math.h>

#define HOMING_POS 100
#define LATCH_LENGTH 5
#define HOMING_SPEED_MM_S 10
#define SERVO_PERIOD_MILI 1 

int process_homing(hal_float_t *_home_to, hal_float_t *_physical_pos, hal_float_t *_pos)
{
    float delta = *_home_to - *_physical_pos;
    
    if (fabs(delta) < 1e-7) return 1;   // Arrive at destination
    
    float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0;
    if (step > fabs(delta)) step = fabs(delta);
    if (delta < 0) step = -step;
    *_physical_pos += step;
    *_pos += step;  
    
    return 0;
}

FUNCTION(_) {
    state_op = 1;
    drivestatus_3 = 0;
    inputs_0 = 0;
    inputs_1 = 0;
    
    modestatus_2 = modecontrol_2;
    modestatus_3 = modecontrol_3;
            
    if (!drivecontrol_4) {
        home_to = HOMING_POS;
        if (physical_pos < home_to) {
            home_to += LATCH_LENGTH;
            home_dir = 1;
        }
        else {
            home_to -= LATCH_LENGTH;
            home_dir = 0;
        }
        state = 0;
    }
    
    if (drivecontrol_0 && drivecontrol_1 && drivecontrol_2 && drivecontrol_3) {     // Drive enable
        
        if (!modecontrol_2 && modecontrol_3) {  // Position mode
            drivestatus_12 = 0;
            physical_pos += poscommand - pos;
            pos = poscommand;
        }
        
        if (modecontrol_2 && !modecontrol_3 && drivecontrol_4) {  // Homing mode
            switch (state) {
            case 0: // idle
                state = 1;
                break;
            case 1: // home
                if (process_homing(&home_to, &physical_pos, &pos)) {
                    if (home_dir) home_to -= LATCH_LENGTH;
                    else home_to += LATCH_LENGTH;
                    state = 2;
                }
                break;
            case 2: // latch
                if (process_homing(&home_to, &physical_pos, &pos)) {
                    pos = 0;
                    state = 3;
                }
                break;
            case 3: // homed
                drivestatus_12 = 1;
                break;
            }
        }
    }

}