/*@!Encoding:1252*/
includes
{
  #include "CommonConst.CIN"
}

variables
{
  byte sAutoStart = 0; // > 0 if auto start of the server is enabled
  char sLwATableFile[100] = "ServerLwATable.txt";
  int  sFreezedHearbeatCounter = -1;

  int sNumOfReqForTestlabCertToIgnore;
  int sNumOfReqForManufactCertToIgnore;
  int sNumOfReqForManufactSeriesCertToIgnore;
  int sNumOfReqForDeviceCertToIgnore;
}

on start
{
  sysSetVariableInt(sysvar::TIMServer::svStart, 0);  
  @sysvar::TIMServer::svTIMServerFreezeState = 0;
  
  Iso11783IL_SetVerbosity(cVerbosityErrorsAndWarnings);
  Iso11783IL_TIMSetProperty("enableDemoMode", @svDemoModeActive);  
}

on sysvar sysvar::ISO11783::TIMServer::ILState
{
  if (sAutoStart && @this == 2) //online
  {
    Iso11783IL_TIMActivateServer();
  }
}

on sysvar sysvar::TIMServer::svStart
{
  if(@this > 0)
  {
    StartServer();
  }
  else
  {
    StopServer();
  }
}

on sysvar sysvar::TIMServer::svOperatorACK
{
  if(@this > 0)
  {
    Iso11783IL_TIMOperatorAcknowledge();
  }
}

on sysvar sysvar::TIMServer::svStoreLwAKeyTable
{
  if(@this > 0)
  {
    Iso11783IL_TIMSaveLwATable(sLwATableFile);
  }
}

on sysvar sysvar::TIMServer::svClearLwAKeyTable
{
  if(@this > 0)
  {
    Iso11783IL_TIMClearLwATable();
    Iso11783IL_TIMSaveLwATable(sLwATableFile);
  }
}

on sysvar sysvar::TIMServer::svContinue
{
  if(@this > 0)
  {
    Iso11783IL_TIMContinueConnection(TIMClient);
  }
}

on sysvar sysvar::TIMServer::svContinueAndFreeze
{
  if(@this > 0)
  {
    Iso11783IL_TIMContinueConnection(TIMClient);
  }
  
  // set new freeze state if state has been changed
  if (@sysvar::TIMServer::svTIMServerFreezeState > 0)
  {
    Iso11783IL_TIMFreezeConnection(TIMClient, @sysvar::TIMServer::svTIMServerFreezeState);  
  } 
}

on sysvar sysvar::TIMServer::svDisableNameCheck
{
  UpdateDeactivatedCertificateChecks();
}

on sysvar sysvar::TIMServer::svDisableICCCheck
{
  UpdateDeactivatedCertificateChecks();
}

on sysvar sysvar::TIMServer::svDisableFctCapibilitiesCheck
{
  UpdateDeactivatedCertificateChecks();
}

on sysvar sysvar::TIMServer::svDisableCRLCheck
{
  UpdateDeactivatedCertificateChecks();
}

on sysvar sysvar::TIMServer::svAuxValve1State
{
  if (@this != cAutomationEnabled)
  {
    // reset system variable
    sysSetVariableInt(sysvar::TIMServer::svAuxValve1ClientReadyToControl, 0);
  }
}

on sysvar sysvar::TIMServer::svRearPTOState
{
  if (@this != cAutomationEnabled)
  {
    // reset system variable
    sysSetVariableInt(sysvar::TIMServer::svRearPTOClientReadyToControl, 0);
  }
}

on sysvar sysvar::TIMServer::svVehicleSpeedSetByServer
{
  dword newValue;
  newValue = (@this - TIM12_TIMServer::VehicleSpeed.offset) / TIM12_TIMServer::VehicleSpeed.factor;
  Iso11783IL_TIMSetFunctionValue(cVehicleSpeed, newValue);
  @sysvar::TIMServer::svVehicleSpeed = @this;
}

