#include <_Task_measure.h>

//Measure every 10ms
void clock0_function(){
    Semaphore_post(semaphore_measure);
    //GPIO_toggle(Board_LED0); //JUST4FUN
}


uint8_t delayTaskMeasure;

void task_measure(void){ //-Measure Cell Voltages
    //HIGH PRIO

    Semaphore_pend(semaphore_booted, BIOS_WAIT_FOREVER);
    putTimestamp("_task1_");
    while(1){
            Semaphore_pend(semaphore_measure, BIOS_WAIT_FOREVER);
            measureCounter++;
            if(measureCounter==209){
                int i=0;
            }
            if(delayTaskMeasure==true){
                //TaskDelayMilliseconds(2000*total_ic);
                delayTaskMeasure=false;
            }

            putTimestamp("startmea");
            if(USE_RANDOM_MEASUREMENTS){
                measureRandom();
            }
            else{
                if(doBalancing && didFirstMedian && sample==0){
                    stopBalancing();
                }
                measure();
                putTimestamp("donemea");
                //TaskDelayMilliseconds(10);
            }
            sendMinMaxStatus();
            if(sample==SAMPLE_COUNT/2 && !USE_RANDOM_MEASUREMENTS){
                //ltcChangeConfig();
            }

            if(doBalancing && didFirstMedian && sample==0 && !USE_RANDOM_MEASUREMENTS){
                balance();
            }
            sample++;
            if( sample >= SAMPLE_COUNT ){
                prepareFiltering();
                //send semaphore to Task2
                Semaphore_post(semaphore_median);
                sample = 0;
            }
            //putTimestamp("stopmea");

    }
}

void task_median(void){

    /*
     *
     *          THIS CODE is used for Testing the Eeprom Storage with Can
    //TODO only for debug
    //AMSsendDataES910();

    uint8_t TestData[8];
    int i;
    for(i = 0; i <= 7; i++){
        TestData[i] = 17*i+14 ;
       }
    saveDatafromES910(&TestData);
*/
    //LOW PRIO

    while(1){
        Semaphore_pend(semaphore_median, BIOS_WAIT_FOREVER);
        filterMeasurements();
        sendFilteredMeasurements();
        if(didFirstMedian==false
                && battery_status.max_cell_temperature<maxAllowedTemp
                && battery_status.min_cell_voltage>minAllowedVoltage
                && battery_status.max_cell_voltage<maxAllowedVoltage){

            Semaphore_post(semaphore_didSelftest);
            didFirstMedian=true;
        }

        if(csc[0].median_temperature[0]<100){
            delayTaskMeasure=true;
        }
    }


}