#include "decodechassis.h"
#include <iostream>
#include "ivlog.h"
#include "ivfault.h"

iv::Ivlog * mygivlog;
iv::Ivfault * mygivfault;

/**
  * @brief ShareChassis
  * Share Chassis State
  * @param pa                pointer to module communication handle
  * @param pchassis          pointer to chassis state
**/
static void ShareChassis(void * pa,iv::chassis  * pchassis)
{
    char * str;
    int ndatasize = pchassis->ByteSize();
    str = new char[ndatasize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(!pchassis->SerializeToArray(str,ndatasize))
    {
        std::cout<<"ShareChassis Error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);

}

/**
  * @brief ProcGE3Chassis
  * Process GE3 Chassis State
  * @param pa                pointer to module communication handle
  * @param pmsg              pointer to CAN Message
  * @retval                  0 No Chassis Message   1 Have Chassis Message And Update
**/
int ProcGE3Chassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;

//    mygivlog = new iv::Ivlog("chassis");
 //   mygivfault = new iv::Ivfault("chassis");

    for(i=0;i<pmsg->rawmsg_size();i++)
    {
        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        if(praw->id() == 0x13)
        {
            std::cout<<"receive chassis."<<std::endl;
            memcpy(data,praw->data().data(),8);
            unsigned char EPSMode,EPBFault,DriveMode,Shift,AEBAvailable,CDDAvailable;
            unsigned short angle_feedback;

            EPSMode = (data[1] & 0x60)>>5;     //EPS_mode,00：失效；01：人工；10自动
            EPBFault = (data[1] & 0x10)>>4;    //EPB故障状态，0：无故障  1：故障
            DriveMode = (data[1] & 0x0C)>>2;   //驾驶模式，1：Manual  3:Auto
            Shift = data[1] & 0x03;   //Gear,00N,01D，10R,11p

            angle_feedback = data[2] * 256 + data[3];
            angle_feedback = angle_feedback * 0.1 - 780;


            //制动的两个状态为反馈，AEBAvailable 和CDDAvailable，这两个都为1的时候制动有效，有一个为0，制动失效
            AEBAvailable = data[4] & 0x01; //0=Not available,1=Available
            CDDAvailable = (data[4] & 0x02) >> 1;  //0=Not available,1=Available

            iv::chassis xchassis;

            //xchassis.set_epsmode(EPSMode);
            if(EPSMode == 0)
            {
                std::cout<<"eps mode"<<std::endl;
 //               mygivlog->warn("chassis"," Warning!!  NOW GE3 chassis triggers a epsmode！！");
            }
            if(EPSMode == 1)
            {
//                mygivlog->info("chassis","OK!!  NOW GE3 chassis epsmode is Manual！");
            }
            if(EPSMode == 2)
            {
  //              mygivlog->info("chassis","OK!! NOW GE3 chassis epsmode is Auto！");
            }

//            xchassis.set_epsmode(2);//为了不退出自动驾驶，经讨论，在这个地方写定值。

            xchassis.set_epsmode(EPSMode);
            xchassis.set_epbfault(EPBFault);
            xchassis.set_drivemode(DriveMode);
            xchassis.set_shift(Shift);

            xchassis.set_aebavailable(AEBAvailable);
            xchassis.set_cddavailable(CDDAvailable);
            xchassis.set_angle_feedback(angle_feedback);

            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;


}


//struct Command_Response_Bit_0x14
//{
//    unsigned char feedback_angle_H;
//    unsigned char feedback_angle_L;
//    unsigned char feedback_angle_speed;
//    unsigned char feedback_torque;
//    unsigned char feedback_speed_H;
//    unsigned char feedback_speed_L;
//    unsigned char feedback_battery_power;
//    unsigned char feedback_door_state:2;
//    unsigned char feedback_EPB_state:1;
//    unsigned char feedback_reserved:3;
//    unsigned char feedback_dangwei:2;
//};


//ServiceCarStatus.torque_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_torque;
//ServiceCarStatus.dangwei_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_dangwei;
//ServiceCarStatus.battery_st = ServiceControlStatus.command_reponse_0x14.bit.feedback_battery_power*0.4;

//ServiceCarStatus.vehSpeed_st= (ServiceControlStatus.command_reponse_0x14.bit.feedback_speed_H*256+
//                              ServiceControlStatus.command_reponse_0x14.bit.feedback_speed_L)*0.05625;

int ProcPROBLUEChassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;
    for(i=0;i<pmsg->rawmsg_size();i++)
    {

        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        if(praw->id() == 0x14)
        {
            memcpy(data,praw->data().data(),8);
            double torque,dangwei,soc,vehspeed;
            torque = data[3];
            dangwei = data[7]>>6;
            soc = data[6]; soc = soc *0.4;
            vehspeed = (data[4]*256.0 + data[5])*0.05625;

            iv::chassis xchassis;
            xchassis.set_torque(torque);
            xchassis.set_shift(dangwei);
            xchassis.set_soc(soc);
            xchassis.set_vel(vehspeed);

            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;
}

int ProcMIDCARChassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;
    for(i=0;i<pmsg->rawmsg_size();i++)
    {

        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        if(praw->id() == 0x14)
        {
            memcpy(data,praw->data().data(),8);
            double torque,dangwei,soc,vehspeed;
            torque = data[3];
            dangwei = data[7]>>6;
            soc = data[6]; soc = soc *0.4;
            vehspeed = (data[4]*256.0 + data[5])*0.05625;

            iv::chassis xchassis;
            xchassis.set_torque(torque);
            xchassis.set_shift(dangwei);
            xchassis.set_soc(soc);
            xchassis.set_vel(vehspeed);

            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
        else if(praw->id() == 0xC2)
        {
            memcpy(data,praw->data().data(),8);
            float soc;
            soc = data[0];

            iv::chassis xchassis;
            xchassis.set_soc(soc);

            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;

}

int ProcVV7Chassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;
    for(i=0;i<pmsg->rawmsg_size();i++)
    {

        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        if(praw->id() == 0x13)
        {
            memcpy(data,praw->data().data(),8);
            double engine_speed;

            engine_speed = (data[5]*256.0 + data[6])*0.125;

            iv::chassis xchassis;
            xchassis.set_engine_speed(engine_speed);


            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;

}


inline float gettwomotovalue(unsigned char * strdata,float fratio,float foff)
{
    int a,b;
    a = strdata[0];
    b = strdata[1];
    float fvalue = a*256+ b;
    fvalue = fvalue * fratio + foff;
    return fvalue;
}

inline float getonevalue(unsigned char * strdata,float fratio,float foff)
{
    int a;
    a = strdata[0];
    float fvalue = a;
    fvalue = fvalue * fratio + foff;
 //   std::cout<<"hapo torque: "<<fvalue<<std::endl;
    return fvalue;
}


static bool  ghave0x13= false;
static bool  ghave0x14= false;
static bool  ghave0x15= false;
int ProcMIDBUSChassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;
    static iv::chassis xchassis;
    for(i=0;i<pmsg->rawmsg_size();i++)
    {

        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        memcpy(data,praw->data().data(),8);
        if(praw->id() == 0x13)
        {           
            xchassis.set_angle_feedback(gettwomotovalue(data+0,0.1,-870));
            xchassis.set_torque(getonevalue(data+2,1.0,0));
            xchassis.set_brake_feedback(getonevalue(data+3,0.4,0));
            xchassis.set_vel(gettwomotovalue(data+4,0.05625,0));
            xchassis.set_epb_feedback(data[6]&0x03);
            xchassis.set_shift((data[6]&0xf0)>>4);
            xchassis.set_drivemode(data[7]&0x01);
            xchassis.set_emergencystop_feedback((data[7]&06)>>1);
            xchassis.set_brakelight_feedback((data[7]&0x08)>>3);



            ghave0x13 = true;

        }
        if(praw->id() == 0x14)
        {
            xchassis.set_range_feedback(data[0]);
            xchassis.set_soc(getonevalue(data+1,0.4,0));
            xchassis.set_drivectrltype_feedback(data[5]&0x0f);
            xchassis.set_brakectrltype_feedback((data[5]&0xf0)>>4);
            xchassis.set_epsctrltype_feedback(data[6]&0x0f);
            ghave0x14 = true;
        }
        if(praw->id() == 0x15)
        {

            xchassis.set_frontleftwheel_feedback(gettwomotovalue(data,0.05625,0));
            xchassis.set_frontrightwheel_feedback(gettwomotovalue(data+2,0.05625,0));
            xchassis.set_rearleftwheel_feedback(gettwomotovalue(data+4,0.05625,0));
            xchassis.set_rearrightwheel_feedback(gettwomotovalue(data+6,0.05625,0));
            ghave0x15 = true;
        }

        if(ghave0x13&&ghave0x14&&ghave0x15)
        {
            ghave0x13 = false;
            ghave0x14 = false;
            ghave0x15 = false;
            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;

}

int ProcHAPOChassis(void * pa, iv::can::canmsg * pmsg)
{
    int i;
    int nRtn = 0;
    static iv::chassis xchassis;
    for(i=0;i<pmsg->rawmsg_size();i++)
    {

        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
        unsigned char data[8];
        memcpy(data,praw->data().data(),8);
        if(praw->id() == 0x13)
        {
            xchassis.set_angle_feedback(gettwomotovalue(data+0,0.1,-870));
            xchassis.set_torque(getonevalue(data+2,0.4,0));
            xchassis.set_brake_feedback(getonevalue(data+3,0.4,0));
            xchassis.set_vel(gettwomotovalue(data+4,0.05625,0));
            xchassis.set_epb_feedback(data[6]&0x03);
            xchassis.set_shift((data[6]&0xf0)>>4);
            xchassis.set_drivemode(data[7]&0x01);
            xchassis.set_emergencystop_feedback((data[7]&06)>>1);
            xchassis.set_brakelight_feedback((data[7]&0x08)>>3);

            std::cout<<"torque is "<<xchassis.torque()<<std::endl;
            ghave0x13 = true;

        }
        if(praw->id() == 0x14)
        {
            xchassis.set_range_feedback(data[0]);
            xchassis.set_soc(getonevalue(data+1,0.4,0));
            xchassis.set_drivectrltype_feedback(data[5]&0x0f);
            xchassis.set_brakectrltype_feedback((data[5]&0xf0)>>4);
            xchassis.set_epsctrltype_feedback(data[6]&0x0f);
            ghave0x14 = true;
        }
        if(praw->id() == 0x15)
        {

            xchassis.set_frontleftwheel_feedback(gettwomotovalue(data,0.05625,0));
            xchassis.set_frontrightwheel_feedback(gettwomotovalue(data+2,0.05625,0));
            xchassis.set_rearleftwheel_feedback(gettwomotovalue(data+4,0.05625,0));
            xchassis.set_rearrightwheel_feedback(gettwomotovalue(data+6,0.05625,0));
            ghave0x15 = true;
        }

        if(ghave0x13&&ghave0x14&&ghave0x15)
        {
            ghave0x13 = false;
            ghave0x14 = false;
            ghave0x15 = false;
            ShareChassis(pa,&xchassis);
            nRtn = 1;
        }
    }

    return nRtn;

}


int ProcYUHESENChassis(void * pa, iv::can::canmsg * pmsg)
{
    (void)pa;
    (void)pmsg;
//    int i;
    int nRtn = 0;


    return nRtn;

}