#include "plotter.h"

const int SCALEX=2;
const int SCALEY=3;
const int BACKLASHX=20;
const int BACKLASHY=20;

template <typename T> int sgn(T val) 
{
    int value = (T(0) < val) - (val < T(0));
    return (value == 0) ? 1 : value;
}

Plotter::Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
{
    _serial = serial;
    _penPort=penPort;
    _penPin=penPin;
    basicposition();
}

Point Plotter::getCurrentPos()
{
	return currentPos;
}


void Plotter::demo()
{
        moveto(0,0);
        _delay_us(200000);
	moveto(32760,0);
        _delay_us(200000);
	moveto(32760,21840);
        _delay_us(200000);
	moveto(0,21840);
        _delay_us(200000);
	moveto(0,0);
        _delay_us(200000);
	pd();
        pu();
}

void Plotter::basicposition()
{
	moveto(0, 0);
}

void Plotter::pd()
{
        if(!readPin(_penPort, _penPin))
        {
            _delay_us(200000);
            writePin(_penPort, _penPin, true);
            _delay_us(500);
        }
}

void Plotter::pu()
{
    if(readPin(_penPort, _penPin))
    {
        _delay_us(100000);
	writePin(_penPort, _penPin, false);
	_delay_us(500);
    }
}

uint16_t Plotter::diamondAngle(int16_t y, int16_t x) //0 - 40000
{
    const int32_t scalar = 10000;
    if (y >= 0) return (x >= 0 ? ((int32_t)y*scalar)/(x+y) : 1*scalar-((int32_t)x*scalar)/(-x+y)); 
    else return (x < 0 ? 2*scalar-((int32_t)y*scalar)/(-x-y) : 3*scalar+((int32_t)x*scalar)/(x-y)); 
}

uint16_t Plotter::calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prevDiamondAngle, uint16_t prevSteps)
{
    uint16_t delayTime = abs(currentDiamondAngle - prevDiamondAngle);
    if ( delayTime > 20000) delayTime = delayTime - 20000;
    if( prevSteps <= 4 && delayTime < 3000 ) delayTime = delayTime/5;
    return delayTime;
}

void Plotter::moveRelative(int32_t deltaX, int32_t deltaY)
{
    if( abs(deltaX) > 50 ||  abs(deltaY) > 50 )
    {
        if( 0 > ((deltaX+currentPos.x)*SCALEX))
        {
            moveto(0,deltaY+currentPos.y);
            return;
        }
        else if( ~(uint16_t)0 <= (deltaX+currentPos.x)*SCALEX)
        {
            moveto(32760,deltaY+currentPos.y);
            return;
        }
        
        if( 0 > (deltaY+currentPos.y)*SCALEY)
        {
            moveto(deltaX+currentPos.x,0);
            return;
        }
        else if( ~(uint16_t)0 <= (deltaY+currentPos.y)*SCALEY)
        {
            moveto(deltaX+currentPos.x,21840);
            return;
        }
            
        //delay
        uint16_t currentDiamondAngle = diamondAngle(deltaX,deltaY);
        uint16_t delayTime = calculateDelayTime(currentDiamondAngle, _prevDiamondAngle, _prevSteps);
        if ( delayTime > 20000) delayTime = delayTime - 20000;
        for (uint16_t i = 0; i < delayTime; i++) _delay_us(50);
        
        /*//compensate backlash
        if((_prevDiamondAngle < 10000  || _prevDiamondAngle > 30000 ) && deltaX < 0) deltaX = deltaX - BACKLASHX;
        else if ((_prevDiamondAngle > 10000  && _prevDiamondAngle < 30000 ) && deltaX > 0) deltaX = deltaX + BACKLASHX;
        
        if((_prevDiamondAngle < 20000) && deltaY < 0) deltaY = deltaY - BACKLASHY;
        else if ((_prevDiamondAngle > 20000) && deltaY > 0) deltaY = deltaY + BACKLASHY;*/
        
        //calculate Interporlation
        uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm
        
        int32_t StepSizeX=0;
        int32_t StepSizeY=0;
    
        if( abs(deltaX) > abs(deltaY))
        {
            StepSizeX = 100*sgn(deltaX);// -100
            StepSizeY = (deltaY*100)/abs(deltaX); // 0
            steps = deltaX/StepSizeX;
        }
        else
        {
            StepSizeX = (deltaX*100)/abs(deltaY);
            StepSizeY = 100*sgn(deltaY);
            steps = deltaY/StepSizeY;
        }
    
        //interpolate
        for( uint16_t i = 0; i < steps; i++)
        {
            _pwm.setDutyA(~((uint16_t)(currentPos.x+i*StepSizeX)*SCALEX));
            _pwm.setDutyB(~((uint16_t)(currentPos.y+i*StepSizeY)*SCALEY));
            _delay_us(2500);
        }
        //set prev
        _prevDiamondAngle = currentDiamondAngle;
        _prevSteps = steps;
    }
    else 
    {
        _delay_us(100);
    }

    _pwm.setDutyA(~((uint16_t)(deltaX+currentPos.x)*SCALEX));
    _pwm.setDutyB(~((uint16_t)(deltaY+currentPos.y)*SCALEY));
    
    _delay_us(3000);
    if(highPrecision)  _delay_us(80000); 
    
    if( abs(deltaX) > 50 ||  abs(deltaY) > 50 || highPrecision )
    {
        //set current position
        currentPos.x = deltaX+currentPos.x;
        currentPos.y = deltaY+currentPos.y;
    }
}

void Plotter::setHighPrecision(bool in)
{
    highPrecision = in;
}

void Plotter::moveto(Point *pt)
{
	moveto((*pt).x,(*pt).y);
}

void Plotter::moveto(const uint16_t nx, const uint16_t ny)
{
    int32_t deltaX = (int32_t)nx - (int32_t)currentPos.x; //117 units per mm
    int32_t deltaY = (int32_t)ny - (int32_t)currentPos.y; //121 units per mm
    moveRelative(deltaX, deltaY);
}