#include <plib.h>
#include <math.h>


void init_ADC(void) {
    AD1PCFG = 0x7FFF;    // all PORTB = Digital but RB15 (AN15) = analog
    AD1CON1 = 0x00E0;    // SSRC bits = 111 implies internal counter ends sampling and starts converting
    AD1CON2 = 0;
    AD1CON3 = 0x1F02;    // Sample time = 31 TPB, Convertion time = 12*TAD = 12*[2*(ADCS+1)] TPB = 12*6 TPB
    AD1CSSL = 0;
    AD1CHS = 0x000F0000; // Connect RB15/AN15 as CH0 input
    AD1CON1SET = 0x8000; // turn ADC ON
}

#define SAMP 1
#define DONE 0

void acquire_ADC(void)
{
    int v;

    AD1CON1SET = (1<<SAMP);         // start Sampling (SAMP = 1) and then start converting
    while (!(AD1CON1 & (1<<DONE))); // conversion done?
    v = ADC1BUF0;                   // yes then get ADC value

    data[0] = (v >> 8) & 0x000000FF;
    data[1] =  v       & 0x000000FF;
}

float distancia1( float T_infra1)
{
    float dist1;

    dist1 = 1/((T_infra1+ 0,3 -0,28)/17,77);

    return dist1;
}

float distancia2( float T_infra2)
{
    float dist2;

    dist2 = 1/((T_infra2+ 0,3 -0,28)/17,77);

    return dist2;
}

float decl(float dist1, float dist2, float dist_infra)
{
    float dist_nao_comum, declive;

    dist_nao_comum= dist1 - dist2;

            if(dist_nao_comum < 0)
            {
                dist_nao_comum = dist_nao_comum * (-1)
                declive = atan(dist_nao_comum/dist_infra);
             }
                if(dist_nao_comum > 0)
             declive = atan(dist_nao_comum/dist_infra);

                if(dist_nao_comum = 0)
        {

            declive = 0;
        }
    return declive;
}


int main(void) {

    float T_infra1, T_infra2, dist1, dist2, declive, dist_infra;

    TRISRCLR = 0x0001;  //definir como entrada led nº1
    TRISRCLR = 0x0002;//definir como entrada led nº2

    dist_infra = 10 // valor a confirmar, distancia entre os sensores


    while (1)
    {

        dist1 = distancia1(T_infra1);
        dist2 = distancia2(T_infra2);
        declive = decl(dist1, dist2, dist_infra);





    }
}