 /********************************************************************************
 * Copyright (c) 2025-2026 ZF Friedrichshafen AG
 *
 * This program and the accompanying materials are made available under the 
 * terms of the Apache License Version 2.0 which is available at
 * https://www.apache.org/licenses/LICENSE-2.0
 *
 * SPDX-License-Identifier: Apache-2.0 
 ********************************************************************************/

/**
* @file model.cpp
* @date 2025-09-06 14:55:42
* This file defines the data link object between CAN and the V-API devices.
* This file was generated by the DBC utility from:
*   datalink_4doors_example.dbc
*   DBC file version: 1.0.0.1
*/

#include <memory>
#include <vector>
#include <iostream>
#include <fstream>
#include <support/timer.h>
#include "signal_identifier.h"
#include <support/signal_support.h>
#include <support/app_control.h>

sdv::core::CSignal g_signalDoor01LeftIsOpen;
sdv::core::CSignal g_signalDoor01RightIsOpen;
sdv::core::CSignal g_signalDoor02LeftIsOpen;
sdv::core::CSignal g_signalDoor02RightIsOpen;
sdv::core::CSignal g_signalLockDoor02Right;
sdv::core::CSignal g_signalLockDoor02Left;
sdv::core::CSignal g_signalLockDoor01Right;
sdv::core::CSignal g_signalLockDoor01Left;

// in case the simulation timer should be used
sdv::core::ITimerSimulationStep* g_pTimerSimulationStep;

std::unique_ptr<sdv::app::CAppControl> g_appcontrol;

bool InitializeAppControl(const std::string& resource, const std::string& configFileName)
{
    auto bResult = g_appcontrol->AddModuleSearchDir( resource );
    bResult &= g_appcontrol->Startup("");
    g_appcontrol->SetConfigMode();
    bResult &= g_appcontrol->AddConfigSearchDir( resource );

    if (!configFileName.empty())
    {
        bResult &= g_appcontrol->LoadConfig(configFileName.c_str()) == sdv::core::EConfigProcessResult::successful;
    }

    return bResult;
}

sdv::core::EConfigProcessResult RegisterAllSignals()
{
    std::string msg = "register all signals: ";
    sdv::core::CDispatchService dispatch;

    g_signalDoor01LeftIsOpen = dispatch.RegisterRxSignal("CAN_Input_L1.Door01LeftIsOpen");
    g_signalDoor01RightIsOpen = dispatch.RegisterRxSignal("CAN_Input_R1.Door01RightIsOpen");
    g_signalDoor02LeftIsOpen = dispatch.RegisterRxSignal("CAN_Input_L2.Door02LeftIsOpen");
    g_signalDoor02RightIsOpen = dispatch.RegisterRxSignal("CAN_Input_R2.Door02RightIsOpen");
    g_signalLockDoor02Right = dispatch.RegisterTxSignal("CAN_Output.LockDoor02Right",0);
    g_signalLockDoor02Left = dispatch.RegisterTxSignal("CAN_Output.LockDoor02Left",0);
    g_signalLockDoor01Right = dispatch.RegisterTxSignal("CAN_Output.LockDoor01Right",0);
    g_signalLockDoor01Left = dispatch.RegisterTxSignal("CAN_Output.LockDoor01Left",0);

    if (g_signalDoor01LeftIsOpen && g_signalDoor01RightIsOpen && g_signalDoor02LeftIsOpen && g_signalDoor02RightIsOpen && g_signalLockDoor02Right && g_signalLockDoor02Left
        && g_signalLockDoor01Right && g_signalLockDoor01Left)
    {
        return sdv::core::EConfigProcessResult::successful;
    }

    return sdv::core::EConfigProcessResult::failed;
}

