#include "lepton_i2c.h"


LEP_CAMERA_PORT_DESC_T port;
LEP_SYS_TELEMETRY_ENABLE_STATE_E state;

bool connected;

int lepton_connect(){

    LEP_RESULT result = LEP_OpenPort(1, LEP_CCI_TWI, 400, &port);

    if( result == LEP_OK ){
        connected = true;
        return 0;
    }
    else{
        return -1;
    }
}

int enable_telemetry(int header){

    if(!connected){
        lepton_connect();
    }

    LEP_RESULT tel_location;
    LEP_RESULT tel_enbl = LEP_SetSysTelemetryEnableState(&port, LEP_TELEMETRY_ENABLED);

    if(header == 0){
        tel_location = LEP_SetSysTelemetryLocation(&port, LEP_TELEMETRY_LOCATION_HEADER);
    }
    else if(header == 1){
        tel_location = LEP_SetSysTelemetryLocation(&port, LEP_TELEMETRY_LOCATION_FOOTER);
    }

    if(tel_enbl == LEP_OK && tel_location == LEP_OK){
        return 0;
    }else{
        return -1;
    }

}

bool telemetry_enabled(){

    if(!connected){
        lepton_connect();
    }

    LEP_GetSysTelemetryEnableState(&port, &state);

    if( state == LEP_TELEMETRY_ENABLED ){
        return true;
    }
    else{
        return false;
    }
}