#include "sick_front.hpp"

Sick_front::Sick_front(bool channel):channel(channel){
    wiringPiSPISetupMode(channel, 500000, 3);
    wiringPiSetup();
    pinMode(GPIO_conversion_start, OUTPUT);
    pinMode(GPIO_laser_enable, OUTPUT);
}

optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
    unsigned int sum = 0;
    unsigned int successful_measurements = 0;
    for(unsigned int i = 0; i < measurement_count; i++){
        auto distance = Distance();
        if(distance){
            sum += *distance;
            successful_measurements++;
        }
    }
    if(sum == 0){
        return {};
    }
    return sum/successful_measurements;
}

optional<unsigned int> Sick_front::Distance(){
    auto raw = Conversion_raw();
    float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
    // Calculation of true voltage from reference voltage and ADC value
    double voltage = differential_raw / (1 << 16) * reference_voltage;
    //cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
    // Out of Range
    if((voltage < voltage_close) | (voltage > voltage_far)){
        return {};
    }

    double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
    double distance = round((range_max-range_min)*range_percentage+range_min);
    return distance;
}

uint16_t Sick_front::Conversion_raw(){
    // Start conversion
    digitalWrite(GPIO_conversion_start, 1);
    array<uint8_t, 2> received_data = {0x00, 0x00};
    wiringPiSPIDataRW(channel, received_data.data(), 2);
    // Start conversion
    digitalWrite(GPIO_conversion_start, 0);
    return ((received_data[0]<<8) | received_data[1]);
}

void Sick_front::Laser(bool state){
    if(state){
        // ON
        pinMode(GPIO_laser_enable, INPUT);
    } else {
        // OFF
        pinMode(GPIO_laser_enable, OUTPUT);
        digitalWrite(GPIO_laser_enable, 0);
    }
}