bool ResetAllSignals()
{
    sdv::core::CDispatchService dispatch;

    if (g_signalDoor01LeftIsOpen)
    {
        g_signalDoor01LeftIsOpen.Reset();
    }
    if (g_signalDoor01RightIsOpen)
    {
        g_signalDoor01RightIsOpen.Reset();
    }
    if (g_signalDoor02LeftIsOpen)
    {
        g_signalDoor02LeftIsOpen.Reset();
    }
    if (g_signalDoor02RightIsOpen)
    {
        g_signalDoor02RightIsOpen.Reset();
    }
    if (g_signalLockDoor02Right)
    {
        g_signalLockDoor02Right.Reset();
    }
    if (g_signalLockDoor02Left)
    {
        g_signalLockDoor02Left.Reset();
    }
    if (g_signalLockDoor01Right)
    {
        g_signalLockDoor01Right.Reset();
    }
    if (g_signalLockDoor01Left)
    {
        g_signalLockDoor01Left.Reset();
    }

    SDV_LOG_INFO("Reset signals");
    return true;
}


bool CreateCoreServiceTomlFile(const std::string& resources)
{
    std::ofstream tomlFile("sdv_core_reloc.toml");
    if (tomlFile.is_open())
    {
        tomlFile << R"toml(# Location of the SDV binaries and configuration files
[CoreLocation]
Version = 100
Framework = ")toml";
        tomlFile << resources;
        tomlFile << "\"\n";
        tomlFile.close();
        return true;
    }
    return false;
}


bool OpenAPILoad(const std::string& resources)
{
    bool success = CreateCoreServiceTomlFile(resources);
    g_appcontrol =  std::make_unique<sdv::app::CAppControl> ();

//
// TODO: Dispatch service must be loaded first, adjust the correct toml file
//
    success &= InitializeAppControl(resources, "data_dispatch_config_file.toml");
    if (!success)
    {
        std::cout << "Error: InitializeAppControl() failed" << std::endl;
        SDV_LOG_ERROR("Failed InitializeAppControl");
        return false;
    }
    success &= RegisterAllSignals() == sdv::core::EConfigProcessResult::successful;
    if (!success)
    {
        SDV_LOG_ERROR("Signals could not be registered");
    }

//
//
// TODO: Load all configurations files
//
//
    // Get the simulation task timer service if the simulation timer should be used
    success &= g_appcontrol->LoadConfig("simulation_task_timer_config_file.toml") == sdv::core::EConfigProcessResult::successful;
    g_pTimerSimulationStep = sdv::core::GetObject<sdv::core::ITimerSimulationStep>("SimulationTaskTimerService");
    if (!g_pTimerSimulationStep)
    {
        SDV_LOG_WARNING("Simulation timer step not available, use normal task timer ");
        success &= g_appcontrol->LoadConfig("task_timer_config_file.toml") == sdv::core::EConfigProcessResult::successful;
    }

    success &= g_appcontrol->LoadConfig("front_left_door_example.toml") == sdv::core::EConfigProcessResult::successful;
    success &= g_appcontrol->LoadConfig("front_right_door_example.toml") == sdv::core::EConfigProcessResult::successful;
    success &= g_appcontrol->LoadConfig("rear_left_door_example.toml") == sdv::core::EConfigProcessResult::successful;
    success &= g_appcontrol->LoadConfig("rear_right_door_example.toml") == sdv::core::EConfigProcessResult::successful;
    success &= g_appcontrol->LoadConfig("door_complex_service.toml") == sdv::core::EConfigProcessResult::successful;

    g_appcontrol->SetRunningMode();
    return success;
}

