#include "DefaultParametrs.h"
#include <modules/my_task_creator/TaskCreator.h>
DefaultParametrs::DefaultParametrs()
{
    mParamMap[0]=SFPair("CTRL_TWP_NUM",0.0);
    mParamMap[1]=SFPair("CTRL_SER_BAUD",1500.0);
    mParamMap[2]=SFPair("CTRL_ATTACK_DIST",800.0);
    mParamMap[3]=SFPair("CTRL_ATTACK_ALT",300.0);
    mParamMap[4]=SFPair("CTRL_UPDATE_FW",0);
    mParamMap[5]=SFPair("CTRL_REBOOT",0);
    mParamMap[6]=SFPair("CTRL_WP_REDY", 1);
    mParamMap[7]=SFPair("CTRL_K_YAW", 1.0);
    mParamMap[8]=SFPair("CTRL_K_PITCH", 3);
    mParamMap[9]=SFPair("CTRL_TEST_MODE",1.0);
    mParamMap[10]=SFPair("CTRL_TEST_ALT",100.0);
    mParamMap[11]=SFPair("CTRL_USE_KPITCH",1.0);
    mParamMap[12]=SFPair("VIDEO_FOVX",71.0422f);
    mParamMap[13]=SFPair("VIDEO_FOVY",47.9715f);
    mParamMap[14]=SFPair("VIDEO_WIDTH",1024);
    mParamMap[15]=SFPair("VIDEO_HEIGHT",768);
    mParamMap[16]=SFPair("VIDEO_ENABLE",1);
    mParamMap[17]=SFPair("CTRL_ATTACK_SPD",30.0);
    mParamMap[18]=SFPair("CTRL_I_YAW", 0.5);
    mParamMap[19]=SFPair("CTRL_D_YAW", 0.0);
    mParamMap[20]=SFPair("CTRL_I_MAX", 5.0);
    mParamMap[21]=SFPair("CAM_SRV_CH", 0.0);
    mParamMap[22]=SFPair("CAM_SRV_MAX", 2000.0);
    mParamMap[23]=SFPair("CAM_SRV_MIN", 1000.0);
    mParamMap[24]=SFPair("CAM_ANG_CRUISE", 45.0);
    mParamMap[25]=SFPair("CAM_ANG_MAX", 120.0);
    mParamMap[26]=SFPair("CAM_ANG_MIN", 0.0);
    mParamMap[27]=SFPair("CTRL_FLAP_RC",6.0);
    mParamMap[28]=SFPair("CTRL_FLAP_VAL",1000.0);
    mParamMap[29]=SFPair("CTRL_SRV_CH",8.0);
    mParamMap[30]=SFPair("TEST_SRV_CH",0.0);
    mParamMap[31]=SFPair("TEST_SRV_AMP",30.0);
    mParamMap[32]=SFPair("TEST_SRV_T",5.0);
    mParamMap[33]=SFPair("TEST_SRV_TRIM",1500.0);
    mParamMap[34]=SFPair("CTRL_LOITER_ALT",100.0);
    mParamMap[35]=SFPair("CAM_SRV_REVERSE", 0.0);
    mParamMap[36]=SFPair("CTRL_SAFE_SPD",30.0);
    mParamMap[37]=SFPair("CTRL_SAFE_PITCH",20.0);
    mParamMap[38]=SFPair("CTRL_SAFE_TIME",10.0);
    mParamMap[39]=SFPair("CTRL_RANGE_TYPE",0.0);
    mParamMap[40]=SFPair("CAM_SRV_KI",0.1);
    mParamMap[41]=SFPair("CAM_SRV_KP",0.0);
    mParamMap[42]=SFPair("CAM_PITCH_DELTA",5.0);
    mParamMap[43]=SFPair("CAM_RATE_MAX",45.0);
    mParamMap[44]=SFPair("CTRL_TECS_DALT",10.0);

    mParamCount=mParamMap.size();
    for(int i=0;i<mParamCount;i++)
        mParams.push_back(mParamMap[i].second);
}

DefaultParametrs::~DefaultParametrs()
{
}
float DefaultParametrs::GetValue(uint16_t num)
{
    float res=0;
    if(num>mParamCount-1)
        return res;
    mMutex.Lock();
    res=mParamMap[num].second;
    mMutex.Unlock();
    return res;
}
float DefaultParametrs::GetValue(String name)
{
    float res=0;
    mMutex.Lock();
    for(int i=0;i<mParamCount;i++)
    {
        if(mParamMap[i].first==name)
            res=mParamMap[i].second;
    }
    mMutex.Unlock();
    return res;
}
String DefaultParametrs::GetName(uint16_t num)
{
    String res;
    if(num>mParamCount-1)
        return res;
    mMutex.Lock();
    res=mParamMap[num].first;
    mMutex.Unlock();
    return res;
}
void DefaultParametrs::SetParamsVecot(std::vector<float> p)
{
    mParams=p;
    for(int i=0;i<mParamCount;i++)
    {
        SFPair nv;
        nv=mParamMap[i];
        nv.second=p[i];
        mParamMap[i]=nv;
    }
        
}
void DefaultParametrs::SetParam(uint16_t idx,float p)
{
    if(idx>mParamCount-1)
        return;
    mMutex.Lock();
    mParamMap[idx].second=p;
    mParams.at(idx)=p;
    mMutex.Unlock();
}
uint16_t DefaultParametrs::SetParam(String name,float p)
{
    uint16_t idx=0xffff;
    mMutex.Lock();
    for(int i=0;i<mParamCount;i++)
    {
        if(mParamMap[i].first==name)
        {
            mParamMap[i].second=p;
            idx=i;
            mParams.at(idx)=p;
            break;
        }
    }
    mMutex.Unlock();
    return idx;
}