#include "plotter.h"

const int SCALEX=2;
const int SCALEY=3;

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

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


void Plotter::demo()
{
	moveto(65535,0);
	moveto(65535,42129);
	moveto(0,42129);
	moveto(0,0);
	pd();
        pu();
}

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

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

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

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

void Plotter::moveto(const uint16_t nx, const uint16_t ny)
{
    int deltaX = nx - currentPos.x; //-5100
    int deltaY = ny - currentPos.y; //0
    
    int steps = 0;
    int32_t StepSizeX=0;
    int32_t StepSizeY=0;
    
    if( abs(deltaX) > abs(deltaY))
    {
        if(deltaX != 0) StepSizeX = 100*(deltaX/abs(deltaX));// -100
        if(deltaY != 0) StepSizeY = ((int32_t)abs(deltaY)*100)/abs((int32_t)deltaX)*(deltaY/abs(deltaY));// 0
        steps = deltaX/StepSizeX;
    }
    else
    {
        StepSizeX = (((int32_t)abs(deltaX)*100)/abs((int32_t)deltaY))*(deltaX/abs(deltaX));
        StepSizeY = 100*(deltaY/abs(deltaY));
        steps = deltaY/StepSizeY;
    }
    
    for( int i = 0; i < steps; i++)
    {
        _pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
        _pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
        _delay_us(5000);
    }

    
    _pwm.setDutyA(65535-nx*SCALEX);
    _pwm.setDutyB(65535-ny*SCALEY);
    _delay_us(10000);
    
    if(steps < 50) for(int i = 0; i < steps; i++)_delay_us(11000);
    else if(steps < 5) _delay_us(10000*5);
    else _delay_us(200000);
    
    //_delay_us(500000);
    //else _delay_us(10000);
    
    currentPos.x = nx;
    currentPos.y = ny;
}