#include <stdio.h>
#include <string.h>

#include "ImC/imc_kernel.h"
#include "ImC/imc_extension.h"
#include "ImC/imc_api.h"

extern ADC_HandleTypeDef ADC_HANDLER_SBC;

// int loop_pass_estimation_backup[imcMAX_LOOP_IDS] IMC_KERNEL_NVM;
int loop_pass_estimation[imcMAX_LOOP_IDS];
int imc_is_passing_loops_backup IMC_KERNEL_NVM;
int imc_is_passing_loops;
int imc_energy_at_start;

int estimation_backup[2][imcMAX_LOOP_IDS] IMC_KERNEL_NVM;
int imc_energy_estimation_initialized IMC_KERNEL_NVM;
int latest_buffer_index IMC_KERNEL_NVM;
int last_loop_skip_count;
int pass_due_to_low_energy;

#if(imcPRINT_STATS)
    int imc_checkpoint_triggered IMC_KERNEL_NVM;
    int imc_checkpoint_executed IMC_KERNEL_NVM;
#endif

void imc_recover_estimations() {
    memcpy(loop_pass_estimation, estimation_backup[latest_buffer_index], sizeof(int) * imcMAX_LOOP_IDS);
    __DSB();
}

void imc_backup_estimations() {
    int nextBufferIndex = latest_buffer_index == 0 ? 1 : 0;
    memcpy(estimation_backup[nextBufferIndex], loop_pass_estimation, sizeof(int) * imcMAX_LOOP_IDS);
    __DSB();
    latest_buffer_index = nextBufferIndex;
    __DSB();
}

void imc_init_energy_estimation() {
    if(!imc_energy_estimation_initialized) {
        for(int i=0; i<imcMAX_LOOP_IDS; i++) {
            loop_pass_estimation[i] = 1;
        }
        latest_buffer_index = 0;
        imc_backup_estimations();
    } else {
        imc_recover_estimations();
    }

    if(imc_energy_estimation_initialized && imc_is_passing_loops_backup != -1) {
        int loop_id = imc_is_passing_loops_backup;
        int new_estimation = loop_pass_estimation[loop_id] / 2;
        loop_pass_estimation[loop_id] = new_estimation;
        estimation_backup[latest_buffer_index][loop_id] = new_estimation;
        // printf("(recovery) estimation for loop %d is updated to %d\r\n", loop_id, loop_pass_estimation[loop_id]);
    }
    imc_energy_estimation_initialized = 1;
    imc_is_passing_loops_backup = -1;
    imc_is_passing_loops = -1;
    __DSB();
}

void __imc_unflag_loop_skipping()
{
    imc_is_passing_loops_backup = -1;
    imc_is_passing_loops = -1;
    __DSB();
}

void __imc_finish_loop_skipping() {
    int loop_id = imc_is_passing_loops;
    if(loop_id == 0) {
        printf("imc_is_passing_loop is 0\r\n");
        while(1) { __ASM("  nop"); }
    }
    if(loop_id > 0) {
        int volt = measure_voltage(ADC_HANDLER_SBC, EPS_CAP_ID_SBC);
        // printf("imc_finish_loop_skipping() called with loopId = %d\r\n", loop_id);
        int energy = volt*volt - imcCAP_VOL_LOW*imcCAP_VOL_LOW;
        // int ratio = energy * 100 / imcENERGY_TOTAL;

        int energy_consumed = imc_energy_at_start - energy;
        // int consumed_ratio = energy_consumed * 100 / imcENERGY_TOTAL;

        // if(ratio > 5) {
        // printf("last loop skip count: %d\r\n", last_loop_skip_count);
        int last_skip_is_zero = last_loop_skip_count == 0;
        int energy_per_iteration = energy_consumed / last_loop_skip_count;
        int underestimated = energy_per_iteration * 10 < energy;
        // if(energy_consumed / last_loop_skip_count < 2 * energy) {
        if(!last_skip_is_zero && underestimated) {
            int current_estimation = loop_pass_estimation[loop_id];
            // printf("current estimation for loop_%d: %d\r\n", loop_id, current_estimation);
            double inc_factor = 2;
            if (current_estimation * inc_factor > (double)INT32_MAX)
            {
                // printf("remaining energy: %d\r\n", remaining_energy);
                current_estimation = INT32_MAX/2-1;
            }
            else
            {
                current_estimation = (int)(current_estimation * inc_factor);
                // printf("remaining energy: %d\r\n", remaining_energy);
            }
            if (current_estimation < 1) {
                current_estimation = 1;
            }
            loop_pass_estimation[loop_id] = current_estimation;
            // loop_pass_estimation_backup[loop_id] = current_estimation;
            portDISABLE_INTERRUPTS();
            // printf("estimation for loop %d is updated to %d\r\n", loop_id, current_estimation);
            portENABLE_INTERRUPTS();
        }
    }
    imc_is_passing_loops_backup = -1;
    imc_is_passing_loops = -1;
    __DSB();
}

int __imc_get_loop_pass_count(int loop_id) {
    #if(imcENABLE_STATIC_LOOP_PASS_COUNT)
        return imcLOOP_PASS_COUNT;
    #elif(imcENABLE_ADAPTIVE_LOOP_PASS_COUNT)
        int volt = measure_voltage(ADC_HANDLER_SBC, EPS_CAP_ID_SBC);
        // if(volt < imcCHECKPOINT_VOLTAGE) {
        //     imcREQUEST_CHECKPOINT();
        // }
        int energy = volt*volt - imcCAP_VOL_LOW*imcCAP_VOL_LOW;
        int ratio = energy * 100 / imcENERGY_TOTAL;
        // float energy_available = (float)(volt*volt - imcCAP_VOL_LOW*imcCAP_VOL_LOW) / imcENERGY_TOTAL;
        imc_is_passing_loops = loop_id;
        imc_is_passing_loops_backup = loop_id;
        __DSB();
        imc_energy_at_start = energy;
        // printf("write %d to imc_is_passing_loops\r\n", imc_is_passing_loops);
        // printf("energy available: %f\r\n", energy_available);
        // if(energy_available > 1) return (int)(energy_available / loop_energy_estimation[loop_id]);
        // else if(energy_available < 0) return 0;
        // else return (int)(energy_available / loop_energy_estimation[loop_id]);
        int current_estimation = loop_pass_estimation[loop_id];
        if(ratio > 100) {
            last_loop_skip_count = current_estimation;
        }
        else if(ratio < 0) {
            last_loop_skip_count = 0;
            // imc_sbc_power_off();
            // while(1) { __ASM(" nop"); }
            // printf("__imc_get_loop_pass_count(%d) called; %d returned\r\n", loop_id, last_loop_skip_count);
        }
        else {
            double r = (double)(ratio) / 100.0;
            last_loop_skip_count = (int)((double)current_estimation * r);
            // last_loop_skip_count = loop_pass_estimation[loop_id] * ((double)(ratio) / 100.0);
        }
        // printf("loopId: %d, volt: %d, energy: %d, total: %d, ratio: %d, est:%d, count:%d\r\n", loop_id, volt, energy, imcENERGY_TOTAL, ratio, current_estimation, last_loop_skip_count);
        return last_loop_skip_count;
    #else
        return 0;
    #endif
}