on sysvar sysvar::TIMServer::svAuxValve1SetByServer
{
  dword newValue;
  newValue = (@this - TIM12_TIMServer::TIMAuxValveFlow.offset) / TIM12_TIMServer::TIMAuxValveFlow.factor;
  Iso11783IL_TIMSetFunctionValue(cAuxValve01, newValue);
  @sysvar::TIMServer::svAuxValve1 = @this;
}

on sysvar sysvar::TIMServer::svRearPTOSetByServer
{
  dword newValue;
  newValue = (@this -  TIM12_TIMServer::PTOSpeed.offset) / TIM12_TIMServer::PTOSpeed.factor;
  Iso11783IL_TIMSetFunctionValue(cRearPTO, newValue);
  @sysvar::TIMServer::svRearPTO = @this;
}

on sysvar sysvar::TIMServer::svVehicleSpeedState
{
  if (@this != cAutomationEnabled)
  {
    // reset system variable
    sysSetVariableInt(sysvar::TIMServer::svVehicleSpeedClientReadyToControl, 0);
  }
}
void StartServer()
{
  sNumOfReqForTestlabCertToIgnore        = @TIMServer::svNumOfIgnRqstForTestlab;
  sNumOfReqForManufactCertToIgnore       = @TIMServer::svNumOfIgnRqstForManufact;  
  sNumOfReqForManufactSeriesCertToIgnore = @TIMServer::svNumOfIgnRqstForManufactSeries;  
  sNumOfReqForDeviceCertToIgnore         = @TIMServer::svNumOfIgnRqstForDevice;  

  DefaultPreparationSequence_Server();

  Iso11783IL_TIMConnectSysVarToState(TIMClient, "TIMServer::svTIMServerState");

  Iso11783IL_TIMLoadLwATable(sLwATableFile);
  
  if(@sysvar::TIMServer::svTIMServerFreezeState > 0)
  {
    Iso11783IL_TIMFreezeConnection(TIMClient, @sysvar::TIMServer::svTIMServerFreezeState);
  }
  
  Iso11783IL_TIMActivateServer();
}

void StopServer()
{
  Iso11783IL_TIMDeactivateServer(1);
}

