//
// Created by ilya on 06.05.24.
//

#include "Amplifier.h"
//#include "Amplifier_MAX9768.h"
#include "Amplifier_AW87579.h"

#include "SystemDelayInterface.h"
#include "DeviceStorage.h"
#include <stddef.h>

uint8_t ProtMethods_AmplGetLvlVoltIndex(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->voltages); i++){
        if(env->voltages[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetLvlAmperIndex(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=1; i < sizeof(env->ampers); i++){
        if(env->ampers[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetLvlVolumeIndex(tAmplifier *env, uint8_t indexValule) {
    for(uint16_t i=0; i < sizeof(env->levels); i++){
        if(env->levels[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetADP_BoostStep(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->boost_steps); i++){
        if(env->boost_steps[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetADP_En_Boost(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->confStr->data); i++){
        if(env->confStr->data[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetADP_BoostVTH1(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->boost_vths1); i++){
        if(env->boost_vths1[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetADP_BoostVTH2(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->boost_vths2); i++){
        if(env->boost_vths2[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetADP_BoostMode(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->adp_boostModes); i++){
        if(env->adp_boostModes[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC1_AttackTime(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc1_att_times); i++){
        if(env->agc1_att_times[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC3_RelTime(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc3_relTimes); i++){
        if(env->agc3_relTimes[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC3_AttTime(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc3_attTimes); i++){
        if(env->agc3_attTimes[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC3_OutPower(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc3_out_powers); i++){
        if(env->agc3_out_powers[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetEN_Boost(tAmplifier *env, uint8_t indexValule) {
    if(env->en_boost == indexValule){
        return 1;
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC2_AttTime(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc2_attTimes); i++){
        if(env->agc2_attTimes[i] == indexValule){
            return i;
        }
    }
    return 0;
}

uint8_t ProtMethods_AmplGetAGC2_OutPower(tAmplifier *env, uint8_t indexValule) {
    for(uint8_t i=0; i < sizeof(env->agc2Opr_out_powers); i++){
        if(env->agc2Opr_out_powers[i] == indexValule){
            return i;
        }
    }
    return 0;
}

void ProtMethodsAdd_AmplifierSetCard(tAmplifier *env) {
    if(env != NULL) {
        env->levels[0] = PA_GAIN_0dB;
        env->levels[1] = PA_GAIN_1_5dB;
        env->levels[2] = PA_GAIN_3dB;
        env->levels[3] = PA_GAIN_4_5dB;
        env->levels[4] = PA_GAIN_6dB;
        env->levels[5] = PA_GAIN_7_5dB;
        env->levels[6] = PA_GAIN_9dB;
        env->levels[7] = PA_GAIN_10_5dB;
        env->levels[8] = PA_GAIN_12dB;
        env->levels[9] = PA_GAIN_13_5dB;
        env->levels[10] = PA_GAIN_15dB;
        env->levels[11] = PA_GAIN_16_5dB;
        env->levels[12] = PA_GAIN_18dB;
        env->levels[13] = PA_GAIN_19_5dB;
        env->levels[14] = PA_GAIN_21dB;
        env->levels[15] = PA_GAIN_22_5dB;
        env->levels[16] = PA_GAIN_24dB;
        env->levels[17] = PA_GAIN_25_5dB;
        env->levels[18] = PA_GAIN_27dB;

        env->ampers[0] = BST_IPEAK_2A;
        env->ampers[1] = BST_IPEAK_2A;
        env->ampers[2] = BST_IPEAK_2_25A;
        env->ampers[3] = BST_IPEAK_2_5A;
        env->ampers[4] = BST_IPEAK_2_75A;
        env->ampers[5] = BST_IPEAK_3A;
        env->ampers[5] = BST_IPEAK_3_25A;
        env->ampers[6] = BST_IPEAK_3_5A;
        env->ampers[7] = BST_IPEAK_3_75A;
        env->ampers[8] = BST_IPEAK_4A;
        env->ampers[9] = BST_IPEAK_4_25A;
        env->ampers[10] = BST_IPEAK_4_5A;

        env->voltages[0] = BST_VOUT_6_5V;
        env->voltages[1] = BST_VOUT_6_75V;
        env->voltages[2] = BST_VOUT_7V;
        env->voltages[3] = BST_VOUT_7_25V;
        env->voltages[4] = BST_VOUT_7_5V;
        env->voltages[5] = BST_VOUT_7_75V;
        env->voltages[6] = BST_VOUT_8V;
        env->voltages[7] = BST_VOUT_8_25V;
        env->voltages[8] = BST_VOUT_8_5V;
        env->voltages[9] = BST_VOUT_8_75V;
        env->voltages[10] = BST_VOUT_9V;
        env->voltages[11] = BST_VOUT_9_25V;
        env->voltages[12] = BST_VOUT_9_5V;
        env->voltages[13] = BST_VOUT_9_75V;
        env->voltages[14] = BST_VOUT_10V;
        env->voltages[15] = BST_VOUT_10_25V;
        env->voltages[16] = BST_VOUT_10_5V;
        env->voltages[17] = BST_VOUT_10_75V;
        env->voltages[18] = BST_VOUT_11V;
        env->voltages[19] = BST_VOUT_11_25V;
        env->voltages[20] = BST_VOUT_11_5V;
        env->voltages[21] = BST_VOUT_11_75V;
        env->voltages[22] = BST_VOUT_12V;

        env->boost_vths1[0] = SET_BOOST_VTH1_0_1W;
        env->boost_vths1[1] = SET_BOOST_VTH1_0_2W;
        env->boost_vths1[2] = SET_BOOST_VTH1_0_3W;
        env->boost_vths1[3] = SET_BOOST_VTH1_0_4W;

        env->boost_vths2[0] = SET_BOOST_VTH2_1_2W;
        env->boost_vths2[1] = SET_BOOST_VTH2_1_4W;
        env->boost_vths2[2] = SET_BOOST_VTH2_1_6W;
        env->boost_vths2[3] = SET_BOOST_VTH2_1_8W;
        env->boost_vths2[4] = SET_BOOST_VTH2_2_0W;
        env->boost_vths2[5] = SET_BOOST_VTH2_2_2W;

        env->boost_steps[0] = ADP_LOW_STEP_1st_BST_OUT_6_5V;
        env->boost_steps[1] = ADP_LOW_STEP_1st_BST_OUT_6_75V;
        env->boost_steps[2] = ADP_LOW_STEP_1st_BST_OUT_7V;
        env->boost_steps[3] = ADP_LOW_STEP_1st_BST_OUT_7_25V;

        env->adp_boostModes[0] = ADPBOOST_MODE_PassThrough;
        env->adp_boostModes[1] = ADPBOOST_MODE_ForceBOOST;
        env->adp_boostModes[2] = ADPBOOST_MODE_MD1;
        env->adp_boostModes[3] = ADPBOOST_MODE_MD2;

        env->agc1_att_times[0] = AGC1_ATT_TIME_0_04ms_dB;
        env->agc1_att_times[1] = AGC1_ATT_TIME_0_08ms_dB;
        env->agc1_att_times[2] = AGC1_ATT_TIME_0_16ms_dB;
        env->agc1_att_times[3] = AGC1_ATT_TIME_0_32ms_dB;


        env->agc3_out_powers[0] = AGC3OP_0_5W_8ohm_OR_0_67W_6ohm;
        env->agc3_out_powers[1] = AGC3OP_0_6W_8ohm_OR_0_8W_6ohm;
        env->agc3_out_powers[2] = AGC3OP_0_7W_8ohm_OR_0_93W_6ohm;
        env->agc3_out_powers[3] = AGC3OP_0_8W_8ohm_OR_1_07W_6ohm;
        env->agc3_out_powers[4] = AGC3OP_0_9W_8ohm_OR_1_2W_6ohm;
        env->agc3_out_powers[5] = AGC3OP_1W_8ohm_OR_1_33W_6ohm;
        env->agc3_out_powers[6] = AGC3OP_1_1W_8ohm_OR_1_47W_6ohm;
        env->agc3_out_powers[7] = AGC3OP_1_2W_8ohm_OR_1_6W_6ohm;
        env->agc3_out_powers[8] = AGC3OP_1_3W_8ohm_OR_1_73W_6ohm;
        env->agc3_out_powers[9] = AGC3OP_1_4W_8ohm_OR_1_87W_6ohm;
        env->agc3_out_powers[10] = AGC3OP_1_5W_8ohm_OR_2W_6ohm;
        env->agc3_out_powers[11] = AGC3OP_1_6W_8ohm_OR_2_13W_6ohm;
        env->agc3_out_powers[12] = AGC3OP_1_7W_8ohm_OR_2_27W_6ohm;
        env->agc3_out_powers[13] = AGC3OP_1_8W_8ohm_OR_2_4W_6ohm;
        env->agc3_out_powers[14] = AGC3OP_1_9W_8ohm_OR_2_53W_6ohm;
        env->agc3_out_powers[15] = AGC3OP_2W_8ohm_OR_2_67W_6ohm;

        env->agc3_relTimes[0] = AGC3_REL_TIME_5_12ms_dB;
        env->agc3_relTimes[1] = AGC3_REL_TIME_10_24ms_dB;
        env->agc3_relTimes[2] = AGC3_REL_TIME_20_48ms_dB;
        env->agc3_relTimes[3] = AGC3_REL_TIME_40_56ms_dB;
        env->agc3_relTimes[4] = AGC3_REL_TIME_81_92ms_dB;
        env->agc3_relTimes[5] = AGC3_REL_TIME_163_84ms_dB;
        env->agc3_relTimes[6] = AGC3_REL_TIME_327_68ms_dB;
        env->agc3_relTimes[7] = AGC3_REL_TIME_655_36ms_dB;

        env->agc3_attTimes[0] = AGC3_ATT_TIME_1_28ms_dB;
        env->agc3_attTimes[1] = AGC3_ATT_TIME_2_56ms_dB;
        env->agc3_attTimes[2] = AGC3_ATT_TIME_10_24ms_dB;
        env->agc3_attTimes[3] = AGC3_ATT_TIME_40_96ms_dB;
        env->agc3_attTimes[4] = AGC3_ATT_TIME_82ms_dB;
        env->agc3_attTimes[5] = AGC3_ATT_TIME_164ms_dB;
        env->agc3_attTimes[6] = AGC3_ATT_TIME_328ms_dB;
        env->agc3_attTimes[7] = AGC3_ATT_TIME_656ms_dB;

        env->agc2Opr_out_powers[0] = AGC2OP_1W_8ohm_OR_1_33W_6ohm;
        env->agc2Opr_out_powers[1] = AGC2OP_1_2W_8ohm_OR_1_6W_6ohm;
        env->agc2Opr_out_powers[2] = AGC2OP_1_4W_8ohm_OR_1_87W_6ohm;
        env->agc2Opr_out_powers[3] = AGC2OP_1_6W_8ohm_OR_2_13W_6ohm;
        env->agc2Opr_out_powers[4] = AGC2OP_1_8W_8ohm_OR_2_4W_6ohm;
        env->agc2Opr_out_powers[5] = AGC2OP_2W_8ohm_OR_2_67W_6ohm;
        env->agc2Opr_out_powers[6] = AGC2OP_2_2W_8ohm_OR_2_93W_6ohm;
        env->agc2Opr_out_powers[7] = AGC2OP_2_4W_8ohm_OR_3_2W_6ohm;
        env->agc2Opr_out_powers[8] = AGC2OP_2_6W_8ohm_OR_3_47W_6ohm;
        env->agc2Opr_out_powers[9] = AGC2OP_2_8W_8ohm_OR_3_73W_6ohm;
        env->agc2Opr_out_powers[10] = AGC2OP_3W_8ohm_OR_4W_6ohm;
        env->agc2Opr_out_powers[11] = AGC2OP_OFF;

        env->agc2_attTimes[0] = AGC2_ATT_TIME_0_16ms_dB;
        env->agc2_attTimes[1] = AGC2_ATT_TIME_0_32ms_dB;
        env->agc2_attTimes[2] = AGC2_ATT_TIME_0_64ms_dB;
        env->agc2_attTimes[3] = AGC2_ATT_TIME_2_56ms_dB;
        env->agc2_attTimes[4] = AGC2_ATT_TIME_10_24ms_dB;
        env->agc2_attTimes[5] = AGC2_ATT_TIME_40_96ms_dB;
        env->agc2_attTimes[6] = AGC2_ATT_TIME_82ms_dB;
        env->agc2_attTimes[7] = AGC2_ATT_TIME_164ms_dB;
    }
}

tAW87579_RegsList Amplifier_ConvertStrToRegs(tString32 *conf) {
    tAW87579_RegsList list;
    list.sysctr.data = conf->data[def_sysctr];
    list.batsafe.data = conf->data[def_batsafe];
    list.bstvout.data = conf->data[def_bstvout];
    list.bstcpr2.data = conf->data[def_bstcpr2];
    list.pagr.data = conf->data[def_pagr];
    list.pagc3Opr.data = conf->data[def_pagc3Opr];
    list.pagc3Pr.data = conf->data[def_pagc3Pr];
    list.pagc2Opr.data = conf->data[def_pagc2Opr];
    list.pagc2Pr.data = conf->data[def_pagc2Pr];
    list.pagc1Pr.data = conf->data[def_pagc1Pr];
    list.adpMode.data = conf->data[def_adpMode];
    list.adpbstTime1.data = conf->data[def_adpbstTime1];
    list.adpbstVth.data = conf->data[def_adpbstVth];
    return list;
}

void Amplifier_ConfRegsToStr(tAW87579_RegsList *conf, tString32 *res) {
//    tString32 res;
    res->data[def_sysctr] = conf->sysctr.data;
    res->data[def_batsafe] = conf->batsafe.data;
    res->data[def_bstvout] = conf->bstvout.data;
    res->data[def_bstcpr2] = conf->bstcpr2.data;
    res->data[def_pagr] = conf->pagr.data;
    res->data[def_pagc3Opr] = conf->pagc3Opr.data;
    res->data[def_pagc3Pr] = conf->pagc3Pr.data;
    res->data[def_pagc2Opr] = conf->pagc2Opr.data;
    res->data[def_pagc2Pr] = conf->pagc2Pr.data;
    res->data[def_pagc1Pr] = conf->pagc1Pr.data;
    res->data[def_adpMode] = conf->adpMode.data;
    res->data[def_adpbstTime1] = conf->adpbstTime1.data;
    res->data[def_adpbstVth] = conf->adpbstVth.data;
    res->length = 12;
    res->data[4]= -1;
//    return res;
}


void Amplifier_BoostAdpMode_SetSTEP(tAmplifier *env, uint8_t index){
    tAW87579_RegsList *list;
    list = &env->confRegs;
    list->adpbstVth.ADP_LOW_STEP = index;
}

void Amplifier_BoostAdpMode_SetVTH1(tAmplifier *env, uint8_t index){
    tAW87579_RegsList *list;
    list = &env->confRegs;
    list->adpbstVth.SET_BOOST_VTH1 = env->boost_vths1[index];
}

void Amplifier_BoostAdpMode_SetVTH2(tAmplifier *env, uint8_t index){
    tAW87579_RegsList *list;
    list = &env->confRegs;
    list->adpbstVth.SET_BOOST_VTH2 = env->boost_vths2[index];
}

tAW87579_RegsList Amplifier_GetDefaultConf(){
    tAW87579_RegsList list = {
            .sysctr.EN_SW = true,
            .sysctr.EN_PA = true,
            .sysctr.EN_BOOST = true,
            .sysctr.RCV_MODE = false,
            .sysctr.EN_HVBAT = true,

            .batsafe.EN_BAT_SFGD = true,
            .batsafe.BAT_SFGD_LEVEL = BAT_SFGD_LEVEL_6_5V,
            .batsafe.BAT_SFGD_VTH = BAT_SFGD_VTH_3_6V,
            .batsafe.BAT_SFGD_DEGLITCH = BAT_SFGD_DEGLITCH_1ms,

            .bstvout.BST_VOUT = BST_VOUT_6_5V,

            .bstcpr2.BST_IPEAK = BST_IPEAK_4_5A,

            .pagr.PA_GAIN = PA_GAIN_SET__dB(false,27),

            .pagc3Opr.PD_AGC3 = false,
            .pagc3Opr.AGC3_OUTPUT_POWER = AGC3OP_2W_8ohm_OR_2_67W_6ohm,

            .pagc3Pr.AGC3_REL_TIME = AGC3_REL_TIME_40_56ms_dB,
            .pagc3Pr.AGC3_ATT_TIME = AGC3_ATT_TIME_82ms_dB,

            .pagc2Opr.AGC2_OUTPUT_POWER = AGC2OP_OFF,

            .pagc2Pr.AGC2_ATT_TIME = AGC2_ATT_TIME_0_32ms_dB,

            .pagc1Pr.PD_AGC1 = false,
            .pagc1Pr.AGC1_ATT_TIME = AGC1_ATT_TIME_0_04ms_dB,

            .adpMode.AGC1_ATT_TIMEA = false,
            .adpMode.ADPBOOST_MODE = ADPBOOST_MODE_PassThrough,

            .adpbstTime1.ADP_BST_TIME1 = ADP_BST_TIME_160ms,
            .adpbstTime1.ADP_BST_TIME2 = ADP_BST_TIME_140ms,

            .adpbstVth.ADP_LOW_STEP = ADP_LOW_STEP_1st_BST_OUT_7_25V,
            .adpbstVth.SET_BOOST_VTH1 = SET_BOOST_VTH1_0_1W,
            .adpbstVth.SET_BOOST_VTH2 = SET_BOOST_VTH2_2_2W
    };
    return list;
}

void Amplifier_DumpConf(tAmplifier *env){
    Amplifier_ConfRegsToStr(&env->confRegs, env->confStr);
    EraGlonassUveosDumper_ForceDump(env->dumper);
}

void Amplifier_ChangeConf(tAmplifier *env){
//    env->confRegs = Amplifier_ConvertStrToRegs(env->confStr);
//    *env->confStr = Amplifier_ConfRegsToStr(&env->confRegs);
    xAmplifierAW87579_SetAll(env->amplIoBus,env->confRegs);
}

void Amplifier_PowerOn(tAmplifier *env){
    Pwm_SetMuteInCar(env->power, true);
    Pwm_AmpOn(env->power);
    SystemDelayMs(500);
    Amplifier_StartUp(env);
    SystemDelayMs(500);
}

void Amplifier_PowerOff(tAmplifier *env){
    Pwm_SetMuteInCar(env->power, false);
    SystemDelayMs(500);
    Pwm_AmpOff(env->power);
}

void Amplifier_StartUp(tAmplifier *env){
    if(env->confStr->length < 1 ){
        env->confRegs = Amplifier_GetDefaultConf();
        if(xAmplifierAW87579_SetAll(env->amplIoBus,env->confRegs)){
            env->isTest = 2;
        } else {
            env->isTest = 1;
        }
    } else {
        env->confRegs = Amplifier_ConvertStrToRegs(env->confStr);
        if(xAmplifierAW87579_SetAll(env->amplIoBus,env->confRegs)){
            env->isTest = 2;
        } else {
            env->isTest = 1;
        }
    }
}

void Amplifier_Init(tAmplifier *env, tI2cIO *ampl_IO, tEraGlonassUveosDumperInterface *dumper, tString32 *conf, tPowerManagement *power, bool *speakTestAcsess){
    env->isTest = 0;
    env->speakTestAcsess = speakTestAcsess;
    env->power = power;
    env->confStr = conf;
    env->dumper = dumper;
    env->amplIoBus = ampl_IO;
    ProtMethodsAdd_AmplifierSetCard(env);
}