File content as of revision 85:0cc5931bb9ef:

#include "FIZReader.h"
#include "LTCApp.h"
#include "cc.h"
#include "mbed.h"
#include <cstdint>
#include <vector>
#include <algorithm>

FIZReader::FIZReader(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
{
    _focus = 0;
    _iris = 0;
    _zoom = 0;
    newData = false;
}

bool FIZReader::getMostRecent(uint32_t *focus, uint16_t *iris, uint16_t *zoom)
{
    *focus = _focus;
    *iris = _iris;
    *zoom = _zoom;
    bool wasNew = newData;
    newData = false;
    return wasNew;
}

int FIZReader::hexValue(char ascii) {
    if (((ascii-'0') >= 0) && ((ascii-'0') <= 9))
        return ascii-'0';
    if (((ascii-'A') >= 0) && ((ascii-'A') <= 5))
        return ascii-'A'+10;
    if (((ascii-'a') >= 0) && ((ascii-'a') <= 5))
        return ascii-'a'+10;
    return 0;
}

unsigned int FIZReader::getAbsoluteValue(unsigned int encoder_pos, vector<unsigned int> &encoder_range, vector<unsigned int> &absolute_range) {
    unsigned int result;
    if( encoder_pos <= encoder_range.front() )   return absolute_range.front();
    if( encoder_pos >= encoder_range.back())  return absolute_range.back();

    int lower = encoder_range.size() -2;
    int upper = encoder_range.size() -1;
    for (int pos_pointer = 0; pos_pointer < encoder_range.size(); pos_pointer++) {
        if (encoder_pos < encoder_range[pos_pointer]) {
            lower = pos_pointer - 1;
            upper = pos_pointer;
            break;
        }
    }

    // pos_upper = encoder_range[upper];
    // pos_lower = encoder_range[lower];
    // pos_value = encoder_pos;

    result = absolute_range[lower]+(encoder_pos-encoder_range[lower])*(absolute_range[upper]-absolute_range[lower])/(encoder_range[upper]-encoder_range[lower]);

    return result;

}

    // vector<int>::iterator lb = lower_bound(encoder_range.begin(), encoder_range.end(), encoder_pos);
    // auto upper = lb++;
    
    // if (lb == encoder_range.end())
    //     --upper;  // extrapolating above
    // else if (*lb == encoder_pos)
    //     return absolute_range[*lb];

    // int lower = upper? upper - 1: 1; // if upper is not zero, get value below, else flip and get the one above

    // if(encoder_range[upper]<encoder_range[lower]) 
    //     result = absolute_range[upper]+(encoder_pos-encoder_range[upper])*(absolute_range[lower]-absolute_range[upper])/(encoder_range[lower]-encoder_range[upper]);
    // else 
uint32_t FIZReader::getAbsoluteFocus(uint32_t encoder_pos) {
    if (UserSettings.focus_encoder_map.size())
        return (uint32_t) getAbsoluteValue(encoder_pos, UserSettings.focus_encoder_map, UserSettings.focus_absolute_map);
    else 
        return encoder_pos;
}
uint16_t FIZReader::getAbsoluteIris(uint16_t encoder_pos) {
    if (UserSettings.iris_encoder_map.size())
        return (uint16_t) getAbsoluteValue(encoder_pos, UserSettings.iris_encoder_map, UserSettings.iris_absolute_map);
    else 
        return encoder_pos;
}
uint16_t FIZReader::getAbsoluteZoom(uint16_t encoder_pos) {
    if (UserSettings.zoom_encoder_map.size()){
        return (uint16_t) getAbsoluteValue(encoder_pos, UserSettings.zoom_encoder_map, UserSettings.zoom_absolute_map);
    }
    else 
        return encoder_pos;
}