#include "cworktableoffsetdriver.h"
#include "logic_local_def.h"
CWorktableOffsetDriver::CWorktableOffsetDriver(const QString &name, int32_t id)
    :CDriverCard(name,id)
{
    m_iListCurIndex=0;
    m_iListIndex=0;
}

//计算位置偏移
//dPosCur 当前位置
//dPosRef 理论位置
double CWorktableOffsetDriver::CalcuOffsetCur(double dPosCur, double dPosRef)
{
    return dPosRef-dPosCur;
}

//设置某个位置的偏移数据
void CWorktableOffsetDriver::SetOffsetCur(int iOffsetID, double dPos, double dOffset)
{
    CWorktableOffset* pOffsetCh=(CWorktableOffset*)FindChannel(iOffsetID);
    Q_ASSERT(pOffsetCh);
    int iEnableOffset= pOffsetCh->m_Variants.get_VData(Var_WorkTable_Offset_EnableOffset)
            ->toBool();
    if(!iEnableOffset)
    {
        MQT_THROW_STR(QObject::trUtf8("Offset not Enable."));
    }
    //double dOffsetOrign=GetOffset(  iOffsetID,   dPos);
	double dOffsetOrign = 0;
    double dOffsetCur=dOffsetOrign+dOffset;
    m_iListCurIndex= AddOffsetToList(  iOffsetID,  dPos,
                dOffsetCur, pOffsetCh->m_offsetListCur);
}

//获取Y方向的偏移数据
double CWorktableOffsetDriver::GetOffset(int iOffsetID, double dPos)
{
    CWorktableOffset* pOffsetCh=(CWorktableOffset*)FindChannel(iOffsetID);
    Q_ASSERT(pOffsetCh);
    double dThres= pOffsetCh->m_Variants
            .get_VData(Var_WorkTable_Offset_Threshold)->toDouble();

    Q_ASSERT(!DEQUAL0(dThres));
    int iPos=dPos/dThres;
    dPos=iPos*dThres;
    OFFSETLIST &list=pOffsetCh->m_offsetList;
    if(list.isEmpty())
    {
      return 0;
    }
	//在范围以外，则使用最接近的一条数据进行补偿
    int iPosCur=list.first().first/dThres;
    if(iPos>iPosCur)
    {
        return list.first().second;
    }
    OFFSETLIST::iterator ii;
    for(ii=list.begin();ii!=list.end();ii++)
    {
       iPosCur=(*ii).first/dThres;
       if(iPosCur==iPos)
       {
           return (*ii).second;
       }
       if((ii+1)!=list.end())
       {
           int iPosNext=(*(ii+1)).first/dThres;
           if(iPos<iPosCur&&iPos>iPosNext)
           {
			   //找到补偿区间内的数据，Cur>Next
               double dOffsetCur=(*ii).second;
               double dOffsetNext=(*(ii+1)).second;
			   //直线插补
               return double(iPos-iPosCur)/(iPosNext-iPosCur)*(dOffsetNext-dOffsetCur)
                       +dOffsetCur;
           }
       }
    }
    return list.last().second;
}

void CWorktableOffsetDriver::MargeOffset(int iOffsetID)
{
    CWorktableOffset* pOffsetCh=(CWorktableOffset*)FindChannel(iOffsetID);
    Q_ASSERT(pOffsetCh);
     OFFSETLIST &list=pOffsetCh->m_offsetListCur;
    OFFSETLIST::iterator ii=list.begin();
    for(;ii!=list.end();ii++)
    {
        m_iListIndex= AddOffsetToList(  iOffsetID,  (*ii).first,
                     (*ii).second, pOffsetCh->m_offsetList);

        m_iListCurIndex=ii-list.begin();
    }
    list.clear();
    m_iListCurIndex=0;
}

//合并偏移数据
int CWorktableOffsetDriver::AddOffsetToList(int iOffsetID,double dPos,
                                             double dOffset, OFFSETLIST &list)
{

    CWorktableOffset* pOffsetCh=(CWorktableOffset*)FindChannel(iOffsetID);
    Q_ASSERT(pOffsetCh);
    double dThres= pOffsetCh->m_Variants
            .get_VData(Var_WorkTable_Offset_Threshold)->toDouble();
    Q_ASSERT(!DEQUAL0(dThres));
    int iPos=dPos/dThres;
    dPos=iPos*dThres;
    QPair<double,double> pair=QPair<double,double>(dPos,dOffset);
    if(list.isEmpty())
    {
      list.push_back(pair);
      return 0;
    }
    int iPosCur=list.first().first/dThres;
    if(iPos>iPosCur)
    {
        list.push_front(pair);
        return 0;
    }
    OFFSETLIST::iterator ii;

    for(ii=list.begin();ii!=list.end();ii++)
    {
       iPosCur=(*ii).first/dThres;
       if(iPosCur==iPos)
       {
           (*ii).second=dOffset;
           return ii-list.begin();
       }
       if((ii+1)!=list.end())
       {
           int iPosNext=(*(ii+1)).first/dThres;
           if(iPos<iPosCur&&iPos>iPosNext)
           {
               list.insert(ii+1,pair);
               return ii+1-list.begin();
           }
       }
    }
    list.push_back(pair);
    return list.size()-1;
}

void CWorktableOffsetDriver::Serialize(QDataStream &stream, bool bwrite)
{

    UNUSED(stream);
    UNUSED(bwrite);
}

