/*
 * FailureTermination.cpp
 *
 *  Created on: 4 окт. 2019 г.
 *      Author: titov
 */

#include "FailureTermination.hh"

driver::safety::FailureTermination::FailureTermination(
        std::pmr::memory_resource * memory ) : gpios( Gpios::allocator_type(memory) ),
        pwms( Pwms::allocator_type(memory) ), terminated(false) {}

void driver::safety::FailureTermination::addTerminate( peripheral::IPwmOverride * pwm ) {
    pwms.push_back(pwm);

    if( terminated ) {
        pwm->setupOverride();
        pwm->overrideDisableOutput();
    }
}

void driver::safety::FailureTermination::addTerminate(
        peripheral::IGpioOverride * gpio, peripheral::IGpioOverride::TOutValue value  ) {
    gpios.push_back( std::make_pair(gpio, value) );

    if( terminated ) {
        gpio->setupOverride();
        gpio->setOverrideValue( value );
    }
}

void driver::safety::FailureTermination::terminate() {

    for( Pwms::iterator iter = pwms.begin(); iter != pwms.end(); ++iter ) {
        peripheral::IPwmOverride & pwm = **iter;

        pwm.setupOverride();
        pwm.overrideDisableOutput();
    }

    for( Gpios::iterator iter = gpios.begin(); iter != gpios.end(); ++iter ) {
        peripheral::IGpioOverride & gpio = *iter->first;

        gpio.setupOverride();
        gpio.setOverrideValue( iter->second );
    }

    terminated = true;

}

void driver::safety::FailureTermination::recovery() {

    terminated = false;

    for( Pwms::iterator iter = pwms.begin(); iter != pwms.end(); ++iter ) {
        peripheral::IPwmOverride & pwm = **iter;

        pwm.resetOverride();
    }

    for( Gpios::iterator iter = gpios.begin(); iter != gpios.end(); ++iter ) {
        peripheral::IGpioOverride & gpio = *iter->first;

        gpio.resetOverride();
    }

}

bool driver::safety::FailureTermination::isTerminated() const {

    return terminated;

}