#include <stdio.h>
#include <stdlib.h>
#include <p32xxxx.h>
#define _SUPPRESS_PLIB_WARNING 1
#include <plib.h>
#include <math.h>

#define SYSCLK 80000000L // System clock frequency, in Hz
#define PBUSCLK 40000000L // Peripheral bus clock

void delay(unsigned int);
int ADC(int channel);
float calc_massa(int channel,int nConversions);
void initADC (void);
int timerPWM(void);
float calc_temp(int channel,int nConversions);
float calc_altura (int temperatura);
float media (float* array, int medidas, int ordem);

int main(int argc, char** argv) {

    TRISEbits.TRISE7 = 1;
    int i;
    float altura, temperatura, massa, imc;
    int channel_massa = 0, channel_temp = 1;
    int nConversions = 10;


    SYSTEMConfigPerformance(SYSCLK);
    mOSCSetPBDIV(OSC_PB_DIV_2);

    initADC();

   /* float array [8] = {5.1,5.2,6.0,5.3,6.2,1.0,20.4,4.5};
    printf("Media %4.1f \n",media(&array,num_medidas,ordem));

    while(1);*/


    while(1) {
    temperatura= calc_temp(channel_temp, nConversions);
    printf("Temperatura %4.1f (K) \n",temperatura);
    altura=calc_altura(temperatura);
    printf("Altura %4.2f (m) \n", altura);
    massa=calc_massa (channel_massa, nConversions);
    printf("Peso %4.1f (Kg) \n",massa);
    imc = massa/(altura*altura);
    printf("IMC %4.1f f\n",imc);

    if (imc < 16) printf("Magreza grave\r");
    else if (imc >= 16 && imc <17) printf("Magreza moderada\r");
    else if (imc >= 17 && imc <18.5) printf("Magreza leve\r");
    else if (imc >= 18.5 && imc <25) printf("Saudavel\r");
    else if (imc >= 25 && imc <30) printf("Sobrepeso\r");
    else if (imc >= 30 && imc <35) printf("Obsidade Grau I\r");
    else if (imc >= 35 && imc <40) printf("Obsidade Grau II (Severa)\r");
    else if (imc >= 40) printf("Obsidade Grau III (Morbida)\r");

    printf("\n");
    Delay_ms(300);
    }
}

float calc_massa(int channel,int nConversions){
    int i;
    float s = 0;
    float res[nConversions];
    int medidas=20, ordem=2;

    for (i=0 ; i < nConversions; i++){
        res[i] = (float) ADC(channel);
    }

    for(i=0;i<nConversions;i++) {
        s=s+res[i];
    }

    s=media(&res,medidas,ordem);
    int valor_ADC = s / nConversions;
    float valor_ADC_float=0.0+valor_ADC;
    valor_ADC_float = ((valor_ADC_float*3.3)/1023);
    float massa = ((valor_ADC_float-0.4942)/0.0109);
    return massa+1.0;
}

float calc_temp(int channel,int nConversions){
    int res[nConversions];
    int i;
    float s = 0;
    for(i=0 ; i < nConversions ; i++){
        res[i] = ADC(channel);
     }
    for(i=0 ; i < nConversions ; i++){
       s = s + res[i];
    }
    float resultado = s/nConversions;

    float temperatura_K = ((resultado*3.3/1024)/0.01);
    return temperatura_K;
}

float calc_altura (int temperatura){
    int i,j;
    float dist, soma=0, altura = 2.10, media2=0;
    int tempo;
    int tempo_max=0;
    int inicio;
    float array_tempo [20];
    float Vsom;
    int medidas=20, ordem=2;


    for (i=0; i<20; i++) {
        WriteCoreTimer(0);
        timerPWM();
        array_tempo[i] = (float) ReadCoreTimer();
        Delay_ms(100);
    }

    /*media2=media (&array_tempo,medidas,ordem);
    printf(" media %4.1f",media2);
     * */
    for (i=0; i<20; i++) {
        media2=media2+array_tempo[i];
    }
    media2=media2/medidas;

    Vsom=20*sqrt(temperatura);
    dist=Vsom*(media2/40000);
    dist=(dist*3)/80;
    return altura=altura-(dist*0.01);
    }


void Delay_ms(unsigned int dms)
{
    unsigned int t;
    t=ReadCoreTimer()+40000*dms;
    while(ReadCoreTimer() < t);
}

void _mon_putc(char c) {
    while (U1STAbits.UTXBF); // Wait till buffer available (TX Buffer Full)
    U1TXREG = c; // Put char in Tx buffer
    return;
}

int _mon_getc(int canblock) {

    // Reset Overrun Eror Flag - if set UART does not receive any chars
    if (U1STAbits.OERR)
        U1STAbits.OERR;

    if (canblock == 0) {
        if (U1STAbits.URXDA)
            return (int) U1RXREG;
    }
    else {
        while (!U1STAbits.URXDA);
        return (int) U1RXREG;
    }
    return -1;
}

/*float media (float *array_dist, int medidas) {
    float min=100000000; ////rever isto
    float max=0;
    float soma=0, media;
    int i,j;
    int minPos, maxPos;

    for(i=0;i<medidas-1;i++) {
        if(array_dist[i]<min){
            min=array_dist[i];
            minPos=i;
        }
        else if(array_dist[i]>max){
            max=array_dist[i];
            maxPos=i;
        }
    }
    array_dist[minPos]=0;
    array_dist[maxPos]=0;
    for(j=0;j<medidas-1;j++) {
       soma=soma+array_dist[j];
    }
    return media=soma/(medidas-2); ////-2 porque assim desconta os zeros
}*/

float media (float* array, int medidas, int ordem) {
    int i;
    float soma, media, aux, tmpsum;
    while(ordem>0) {
        soma=0;
        media=0;
        float afast=0;
        int posAfast=0;

        for(i=0;i<medidas;i++) {
            soma=soma+array[i];
        }

        media=soma/medidas;

        for(i=0;i<medidas;i++) {
            aux=fabsf(media-array[i]);
            if(aux>afast) {
                afast=aux;
                posAfast=i;
            }
        }
        tmpsum = soma - array[posAfast] + media;
        array[posAfast]=media;
        ordem--;
    }

    return tmpsum/medidas;
}