#include "PpmRegen.h"

PpmRegen::PpmRegen(InterruptIn* interruptPort){
    ppmPin = interruptPort;
    
    last_channels[ROLL] = 0;
    last_channels[PITCH] = 0;
    last_channels[YAW] = 0;
    last_channels[THROTTLE] = 0;
    last_channels[AUX1] = 0;
    last_channels[AUX2] = 0;
    
    current_channel = 0;
    timer.start();
    ppmPin->fall(callback(this, &PpmRegen::fall));
}
    

void PpmRegen::getAllChannels(uint16_t all_channels[]){
    for(int i = 0; i < CHANNELS; i++){
        all_channels[i] = last_channels[i];   
    }           
    
}
            
void PpmRegen::fall(){
    __disable_irq();
    uint16_t time = timer.read_us();
    
    // we are in synchro zone
    if(time > 2500)
    {
        current_channel = 0;
        last_channels[ROLL] = channels[ROLL];
        last_channels[PITCH] = channels[PITCH];
        last_channels[YAW] = channels[YAW];
        last_channels[THROTTLE] = channels[THROTTLE];
        last_channels[AUX1] = channels[AUX1];
        last_channels[AUX2] = channels[AUX2];
      
        /*
        _roll->pulsewidth_us(    channels[ROLL]);
        _pitch->pulsewidth_us(   channels[PITCH]);
        _yaw->pulsewidth_us(     channels[YAW]);
        _throttle->pulsewidth_us(channels[THROTTLE]);
        _aux1->pulsewidth_us(    channels[AUX1]);
        _aux2->pulsewidth_us(    channels[AUX2]);
        */
    }
    else
    {
        channels[current_channel] = timer.read_us();  
        current_channel += 1;     
    }
    
    timer.reset();
    
    __enable_irq();
}

void PpmRegen::channel_correction(){
    corrections[ROLL] = 1500 - channels[ROLL];    
    corrections[PITCH] = 1500 - channels[PITCH];
    corrections[YAW] = 1500 - channels[YAW];
    corrections[THROTTLE] = 1010 - channels[THROTTLE];
    corrections[AUX1] = 1010 - channels[AUX1];
    corrections[AUX2] = 1010 - channels[AUX2];
}