#include <control/control_status.h>
#include <QDebug>


#define GET_HIGH_BYTE(x)  ((x >> 8) & 0xFF)
#define GET_LOW_BYTE(x)   (x & 0xFF)


//#if 1


void iv::control::ControlStatus::set_cmd_checksum(unsigned char cmd_id)
{
//    unsigned int check_sum=0;
//    if(cmd_id==0x10)
//    {
//        for(int i=0;i<7;i++)
//        {
//            check_sum^=(ServiceControlStatus.command10.byte[i]);
//        }
//        ServiceControlStatus.command10.byte[7]=check_sum;
//    }else if(cmd_id==0x11)
//    {
//        for(int i=0;i<7;i++)
//        {
//            check_sum^=(ServiceControlStatus.command11.byte[i]);
//        }
//        ServiceControlStatus.command11.byte[7]=check_sum;
//    }else if(cmd_id==0x12)
//    {
//        for(int i=0;i<7;i++)
//        {
//            check_sum^=(ServiceControlStatus.command12.byte[i]);
//        }
//        ServiceControlStatus.command12.byte[7]=check_sum;
//    }
}




void iv::control::ControlStatus::set_wheel_angle(float angle)
{

    int ang =angle*0.0175*0.037*1000;//角度单位线控是弧度，决策是度，因此要进行转换，决策是方向盘转角，线控是车轮转角也要进行转换
  if(ang>576)
      ang=570;
  if(ang<-576)
      ang=-570;
    command111.bit.ads_ang_h = GET_HIGH_BYTE(ang);
    command111.bit.ads_ang_l = GET_LOW_BYTE(ang);
}

void iv::control::ControlStatus::set_wheel_speed(float speed)
{

    int angSpeed =speed;
    //ServiceControlStatus.command12.bit.ang_speed = angSpeed;
}

void iv::control::ControlStatus::set_wheel_enable(bool enable)
{

    if(enable){
        command2d0.bit.ads_steer_en = 1;
    }else{
        command2d0.bit.ads_steer_en= 0;
    }
}




 void iv::control::ControlStatus::set_speed_limit(float speed)
 {
//     ServiceControlStatus.command11.bit.speed_limit = (unsigned)speed;
 }



void iv::control::ControlStatus::set_aeb(float aeb)
{
//    int aebBrake =(aeb+16)/0.000488;
//    ServiceControlStatus.command11.bit.aeb_H = (aebBrake)  /256;
//    ServiceControlStatus.command11.bit.aeb_L = (aebBrake)  % 256;
}

void iv::control::ControlStatus::set_torque(float torque)
{
//    int spd=torque/3.6*1000;//决策的速度单位是km/h，线控的速度单位mm/s
    int spd=int((torque*1000.0)/3.6);

    if(spd>1500)  //zuoxiansu
        spd=1500;
    if(spd<-1500)
        spd=-1500;
    command111.bit.ads_spd_h=GET_HIGH_BYTE(spd);
    command111.bit.ads_spd_l= GET_LOW_BYTE(spd);
}

void iv::control::ControlStatus::set_brake(float brake)
{
   // command11.bit.brake=(unsigned)brake;
}



void iv::control::ControlStatus::set_gear(int gear)
{
    //command11.bit.gear = (unsigned)gear;
}

void iv::control::ControlStatus::set_handBrake(bool handbrake)
{
    if(handbrake){
        command131.bit.ads_epb = 1;
    }else{
        command131.bit.ads_epb = 0;
    }
}

void iv::control::ControlStatus::set_driveMode(char driveMode)
{
    command421.bit.ads_drive_mode = (unsigned)driveMode;
}

void iv::control::ControlStatus::set_gear_enable(bool enable){
    if (enable == true)
        command1d0.bit.ads_gearen = 1;
    else
        command1d0.bit.ads_gearen = 0;
}

void iv::control::ControlStatus::set_acc_enable(bool enable){
    if (enable == true)
        command3d0.bit.ads_spd_en = 1;
    else
        command3d0.bit.ads_spd_en = 0;
}