void OpenAPIShutdown()
{
    ResetAllSignals();
}
#ifdef __cplusplus
extern "C" {
#endif

#include <float.h>  // for DBL_EPSILON
#include <math.h>   // for fabs()
#include "config.h"
#include "model.h"

Status cleanup(ModelInstance*)
{
    SDV_LOG_INFO("Shutting down...");
    OpenAPIShutdown();
    return OK;
}


bool setStartValues(ModelInstance* comp)
{
    std::string path(comp->resourceLocation);
    std::string resourcePath = path.substr(8);
    std::replace(resourcePath.begin(), resourcePath.end(), '\\', '/');
    if (!OpenAPILoad(resourcePath))
    {
        std::cout << "Error: OpenAPILoad() failed." << std::endl;
        comp->terminateSimulation = true;
        return false;
     }

     // TODO: move this to initialize()?
     comp->nextEventTime = 0;
     comp->nextEventTimeDefined = true;
     return true;
}

Status calculateValues(ModelInstance* comp)
{
    UNUSED(comp);
    // nothing to do
    return OK;
}

Status getFloat64(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] double values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
{
    ASSERT_NVALUES(1);

    return OK;
}

Status getInt32(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] int32_t values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
{
    ASSERT_NVALUES(1);

        switch (vr)
        {
            case vr_Door01LeftIsOpen:
                values[(*index)++] = M(Door01LeftIsOpen);
                break;
            case vr_Door01RightIsOpen:
                values[(*index)++] = M(Door01RightIsOpen);
                break;
            case vr_Door02LeftIsOpen:
                values[(*index)++] = M(Door02LeftIsOpen);
                break;
            case vr_Door02RightIsOpen:
                values[(*index)++] = M(Door02RightIsOpen);
                break;
            case vr_LockDoor02Right:
                values[(*index)++] = M(LockDoor02Right);
                break;
            case vr_LockDoor02Left:
                values[(*index)++] = M(LockDoor02Left);
                break;
            case vr_LockDoor01Right:
                values[(*index)++] = M(LockDoor01Right);
                break;
            case vr_LockDoor01Left:
                values[(*index)++] = M(LockDoor01Left);
                break;

            default:
                logError(comp, "Get Int32 is not allowed for value reference u.", vr);
                return Error;
        }

    return OK;
}

Status setFloat64(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] const double values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
{
    ASSERT_NVALUES(1);

    return OK;
}

Status setInt32(ModelInstance* comp, [[maybe_unused]] ValueReference vr, [[maybe_unused]] const int32_t values[], [[maybe_unused]] size_t nValues, [[maybe_unused]] size_t* index)
{
    ASSERT_NVALUES(1);

        switch (vr)
        {
            case vr_Door01LeftIsOpen:
                M(Door01LeftIsOpen) = values[(*index)++];
                break;
            case vr_Door01RightIsOpen:
                M(Door01RightIsOpen) = values[(*index)++];
                break;
            case vr_Door02LeftIsOpen:
                M(Door02LeftIsOpen) = values[(*index)++];
                break;
            case vr_Door02RightIsOpen:
                M(Door02RightIsOpen) = values[(*index)++];
                break;
            case vr_LockDoor02Right:
                M(LockDoor02Right) = values[(*index)++];
                break;
            case vr_LockDoor02Left:
                M(LockDoor02Left) = values[(*index)++];
                break;
            case vr_LockDoor01Right:
                M(LockDoor01Right) = values[(*index)++];
                break;
            case vr_LockDoor01Left:
                M(LockDoor01Left) = values[(*index)++];
                break;

            default:
                logError(comp, "Set Int32 is not allowed for value reference u.", vr);
                return Error;
        }

    return OK;
}

void eventUpdate(ModelInstance* comp)
{

        if (g_pTimerSimulationStep) // in case the simulation timer was used, maybe the step size has to be adjusted
        {
            g_pTimerSimulationStep->SimulationStep(1000);
        }

        g_signalDoor01LeftIsOpen.Write( M(Door01LeftIsOpen));
        g_signalDoor01RightIsOpen.Write( M(Door01RightIsOpen));
        g_signalDoor02LeftIsOpen.Write( M(Door02LeftIsOpen));
        g_signalDoor02RightIsOpen.Write( M(Door02RightIsOpen));
        M(LockDoor02Right) = g_signalLockDoor02Right.Read().get<uint32_t>();
        M(LockDoor02Left) = g_signalLockDoor02Left.Read().get<uint32_t>();
        M(LockDoor01Right) = g_signalLockDoor01Right.Read().get<uint32_t>();
        M(LockDoor01Left) = g_signalLockDoor01Left.Read().get<uint32_t>();

    double epsilon = (1.0 + fabs(comp->time)) * DBL_EPSILON;

    if (comp->nextEventTimeDefined && comp->time + epsilon >= comp->nextEventTime) {
        comp->nextEventTime += FIXED_SOLVER_STEP;
    }

    comp->valuesOfContinuousStatesChanged = false;
    comp->nominalsOfContinuousStatesChanged = false;
    comp->terminateSimulation = false;
    comp->nextEventTimeDefined = true;
}

#ifdef __cplusplus
} // end of extern "C"
#endif

