/*
 * Demux.cpp
 *
 *  Created on: 23 окт. 2019 г.
 *      Author: user
 */

#include "Demux.hh"

driver::Demux::Demux( peripheral::IPort & gpio_port,
                                             unsigned long pin_mask, SelectPinStrategy strategy ) :
        gpio_port(gpio_port), pin_mask(pin_mask), strategy(strategy) {}

void driver::Demux::select( unsigned short pin ) {

    if( pin_mask == 0 ) return;

//    for( ; ( bit_pos ) < 32 and ( pin != 0 ) ; bit_pos++ ) {
//        if( pin_mask & ( 1 << (bit_pos + pin) ) ) pin--;
//    }

//    if( pin == 0 ) {
//        gpio_port.clear( pin_mask );
//        return;
//    }

    unsigned short result_set_bit_mask = 0;

    for( unsigned short bit_pos = 0; (bit_pos < 32) && (pin != 0); bit_pos++ ) {
        if( pin_mask & (1 << bit_pos) ) {
            result_set_bit_mask |= (pin & 1) << bit_pos;
            pin >>= 1;
        }
    }

    switch( strategy ) {
    case clear_set : {
        gpio_port.clear( pin_mask );
        gpio_port.set( result_set_bit_mask );
    } break;
    case set_clear : {
        gpio_port.set( pin_mask );
        gpio_port.clear( ~result_set_bit_mask );
    } break;
    }

}