void iv::control::ControlStatus::set_brake_enable(bool enable){
    if (enable == true)
         command4d0.bit.ads_brake_en  = 1;
    else
         command4d0.bit.ads_brake_en  = 0;
}

void iv::control::ControlStatus::set_park_enable(bool enable){
    if (enable == true)
         command5d0.bit.ads_park_en  = 1;
    else
         command5d0.bit.ads_park_en  = 0;
}


//void iv::control::ControlStatus::set_win_lf(char para)
//{
//    command10.bit.windowlf = (unsigned)para;
//}

//void iv::control::ControlStatus::set_win_rf(char para)
//{
//    command10.bit.windowrf = (unsigned)para;
//}

//void iv::control::ControlStatus::set_win_lr(char para)
//{
//    command10.bit.windowlr = (unsigned)para;
//}

//void iv::control::ControlStatus::set_win_rr(char para)
//{
//    command10.bit.windowrr = (unsigned)para;
//}

//空调控制
void iv::control::ControlStatus::set_air_on(char para)
{
//    command10.bit.air_on = (unsigned)para;
}



//void iv::control::ControlStatus::set_air_cricle(char para)
//{
//    command10.bit.air_cricle = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_auto(char para)
//{
//    command10.bit.air_auto = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_off(char para)
//{
//    command10.bit.air_off = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_temup(char para)
//{
//    command10.bit.tem_up = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_temdown(char para)
//{
//    command10.bit.tem_down = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_powerup(char para)
//{
//    command10.bit.power_up = (unsigned)para;
//}


//void iv::control::ControlStatus::set_air_powerdown(char para)
//{
//    command10.bit.power_down = (unsigned)para;
//}


void iv::control::ControlStatus::set_obligate(char para)
{
//    command10.bit.ignition = (unsigned)para;
}


void iv::control::ControlStatus::set_door(char enable)
{
//    command10.bit.door = (unsigned)enable;
}


void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right)
{
//    if ( (left == true) && (right == false) )
//    {
//        command10.bit.turn_light = 0x01;
//    }
//    else if ( (left == false) && (right == true) )
//    {
//        command10.bit.turn_light = 0x02;
//    }
//    else if ((left == false) && (right == false))
//    {
//        command10.bit.turn_light = 0x00;
//    }
//    else
//    {
//        command10.bit.turn_light = 0x03;
//    }
}


void iv::control::ControlStatus::set_small_light(char para)
{
//    command10.bit.small_light = (unsigned)para;
}


void iv::control::ControlStatus::set_near_light(char para)
{
//    command10.bit.near_light = (unsigned)para;
}


void iv::control::ControlStatus::set_horn(char para)
{
//    command10.bit.horn = (unsigned)para;
}


void iv::control::ControlStatus::set_far_light(char para)
{
//    command10.bit.far_light = (unsigned)para;
}


void iv::control::ControlStatus::set_frog_light(char para)
{
//    command10.bit.fog_light = (unsigned)para;
}


void iv::control::ControlStatus::set_wiper(char para)
{
//    command10.bit.wiper = (unsigned)para;
}


void iv::control::ControlStatus::set_brake_light(char para)
{
//    command10.bit.brake_light = (unsigned)para;
}


void iv::control::ControlStatus::set_defrog(char para)
{
//    command10.bit.defrog = (unsigned)para;
}


void iv::control::ControlStatus::set_reverse_light(char para)
{
//    command10.bit.revers_light = (unsigned)para;
}


void set_air_temp(char para);
void set_air_mode(char para);
void set_air_enable(bool enable);
void set_wind_level(char para);
void set_roof_light(char para);
void set_home_light(char para);
void set_air_worktime(char para);
void set_air_offtime(char para);

void iv::control::ControlStatus::set_air_temp(char para)
{
//    command10.bit.air_temp = (unsigned)para+40;
}


void iv::control::ControlStatus::set_air_mode(char para)
{
//    command10.bit.air_mode = (unsigned)para;
}

