sim=require'sim'

function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf,enable)
    for i=1,#handles,1 do
        targetConf[i]=targetConf[i]*math.pi/180
    end
    local params = {
        joints = handles,
        targetPos = targetConf,
        maxVel = maxVel,
        maxAccel = maxAccel,
        maxJerk = maxJerk,
    }
    sim.moveToConfig(params)

    if enable then
        sim.writeCustomStringData(gripperHandle,'activity','on')
    else
        sim.writeCustomStringData(gripperHandle,'activity','off')
    end
end

function sysCall_thread()
    modelBase=sim.getObject('..')
    gripperHandle=sim.getObject('../suctionCup_link2')
    motorHandles = {}
    for i=1,4,1 do
        motorHandles[i]=sim.getObject('../motor'..i)
    end
    auxMotor1=sim.getObject('../auxMotor1')
    auxMotor2=sim.getObject('../auxMotor2')
    local vel=22  
    local accel=40
    local jerk=80
    local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}

    while true do
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{50,0,0,45},false)
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{50,50,47,0},true)
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{50,0,0,45},true)
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{50,50,47,45},false)
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{50,0,0,45},false)
        moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,0,0,0},false)
    end
end

function sysCall_joint(inData)
    if inData.handle==auxMotor1 then
        local t2=sim.getJointPosition(motorHandles[2])
        local t3=sim.getJointPosition(motorHandles[3])
        error=t3-t2-inData.pos
    end
    if inData.handle==auxMotor2 then
        local t3=sim.getJointPosition(motorHandles[3])
        error=-t3-inData.pos
    end

    local ctrl=error*20
    local maxVelocity=ctrl
    if (maxVelocity>inData.maxVel) then
        maxVelocity=inData.maxVel
    end
    if (maxVelocity<-inData.maxVel) then
        maxVelocity=-inData.maxVel
    end
    local forceOrTorqueToApply=inData.maxForce

    local outData={vel=maxVelocity,force=forceOrTorqueToApply}
    return outData
end