#include "simmodel_shenlan.h"

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

simmodel_shenlan::simmodel_shenlan()
{
    mbInitOK = true;
}

simmodel_shenlan::~simmodel_shenlan()
{

}

void simmodel_shenlan::CalcModel()
{
    int64_t ncmdtime = mlastcmdtime;
    const double maxnocmdtime = 1000;
    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
    if((abs(nnow - ncmdtime)/1000000)>maxnocmdtime)
    {
//        std::cout<<" more than 1 seconds no cmd. not calc."<<std::endl;
        mvel = 0;
        mwheelsteer = 0;
        return;
    }

    static int64_t nlastcalctime = 0;

    double fdifftime = 0;
    if((abs(nnow - nlastcalctime)/1000000)>100)
    {
        fdifftime = 0;
    }
    else
    {
        fdifftime = (nnow - nlastcalctime)/1000000.0;
        fdifftime = fdifftime/1000.0; //seconds
        if(fdifftime <0)fdifftime=0;
    }

    if(fabs(fdifftime) > 0.000000001)
    {



        if(mshift == SHIFT_GEER::DRIVE)
        {
        mx = mx + mvel*fdifftime * cos(mhdg);
        my = my + mvel*fdifftime * sin(mhdg);
        mhdg = mhdg + mvel *fdifftime* tan(mwheelsteer)/mfwheelbase;
        }
        if(mshift == SHIFT_GEER::REVERSE)
        {
        mx = mx - mvel*fdifftime * cos(mhdg);
        my = my - mvel*fdifftime * sin(mhdg);
        mhdg = mhdg - mvel * fdifftime * tan(mwheelsteer)/mfwheelbase;
        }

        if(mshift == SHIFT_GEER::PARK)
        {

        }

        double facc;

        double fVehWeight = 1800;
//        double fg = 9.8;
        double fRollForce = 50;
        const double fRatio = 2.5;

        if(mfcmd_brake<0)
        {
        facc = mfcmd_brake;
        if((mvel<=0)) // &&(mshift == SHIFT_GEER::DRIVE))
        {
            mvel = 0;
            facc = 0;
        }
        }
        else
        {
        facc = (mfcmd_torque * fRatio - fRollForce)/fVehWeight;
        if((facc<0)&&(mvel == 0))
        {
            facc = 0;
        }
        }

        if(mshift == SHIFT_GEER::PARK)
        {
            facc = 0;
        }

        macc = facc;

        if(mshift == SHIFT_GEER::PARK)
        {
            mvel = 0;
            macc = 0;
        }
        mvel = mvel + facc * fdifftime;

//        std::cout<<" acc: "<<macc<<std::endl;

        double fwheelsteer = mwheelsteer;

        fwheelsteer = mfwheelratio * fwheelsteer *180.0/M_PI;

        if(fwheelsteer < mfcmd_wheelangle)
        {
            fwheelsteer = fwheelsteer + mfWHEELSPEED * fdifftime;
            if(fwheelsteer > mfcmd_wheelangle)fwheelsteer = mfcmd_wheelangle;
        }
        if(fwheelsteer > mfcmd_wheelangle){
            fwheelsteer = fwheelsteer - mfWHEELSPEED * fdifftime;
            if(fwheelsteer < mfcmd_wheelangle)fwheelsteer = mfcmd_wheelangle;
        }

        fwheelsteer = fwheelsteer *(M_PI/180.0) /mfwheelratio;
        mwheelsteer = fwheelsteer;




    }


    nlastcalctime = nnow;
}