#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "PCANBasic.h"
#include "message_queue.h"
#include "encoder_sensor.h"
CEncoderSensor::CEncoderSensor(CMessageQueue *q):_message(q)
{

}
void CEncoderSensor::Start()
{
     _channelReady=false;
     CIOBuffer pBuffer;
     cannet_frame* frame=reinterpret_cast<cannet_frame *>(pBuffer.Buffer);
     memset(frame->data,0,sizeof(frame->data));
     frame->dlc=2;
     frame->canid=0x0000;
     frame->data[0]=0x01;
     frame->data[1]=0x00;
     _socket->Write(pBuffer.Buffer,sizeof(cannet_frame));

}
void CEncoderSensor::SetSensorSocket(SensorSocket<CEncoderSensor>* can)
{
    _socket=can;
}
void CEncoderSensor::Stop()
{
    _channelReady=false;
}
extern bool g_collbrate;
void CEncoderSensor::Notify(int8_t * buffer,int32_t size)
{
    
     int32_t receivedCnt = size/CAN_MSG_LEN;
	 cannet_frame* p = (cannet_frame*)buffer;
     for(int32_t i=0;i<receivedCnt;i++)
     {
         int32_t id_normal=ntohl(p[i].canid);
       
        switch(id_normal)
        {
             case 0x1B1:
            {
               int32_t l0=p[i].data[0]&0x000000ff;
               int32_t l1=p[i].data[1]&0x000000ff;
               int16_t last_angle=  (l1<<8)|l0;
               //_data.left_angle=last_angle;
               _message->SetLeftAngle(last_angle);
               if(g_collbrate)
                std::cout<<"1b1 "<<","<<last_angle<<std::endl;
            }

            break;
         case 0x1B2:
           {
               int32_t l0=p[i].data[0]&0x000000ff;
               int32_t l1=p[i].data[1]&0x000000ff;
               int16_t last_angle=  (l1<<8)|l0;
               //_data.right_angle=last_angle;
               _message->SetRightAngle(last_angle);
               if(g_collbrate)
                  std::cout<<"1b2 "<<","<<last_angle<<std::endl;
           }
           break;
        }
     }
        CIOBuffer Buffer; 
        static auto tick= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        auto now= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        if(now-tick>1000)
        {
            
            cannet_frame* frame=reinterpret_cast<cannet_frame *>(Buffer.Buffer);
            memset(frame->data,0,sizeof(frame->data));
            frame->dlc=2;
            frame->canid=0x0000;
            frame->data[0]=0x01;
            frame->data[1]=0x00;
            _socket->Write(Buffer.Buffer,sizeof(cannet_frame));
            tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        }
         std::this_thread::sleep_for(std::chrono::milliseconds(100));   
}