#include "encoder_implement.h"
#include "board.h"
#include "print.h"
EncoderImp::EncoderImp(unsigned char _num, bool _reverse) : reverse(_reverse), num(_num)
{
    clear();
}

void EncoderImp::init()
{
    Board::get()->encoder_init(num);
    clear();
}

void EncoderImp::clear()
{
    total_count = 0;
    pid_pos = odom_pos = 0;
}

long EncoderImp::get_total_count()
{
    if (reverse)
        total_count += Board::get()->GetEncoderCount(num);
    else
        total_count -= Board::get()->GetEncoderCount(num);
		
    return total_count;
}

long EncoderImp::get_increment_count_for_dopid()
{
    if (reverse)
        total_count += Board::get()->GetEncoderCount(num);
    else
        total_count -= Board::get()->GetEncoderCount(num);
    long l = total_count-pid_pos;
    pid_pos = total_count;
    return l;
}

long EncoderImp::get_increment_count_for_odom()
{
    if (reverse)
        total_count += Board::get()->GetEncoderCount(num);
    else
        total_count -= Board::get()->GetEncoderCount(num);
    
    long l = total_count-odom_pos;
    odom_pos = total_count;
    return l;
}