#ifndef _RC_H_
#define _RC_H_


#define RC

#include <PulsePosition.h>
PulsePositionInput ppmIn;

#define PPMREADPERIOD 20 //period for d4r-ii is 18ms
static int count=0;

#define PIN_PPM 9

#define PPM_CHANNELS 8
#define PPM_SAVE_MIN 950 //minimum allowed for valid data
#define PPM_SAVE_MAX 2050 //maximum allowed for valid data
#define PPM_MIN 1000
#define PPM_MAX 2000
#define PPM_MID 1500
#define PPM_DEADBAND 5
#define PPM_CHANNEL_THROTTLE 1
#define PPM_CHANNEL_STEER 2
#define PPM_CHANNEL_MODE 5
float ppm[PPM_CHANNELS];

unsigned long ppm_count_success=0;
unsigned long ppm_count_fail=0;

void initRC();
void readPPM(unsigned long loopmillis);
void printPPM();

int16_t getRCThrottle();
int16_t getRCSteer();
uint8_t getRCMode(uint8_t modes);

bool rc_enabled=false;

void initRC() {
  ppmIn.begin(PIN_PPM);
}

void readPPM(unsigned long loopmillis) {
    static unsigned long ppm_last_received=0;
    static unsigned long ppm_last_received_success=0;
    int ppm_num = ppmIn.available();
    if (ppm_num > 0) {
        bool ppm_ok=true;
        ppm_last_received=loopmillis;
        float new_ppm[PPM_CHANNELS];
        if (ppm_num != PPM_CHANNELS) {
            ppm_ok=false;
        }else{
            for (int i=1; i <= ppm_num; i++) {
                float val=ppmIn.read(i);
                new_ppm[i-1]=val;
                if (val<PPM_SAVE_MIN || val>PPM_SAVE_MAX) {
                    ppm_ok=false;
                }
            }
        }
        
        if (ppm_ok) {
            ppm_count_success+=1;
            ppm_last_received_success=loopmillis;
            std::copy(std::begin(new_ppm), std::end(new_ppm), std::begin(ppm));
        }else{
            ppm_count_fail+=1;
        }
    }
}

float getPPMSuccessrate() {
    float rate=ppm_count_success*1.0/(ppm_count_success+ppm_count_fail);
    if (ppm_count_success+ppm_count_fail>500) {
        ppm_count_success=0;
        ppm_count_fail=0;
    }
    return rate;
}

//Return value 0 - 1000
int16_t getRCThrottle() {
    return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
}

int16_t getRCBrake() {
    return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,1000), 0,1000);
}

//Return value -1000 - 1000
int16_t getRCSteer() {
    if (ppm[PPM_CHANNEL_STEER-1]>PPM_MID+PPM_DEADBAND){
        return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
    }else if (ppm[PPM_CHANNEL_STEER-1]<PPM_MID-PPM_DEADBAND){
        return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,-1000), -1000,0);
    }else{
        return 0;
    }
    
}

//Return descrete value for equidistant ranges. modes = number of ranges.
uint8_t getRCMode(uint8_t modes) {
    float v=constrain(map(ppm[PPM_CHANNEL_MODE-1],PPM_MIN,PPM_MAX,0.0,1.0), 0.0,0.999);
    return uint8_t(v*modes);
}


void printPPM(unsigned long loopmillis){
    
    count = count + 1;
    Serial.print("ms :");
    Serial.print(count);
    Serial.print(" :  ");
    for (int i=1; i <= PPM_CHANNELS; i++) {
        Serial.print(ppm[i-1]);
        Serial.print("  ");
    }
    Serial.println();
}

#endif