package loader

import org.lsst.ccs.subsystems.fcs.*
import org.lsst.ccs.subsystems.fcs.common.*
import org.lsst.ccs.subsystems.fcs.drivers.*
import org.lsst.ccs.subsystems.fcs.simulation.*
import org.lsst.ccs.drivers.canopenjni.*
import org.lsst.ccs.drivers.canopenjni.rmi.*
import org.lsst.ccs.description.groovy.CCSBuilder

//********************************************************************************//
// This groovy file is for the loader prototype.
// It's different from the loader final hardware for the sensors :
// loaderEngagedSensors and loaderEngagedSensors
// In the prototype this sensors are not redondant while they are redondant for the final hardware.
//********************************************************************************//

//********************************************************************************//
//Loader variables
//********************************************************************************//
def bridgeToLoaderName = "loaderTcpProxy" // to monitor the loader sensors
def loaderPlutoGatewayName = "loaderPlutoGateway" // to monitor the loader sensors
//********************************************************************************//

CCSBuilder builder = ["loaderProto-standalone"]

builder.
    main ( LoaderMain, bridge:ref("$bridgeToLoaderName")
    )
{ //begin description of Main submodules

    // tcp Proxy
    "$bridgeToLoaderName"(CanOpenProxy, hardwareBootTimeout:3000, busName:"1", baud:"125k") {
        
        canInterface(CanOpenCommands, coi:new CanOpenRMIClient("134.158.155.194", "canbus1"))
        
        "$loaderPlutoGatewayName" (CanOpenPlutoGateway, 
            nodeID:1,serialNB:"22d6")
        
        // can open devices
        hooksController (CanOpenEPOS,
            nodeID:0x1b,serialNB:"79007141",
            paramsForCurrent:[
                ContinuousCurrentLimit:500,
                OutputCurrentLimit:6880,
                MaxSpeedInCurrentMode:750,
                MotorType:10,PolePairNumber:1,
                PositionSensorType:1],
            paramsForProfilePosition:[
                MinPositionLimit:-2147483648,
                MaxPositionLimit:537000,
                MotionProfileType:0],
            paramsForHoming:[HomeOffset:0]) 
                        
        carrierController (CanOpenEPOS,nodeID:0x1a,serialNB:"80003184",
            paramsForCurrent:[
                ContinuousCurrentLimit:7290,
                OutputCurrentLimit:14580,
                MaxSpeedInCurrentMode:9840,
                MotorType:10,
                PositionSensorType:4,
                ThermalTimeConstantWinding:187,
                PolePairNumber:1],
            paramsForProfilePosition:[
                MinPositionLimit:-2147483648,
                MaxPositionLimit:1037800,
                MotionProfileType:1,
                MaxFollowingError:2000,
                MaxProfileVelocity:228, 
                ProfileVelocity:200,
                ProfileAcceleration:30,
                ProfileDeceleration:8,
                QuickStopDeceleration:1000],
            paramsForHoming:[
                HomeOffset:1000,
                HomePosition:0,
                SpeedForSwitchSearch:10,
                SpeedForZeroSearch:10,
                HomingAcceleration:200,
                CurrentThresholdHomingMode:500])
        
        /*
         * QuickStopDeceleration=10000 SpeedForSwitchSearch=10 SpeedForZeroSearch=10 HomingAcceleration=200 CurrentThresholdHomingMode=500 HomePosition=0
         * */
    }//end description of bridge's childrens
    //----------------------------------------------
    // END Bridge To LOADER description
    //----------------------------------------------

    //signaux envoyés à l'autochanger par le loader:
    //LRH user_to_gateway 3 input 4
    //LPS user_to_gateway 3 input 5
                
    //begin description of loader
    loader (Loader)
    {//begin description of loader's children

        carrier(LoaderCarrier,
            handoffSensors:ref("loaderHandoffSensors"),
            storageSensors:ref("loaderStorageSensors"),
            engagedSensors:ref("loaderEngagedSensors"),
        )
        {//begin description of loader carrier's children
            //user_to_gateway n° 1
            loaderHandoffSensors (DigitalSensor,
                dioName:loaderPlutoGatewayName,inputNumero:7,byteNumero:3)
            //proto : input:1, byteNumero:6

            loaderEngagedSensors (DigitalSensor,
                dioName:loaderPlutoGatewayName,inputNumero:6,byteNumero:3)
            
            loaderStorageSensors (RedondantSensors,
                sensor:ref("loaderStoragePositionSensor0"), //Xminus
                sensorR:ref("loaderStoragePositionSensor1"), //Xplus
            )
            {//begin description of loader storageSensors
                //user_to_gateway n° 1
                loaderStoragePositionSensor0 ( DigitalSensor ,
                    dioName:loaderPlutoGatewayName, inputNumero:4,byteNumero:3)

                loaderStoragePositionSensor1 ( DigitalSensor ,
                    dioName:loaderPlutoGatewayName, inputNumero:5, byteNumero:3)  
            }//end description of loader storageSensors
        }//end description of loader carrier's children

        clamp(LoaderClamp,
            hook1:ref("hook1"),hook2:ref("hook2"), hook3:ref("hook3"),hook4:ref("hook4"),
            forceSensor0:ref("forceSensor0"), forceSensor1:ref("forceSensor1"),
            clampedStatusSensor: ref("clampedStatusSensor"),
            )
        {//begin description of loader clamp's children
            int[] inputNumeroClose = [4,5,6,7];
            int[] inputNumeroOpen = [0,1,2,3];
            for (int i = 1; i<5; i++) {
            
            "hook${i}"(LoaderHook,
                    closeSensor:ref("loaderCloseSensor${i}"), openSensor:ref("loaderOpenSensor${i}"))
                { //user_to_gateway n° 1
                    "loaderCloseSensor${i}" ( DigitalSensor , //closeHookSensor0
                        dioName:loaderPlutoGatewayName, inputNumero:inputNumeroClose[i-1], byteNumero:2)
                       
                    "loaderOpenSensor${i}" ( DigitalSensor , //openHookSensor0
                        dioName:loaderPlutoGatewayName, inputNumero:inputNumeroOpen[i-1], byteNumero:2)
                }
            }
            
            forceSensor0(ForceSensor, deviceName:loaderPlutoGatewayName, byteNumero:4)
            
            forceSensor1(ForceSensor, deviceName:loaderPlutoGatewayName, byteNumero:8)
            
            // user_to_gateway n° 2
            unclampedStatusSensor ( DigitalSensor ,
                dioName:loaderPlutoGatewayName,inputNumero:2,byteNumero:6)
            
            underClampedStatusSensor ( DigitalSensor ,
                dioName:loaderPlutoGatewayName,inputNumero:3,byteNumero:6)
            
            clampedStatusSensor ( DigitalSensor ,
                dioName:loaderPlutoGatewayName,inputNumero:4,byteNumero:6) 
            
            overClampedStatusSensor ( DigitalSensor ,
                dioName:loaderPlutoGatewayName,inputNumero:5,byteNumero:6)        
        }//end description of loader clamp's children

       
        loaderFilterPresenceSensors (RedondantSensors,
            sensor:ref("loaderFilterPresenceSensor0"),
            sensorR:ref("loaderFilterPresenceSensor1"),
        )
        { //user_to_gateway n° 1
            loaderFilterPresenceSensor0(DigitalSensor ,
                dioName:loaderPlutoGatewayName,inputNumero:0, byteNumero:3)

            loaderFilterPresenceSensor1(DigitalSensor ,
                dioName:loaderPlutoGatewayName, inputNumero:1, byteNumero:3) 
        }

        //autochanger presence
        loaderOnCameraSensors (RedondantSensors,
            sensor:ref("loaderOnCameraSensor0"),
            sensorR:ref("loaderOnCameraSensor1"),
        )
        { //user_to_gateway n° 1
            loaderOnCameraSensor0(DigitalSensor ,
                dioName:loaderPlutoGatewayName, inputNumero:2, byteNumero:3)

            loaderOnCameraSensor1(DigitalSensor ,
                dioName:loaderPlutoGatewayName, inputNumero:3, byteNumero:3)
        }
        //Signals comming from autochanger//
        //AP2 user_to_gateway 3 input 0
        //AF0 user_to_gateway 3 input 1
        //AF1 user_to_gateway 3 input 2
        //AF3 user_to_gateway 3 input 3
        acAP2 (ComplementarySensors,
            sensor:ref("acAP2s"),
            sensorC:ref("acAP2b"),
        ) {
            acAP2s (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:0, byteNumero:11)
            acAP2b (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:1, byteNumero:11)
        }
        
        acAF0 (ComplementarySensors,
            sensor:ref("acAF0s"),
            sensorC:ref("acAF0b"),
        ) {
            acAF0s (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:2, byteNumero:11)
            acAF0b (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:3, byteNumero:11)
        } 
        
        acAF1 (ComplementarySensors,
            sensor:ref("acAF1s"),
            sensorC:ref("acAF1b"),
        ) {
            acAF1s (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:4, byteNumero:11)
            acAF1b (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:5, byteNumero:11)
        } 
        
        acAF3 (ComplementarySensors,
            sensor:ref("acAF3s"),
            sensorC:ref("acAF3b"),
        ) {
            acAF3s (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:6, byteNumero:11)
            acAF3b (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:7, byteNumero:11)
        } 
        
        //key lock or permit signal
        keyLockSensors (ComplementarySensors,
            sensor:ref("keyLock"),
            sensorC:ref("keyLockb"),
        ) {
            keyLock (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:0, byteNumero:10)
            keyLockb (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:1, byteNumero:10)
        }
        
        //engineering key
        keyEngSensors (ComplementarySensors,
            sensor:ref("keyEng"),
            sensorC:ref("keyEngb"),
        ) {
            keyEng (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:2, byteNumero:10)
            keyEngb (DigitalSensor,
                dioName:loaderPlutoGatewayName, inputNumero:3, byteNumero:10)
        }
        //end of Signals comming from autochanger//            
    }//end description of loader's children
    
    /**********************************************************************/
    /*                FAKE AUTOCHANGER DESCRIPTION                       */
    /**********************************************************************/
    //in the loader standalone subsystem the fake autochanger is a switch which
    //simulate that autochanger holds a filter at HANDOFF 
    
    autochanger(FakeFilterHolder, holdingFilterSensorName:"acAF3")
    
    /**********************************************************************/
    /*                END of FAKE AUTOCHANGER DESCRIPTION                 */
    /**********************************************************************/
    
} //end description of LoaderMain

// user_gateway n° 2 (byte 6 et 7)
// byte6:
// 0 : handoffSensor0, 1: handoffSensor1, 
// 2: unclampedStatus, 3: underClampedStatus, 4: ClampedStatus, 5: overClampedStatus
// 6: carrierRelayStatus, 7: hookRelayStatus


                