void DefaultPreparationSequence_Server()
{
  char buf[100];
  long value, implementedVersion;
  
  UpdateDeactivatedCertificateChecks();

  value = sysGetVariableInt(sysvar::TIMServer::svIsServerMaster);
  Iso11783IL_TIMSetProperty("isServerMaster", value);
  value = sysGetVariableInt(sysvar::TIMServer::svBootTime);
  Iso11783IL_TIMSetProperty("bootTime", value);
  value = sysGetVariableInt(sysvar::TIMServer::svMinimumVersion);
  Iso11783IL_TIMSetProperty("minimumVersion", value);
  implementedVersion = sysGetVariableInt(sysvar::TIMServer::svImplementedVersion);
  Iso11783IL_TIMSetProperty("implementedVersion", implementedVersion);
  if (implementedVersion <= 1)
  {
    if (@TIMServer::svSupportedFctVehicleSpeedChangeOfDirection == 1)
    {
      Write("TIM Server Master: Because implemented version is <= 1, the facility 'Vehicle change of direction' is disabled");
      @TIMServer::svSupportedFctVehicleSpeedChangeOfDirection = 0;
    }    
  }
  
  //Set product identification
  sysGetVariableString(sysvar::TIMServer::svProductIdentification, buf, elcount(buf));
  Iso11783IL_TIMSetProductIdentification(buf);

  //Set compliance information
  Iso11783IL_TIMSetComplianceCertification(@TIMServer::svCCITestProtocolPublicationYear, @TIMServer::svCCITestProtocolRevision, @TIMServer::svCCILaboratoryType, @TIMServer::svCCILaboratoryID, @TIMServer::svCCIReferenceNumber);
  
  // Set sequence of requested certficates
  if (Iso11783IL_TIMSetCertificateSequence(@TIMServer::svCertificateType1, @TIMServer::svCertificateType2, @TIMServer::svCertificateType3, @TIMServer::svCertificateType4) != 0)
  {
    Write("TIM Server: Failed to set certificate sequence");
  }
  
  //Set suported functions and their facilities
  SetSupportedFunctionFacilities();
   
  // Set root certificates
  Iso11783IL_TIMAddCertificate(cCertificateType_AEFRoot, "Certificates//root_ca.der");
  
  // Set test lab certifiates 
  Iso11783IL_TIMAddCertificate(cCertificateType_Testlab, "Certificates//aef_testlab.der");
  
  // Set manufacturer certificate
  sysGetVariableString(sysvar::TIMServer::svManufacturerCertificate, buf, elcount(buf));
  Iso11783IL_TIMAddCertificate(cCertificateType_Manufacturer, buf);
  
  // Set manufacturer series certificate
  sysGetVariableString(sysvar::TIMServer::svManufacturerSeriesCertificate, buf, elcount(buf));
  if (strncmp(buf, "-", elcount(buf)) == 0)
  {
    Iso11783IL_TIMRemoveCertificate(cCertificateType_ManufacturerSeries);
  }
  else if (strlen(buf) > 0)
  {
    Iso11783IL_TIMAddCertificate(cCertificateType_ManufacturerSeries, buf);
  }
  
  // Set evice certificate
  sysGetVariableString(sysvar::TIMServer::svDeviceCertificate, buf, elcount(buf));
  Iso11783IL_TIMAddCertificate(cCertificateType_Device, buf);
  
  // Set server private key
  sysGetVariableString(sysvar::TIMServer::svPrivateKey, buf, elcount(buf));
  Iso11783IL_TIMSetPrivateKey(buf);

  // Set CRL Signing
  Iso11783IL_TIMAddCertificate(cCertificateType_CRLSigning, "Certificates//crl_ca.der");
  
  // Set CRL
  Iso11783IL_TIMSetCRL(cCRLType_CRL, "Certificates//crl.der");
  Iso11783IL_TIMSetCRL(cCRLType_CRLSignCA, "Certificates//crl_sub_ca.der");
  
  // connect system variable to function states
  Iso11783IL_TIMConnectSysVarToFunctionState(cAuxValve01  , "TIMServer::svAuxValve1State");
  Iso11783IL_TIMConnectSysVarToFunctionState(cRearPTO     , "TIMServer::svRearPTOState");
  Iso11783IL_TIMConnectSysVarToFunctionState(cVehicleSpeed, "TIMServer::svVehicleSpeedState");
  
  // set value of the functions
  @TIMServer::svAuxValve1 = 0;
  @TIMServer::svAuxValve1SetByServer = 0;
  @TIMServer::svRearPTO = 0;
  @TIMServer::svRearPTOSetByServer = 0;
  @TIMServer::svVehicleSpeed = 0.0;
  @TIMServer::svVehicleSpeedSetByServer = 0.0;
  Iso11783IL_TIMSetFunctionValue(cAuxValve01, 0x7D80); // 0% flow
  Iso11783IL_TIMSetFunctionValue(cRearPTO, 0x7DA0); // stop
  Iso11783IL_TIMSetFunctionValue(cVehicleSpeed, 0x7D80); // stop
}