void iv::control::ControlStatus::set_air_enable(bool enable)
{
//    if(enable){
//        command10.bit.air_enable = 0x01;
//    }else{
//        command10.bit.air_enable = 0x00;
//    }
}

void iv::control::ControlStatus::set_wind_level(char para)
{
//    command10.bit.air_wind_level = (unsigned)para;
}

void iv::control::ControlStatus::set_roof_light(char para)
{
//    command10.bit.roof_light = (unsigned)para;
}

void iv::control::ControlStatus::set_home_light(char para)
{
//    command10.bit.home_light = (unsigned)para;
}

void iv::control::ControlStatus::set_air_worktime(char para)
{
//    command10.bit.air_work_time = (unsigned)para;
}


void iv::control::ControlStatus::set_air_offtime(char para)
{
//    command10.bit.air_off_time = (unsigned)para;
}



//void iv::control::ControlStatus::set_speaker(bool enable){
////    if (enable == true)
////        command.bit.speaker = 1;
////    else
////        command.bit.speaker = 0;
//}

//void iv::control::ControlStatus::set_door(char enable){
////    if (enable == true)
////        command.bit.door = 1;
////    else
////        command.bit.door = 0;
//}

//void iv::control::ControlStatus::set_doorEnable(bool enable){
////    if (enable == true){
////        command.bit.doorEnable = 1;
////        command.bit.door=1;
////    }
////    else{
////        command.bit.doorEnable = 0;
////        command.bit.door=0;
////    }
//}

////void iv::control::ControlStatus::set_flicker(bool enable){
////    if (enable == true)
////        command.bit.flicker = 1;
////    else
////        command.bit.flicker = 0;
////}
//void iv::control::ControlStatus::set_light(bool enable){
//    if (enable == true)
//       command.bit.bright = 1;
//    else
//       command.bit.bright = 0;
//}

//void iv::control::ControlStatus::set_leftlight(bool enable){
//    if (enable == true)
//       command.bit.left_turn = 1;
//    else
//       command.bit.left_turn = 0;
//}


//void iv::control::ControlStatus::set_rightlight(bool enable){
//    if (enable == true)
//       command.bit.right_turn = 1;
//    else
//       command.bit.right_turn = 0;
//}






//void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
//    if (left == true)
//    {
//        command.bit.left_turn = 1;
//        command.bit.right_turn = 0;
//    }
//    else if (right == true)
//    {
//        command.bit.left_turn = 0;
//        command.bit.right_turn = 1;
//    }
//    else
//    {
//        command.bit.left_turn = 0;
//        command.bit.right_turn = 0;
//    }
//}








//#else

//void iv::control::ControlStatus::set_accelerate(float percent)
//{
//    command.bit.acce = (unsigned)((percent + 7) * 20);
//}
//void iv::control::ControlStatus::set_wheel_angle(float angle)
//{
//    command.bit.ang_H = angle * 10 >> 8;

//    command.bit.ang_L = angle * 10 % 256;
//}
//void iv::control::ControlStatus::set_engine(char enable)
//{
//    command.bit.engine = enable;
//}
//void iv::control::ControlStatus::set_door(char enable){
//    command.bit.door = enable;
//}
//void iv::control::ControlStatus::set_speaker(bool enable){
//    if (enable == true)
//        command.bit.speaker = 1;
//    else
//        command.bit.speaker = 0;
//}
//void iv::control::ControlStatus::set_flicker(bool enable){
//    if (enable == true)
//        command.bit.flicker = 1;
//    else
//        command.bit.flicker = 0;
//}
//void iv::control::ControlStatus::set_light(bool enable){
//    if (enable == true)
//       command.bit.bright = 1;
//    else
//       command.bit.bright = 0;
//}
//void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
//    if (left == true)
//    {
//        command.bit.left_turn = 1;
//        command.bit.right_turn = 0;
//    }
//    else if (right == true)
//    {
//        command.bit.left_turn = 0;
//        command.bit.right_turn = 1;
//    }
//    else
//    {
//        command.bit.left_turn = 0;
//        command.bit.right_turn = 0;
//    }
//}
//#endif