#include "ctrlcmd2decition.h"

#include <iostream>
#include <math.h>

ctrlcmd2decition::ctrlcmd2decition()
{
    mpLT = new LookupTable();

    ModuleFun xFunctrlcmd = std::bind(&ctrlcmd2decition::ListenCtrlCmd,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpactrlcmd = iv::modulecomm::RegisterRecvPlus("ctrlcmd",xFunctrlcmd);
    mpadecision = iv::modulecomm::RegisterSend("deciton",1000,1);

    ModuleFun xFunGPSIMU = std::bind(&ctrlcmd2decition::ListenGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);

    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",xFunGPSIMU);

}

ctrlcmd2decition::~ctrlcmd2decition()
{
    iv::modulecomm::Unregister(mpagpsimu);
    iv::modulecomm::Unregister(mpadecision);
    iv::modulecomm::Unregister(mpactrlcmd);
    delete mpLT;
}

void ctrlcmd2decition::ListenGPSIMU(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    iv::gps::gpsimu xgpsimu;
    if(xgpsimu.ParseFromArray(strdata,nSize))
    {
        mfVelNow = sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
    }
    else
    {
        std::cout<<" ctrlcmd2decition::ListenGPSIMU Parse Fail ." <<std::endl;
    }
}

void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    iv::autoware::autowarectrlcmd xctrlcmd;
    if(!xctrlcmd.ParseFromArray(strdata,nSize))
    {
        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Parse Fail"<<std::endl;
        return;
    }

    iv::brain::decition xdecition;
    double facc = 0;
    double ftorque = 0;
    double fbrake = 0;
    double fwheelangle = 0;
    std::cout<<" cmd acc: "<<xctrlcmd.linear_acceleration()<<std::endl;
    if(xctrlcmd.linear_velocity() > 1.0)
    {
        if(xctrlcmd.linear_velocity()>mfVelNow)
        {
            if(xctrlcmd.linear_velocity()>(mfVelNow + 0.1))
            {
                if(xctrlcmd.linear_velocity()>(mfVelNow + 0.3))
                {
                    facc =0.3+ 1.0* (xctrlcmd.linear_velocity() -mfVelNow)/xctrlcmd.linear_velocity() ;
                }
                else
                    facc =1.6 * (xctrlcmd.linear_velocity() -mfVelNow)/xctrlcmd.linear_velocity() ;
            }
            else
            {
                facc = 0.0;
            }
        }
        else
        {
            if(xctrlcmd.linear_velocity()>(mfVelNow-0.3))
            {
                facc = 0.0;
            }
            else
            {
                facc = -1.5;
            }
        }
    }
    else
    {
        if(xctrlcmd.linear_velocity()>mfVelNow)
        {
            facc = 1.0;
        }
        else
        {
            facc = -1.0;
        }
    }
    facc = xctrlcmd.linear_acceleration();
    if(mpLT->IsINterpolationOK())
    {
        mpLT->GetTorqueBrake(xctrlcmd.linear_velocity()*3.6,facc,ftorque,fbrake);
    }
    else
    {
        double fVehWeight = 1800;
        double fg = 9.8;
        double fRollForce = 50;
        const double fRatio = 2.5;
        double fNeed = fRollForce + fVehWeight*facc;

        ftorque = fNeed/fRatio;
        fbrake = 0;
        if(ftorque<0)
        {
            fbrake = facc;
            ftorque = 0;
        }
    }
//    facc = xctrlcmd.linear_acceleration();
    fwheelangle = xctrlcmd.steering_angle() * (180.0/M_PI) * 15.0;
    if(fwheelangle>430)fwheelangle = 430;
    if(fwheelangle<-430)fwheelangle = -430;
    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
    xdecition.set_accelerator(facc);
    xdecition.set_brake(fbrake);
    xdecition.set_leftlamp(false);
    xdecition.set_rightlamp(false);
    xdecition.set_speed(xctrlcmd.linear_velocity() * 3.6);
    xdecition.set_wheelangle(fwheelangle);
    xdecition.set_wheelspeed(300);
    xdecition.set_torque(ftorque);
    xdecition.set_mode(1);
    xdecition.set_gear(1);
    xdecition.set_handbrake(0);
    xdecition.set_grade(1);
    xdecition.set_engine(0);
    xdecition.set_brakelamp(false);
    xdecition.set_ttc(15);
//    xdecition.set_air_enable(decition->air_enable);
//    xdecition.set_air_temp(decition->air_temp);
//    xdecition.set_air_mode(decition->air_mode);
//    xdecition.set_wind_level(decition->wind_level);
    xdecition.set_roof_light(0);
    xdecition.set_home_light(0);
//    xdecition.set_air_worktime(decition->air_worktime);
//    xdecition.set_air_offtime(decition->air_offtime);
//    xdecition.set_air_on(decition->air_on);
    xdecition.set_door(0);
    xdecition.set_dippedlight(0);

    int nbytesize = (int)xdecition.ByteSize();
    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
    if(!xdecition.SerializeToArray(pstr_ptr.get(),nbytesize))
    {
        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Serialize Fail. "<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(mpadecision,pstr_ptr.get(),nbytesize);
}