void SetSupportedFunctionFacilities()
{
  Iso11783IL_TIMResetAllSupportedFunctions();
  // Auxiliary Values
  Iso11783IL_TIMSetSupportedFacility("auxiliaryValve1State ", @TIMServer::svSupportedFctAuxiliaryValve1State);
  Iso11783IL_TIMSetSupportedFacility("auxiliaryValve1Flow ",  @TIMServer::svSupportedFctAuxiliaryValve1Flow);
  // Rear PTO
  Iso11783IL_TIMSetSupportedFacility("rearPTODisengagement",              @TIMServer::svSupportedFctRearPTODisengagement);
  Iso11783IL_TIMSetSupportedFacility("rearPTOEngagementCounterClockwise", @TIMServer::svSupportedFctRearPTOEngagementCounterClockwise);
  Iso11783IL_TIMSetSupportedFacility("rearPTOEngagementClockwise",        @TIMServer::svSupportedFctRearPTOEngagementClockwise);
  Iso11783IL_TIMSetSupportedFacility("rearPTOSpeedCounterClockwise",      @TIMServer::svSupportedFctRearPTOSpeedCounterClockwise);
  Iso11783IL_TIMSetSupportedFacility("rearPTOSpeedClockwise",             @TIMServer::svSupportedFctRearPTOSpeedClockwise);
  // Vehicle Speed
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedForward",            @TIMServer::svSupportedFctVehicleSpeedForward);
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedReverse",            @TIMServer::svSupportedFctVehicleSpeedReverse);
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedStartMotion",        @TIMServer::svSupportedFctVehicleSpeedStartMotion);
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedStopMotion",         @TIMServer::svSupportedFctVehicleSpeedStopMotion);
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedForwardDirection",   @TIMServer::svSupportedFctVehicleSpeedForwardDirection);
  Iso11783IL_TIMSetSupportedFacility("vehicleSpeedReverseDirection",   @TIMServer::svSupportedFctVehicleSpeedReverseDirection);
  if (@TIMServer::svSupportedFctVehicleSpeedChangeOfDirection)
  {
    Iso11783IL_TIMSetSupportedFacility("vehicleSpeedChangeOfDirection", 1); // call only succeeds if implemented version is > 1      
  }
}


void UpdateDeactivatedCertificateChecks()
{
  long deactivatedChecks;
  
  deactivatedChecks = 0;
  deactivatedChecks |= (@TIMServer::svDisableICCCheck > 0)             ?  0x01 : 0;   
  deactivatedChecks |= (@TIMServer::svDisableNameCheck > 0)            ?  0x02 : 0;   
  deactivatedChecks |= (@TIMServer::svDisableFctCapibilitiesCheck > 0) ?  0x04 : 0;   
  deactivatedChecks |= (@TIMServer::svDisableCRLCheck > 0)             ?  0x08 : 0;   
  
  if (Iso11783IL_TIMSetProperty("deactivateCertificateValidation", deactivatedChecks) != 0)
  {
    write("Failed to set deactivated certificate validations");
  }
}

long Iso11783IL_OnTxPrepare( pg * txPG )
{ 
  switch( txPG.PGN ) {
  case TIM12.pgn:
    switch (txPG.byte(0))
    {
      case TIMServerStatus_Msg:
        if (sysGetVariableInt(sysvar::TIMServer::svFreezeServerHeartbeatCounter) > 0)
        {
          // if error injection is active we always send the same heartbeat counter
          if (sFreezedHearbeatCounter < 0)
          {
            sFreezedHearbeatCounter =  txPG.byte(1);
          }
          txPG.byte(1) = sFreezedHearbeatCounter;
        }
        else
        {
          // if error injection is deactivated we reset the heartbeat variable  
          sFreezedHearbeatCounter = -1;
        }
        break;
      default:
        break;
    }   
    break;
  default:
    break;
  }
    
  return 1; // send message
}

void Iso11783IL_TIMOnFunctionValueRequested (
  dword fctID, 
  dword requestedRawValue, 
  dword& calculatedRawValue, 
  dword& calculcatedExitReasonCode,
  dword& calculcatedAutomationStatus) 
{ 
  switch (fctID)
  {
     case cAuxValve01:
      if ((calculatedRawValue >= 0x0001) && (calculatedRawValue <= 0x7D7F)) // -128.508 to -0.004 % flow (Extend) 
      {
        if (calculatedRawValue < 0x1BD7)
        {
          calculatedRawValue = 0x1BD7; // -100%
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedLow;
        }     
      }
      else if ((calculatedRawValue >= 0x7D81) && (calculatedRawValue <= 0xFAFF)) // +0.004 to +128.508 % flow (Retract)
      {
        if (calculatedRawValue > 0xDF27) // 100%
        {
          calculatedRawValue = 0xDF27;
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedHigh;
        }
      }
      break;
    case cRearPTO:
      if ((calculatedRawValue >= 0x0001) && (calculatedRawValue <= 0x7D7F)) // speed counter-clockwise 
      {
        if (calculatedRawValue < 0x1FC0)
        {
          calculatedRawValue = 0x1FC0; // -3000 1/min
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedLow;
        }     
      }
      else if ((calculatedRawValue >= 0x7D81) && (calculatedRawValue <= 0xFAFF)) // speed clockwise
      {
        if (calculatedRawValue > 0xDB40) // 3000 1/min
        {
          calculatedRawValue = 0xDB40;
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedHigh;
        }
      }
      break;      
    case cVehicleSpeed:
      if ((calculatedRawValue >= 0x0001) && (calculatedRawValue <= 0x7D7F)) // reverse speed
      {
        if (calculatedRawValue < 0x1BD8)
        {
          calculatedRawValue = 0x1BD8;
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedLow;
        }     
      }
      else if ((calculatedRawValue >= 0x7D81) && (calculatedRawValue <= 0xFAFF)) // forward speed
      {
        if (calculatedRawValue > 0xDF25)
        {
          calculatedRawValue = 0xDF25; // 25 m/s
          calculcatedExitReasonCode = cReasonRemoteCommandOutOfRange;
          calculcatedAutomationStatus = cActiveLimitedHigh;
        }
      }
      break;
    default:
      break;
  }  
}

void Iso11783IL_TIMOnFunctionValueChanged(dword fctID, dword fctValueRaw)
{  
  switch (fctID)
  {
    case cAuxValve01:
      @TIMServer::svAuxValve1 = (fctValueRaw * 0.004) - 128.512;
      break;     
    case cRearPTO:
      @TIMServer::svRearPTO = (fctValueRaw * 0.125) - 4016.0;
      break;    
    case cVehicleSpeed:
      @TIMServer::svVehicleSpeed = (fctValueRaw * 0.001) -32.128;
      break;
    default:
      break;
  }
}

Iso11783IL_TIMOnClientReadyToControl(dword functionID, dword clientAddress) 
{
  switch (functionID)
  {
    case cAuxValve01:
      sysSetVariableInt( sysvar::TIMServer::svAuxValve1ClientReadyToControl, 1 );
      break;
    case cRearPTO:
      sysSetVariableInt( sysvar::TIMServer::svRearPTOClientReadyToControl, 1 );
      break;
    case cVehicleSpeed:
      sysSetVariableInt( sysvar::TIMServer::svVehicleSpeedClientReadyToControl, 1 );
      break;    
    default:
      break;
  }
}

void Iso11783IL_TIMOnFunctionStateChanged(dword functionID, dword newState) 
{
  switch (newState)
  {
    case 4: // // Automation Pending
      switch(functionID)
      {
        case cAuxValve01:
          break;
        case cRearPTO:
          break;
        case cVehicleSpeed:
          break;
        default:
          break;
      }
    default:
      break;
  }
}

long Iso11783IL_OnRxMessage(pg * rxPG)
{
  if((rxPG.pgn == AUTH21.pgn) && (rxPG.byte(0) == 2)) // Server Certificate Request
  {
    switch(rxPG.Byte(4))
    {
      case 1: // testlab
      {
        if(sNumOfReqForTestlabCertToIgnore > 0)
        {
          sNumOfReqForTestlabCertToIgnore--;
          return 0;
        }
        else
        {
          return 1;
        }
      }
      break;

      case 2: // Manufacturer
      {
        if(sNumOfReqForManufactCertToIgnore > 0)
        {
          sNumOfReqForManufactCertToIgnore--;
          return 0;
        }
        else
        {
          return 1;
        }
      }
      break;
      
      case 3: // Manufacturer Series
      {
        if(sNumOfReqForManufactSeriesCertToIgnore > 0)
        {
          sNumOfReqForManufactSeriesCertToIgnore--;
          return 0;
        }
        else
        {
          return 1;
        }
      }
      break;
      
      case 4: // Device
      {
        if(sNumOfReqForDeviceCertToIgnore > 0)
        {
          sNumOfReqForDeviceCertToIgnore--;
          return 0;
        }
        else
        {
          return 1;
        }
      }
      break;
    }
  }
  return 1;
}