//#include <elapsedMillis.h>
//#include <PGMWrap.h>
#include <avr/pgmspace.h>
#include <stdint.h>
#include <math.h>
//#include <avr/io.h>

//0b000 = 360 / 200 = 1.8
//0b001 = 360 / 400 = 0.9
//0b010 = 360 / 800 = 0.45
//0b011 = 360 / 1600 = 0.225 
// 2^3 

//PB0 = Direction
//PB1 = Step
//PB2 = MS1
//PB3 = MS2
//PB4 = Enable

//PD0 = Direction
//PD1 = Step
//PD2 = MS1
//PD3 = MS2
//PD4 = Enable

//PD6 = Laser

//G00 X10 Y25
//PORTB is X
//PORTD is Y



// dict of freq multipliers

const uint16_t clk = 16000000/1024;

const uint8_t y_wheel_diameter = 60;
const uint8_t x_wheel_diameter = 60;


struct StepsMM {
    float x;
    float y;
    
    StepsMM(uint8_t x_diameter, uint8_t y_diameter, uint8_t step_settings){
        uint16_t step_rate = this->microsteps(step_settings);
        this->x = step_rate / (x_diameter * 3.14159);
        this->y = step_rate / (y_diameter * 3.14159);
    };

    uint16_t microsteps(uint8_t step_mask) {
        return 100*pow(2, step_mask+1);
    };
};


StepsMM steps_mm = StepsMM(x_wheel_diameter, y_wheel_diameter, 4);


typedef struct Vec{
    float x;
    float y;
} Vec;


typedef struct Move {
    uint16_t x; // time in TIMER1 counts between x steps
    uint16_t x_steps; // number of steps required to complete the move
    uint8_t x_dir;
    
    uint16_t y; // time in TIMER1 counts between y steps
    uint16_t y_steps; // number of steps required to complete the move
    uint8_t y_dir;
} Move;


struct GlobalCoords {
    float x = 0.0;
    float y = 0.0;
    uint8_t mode = 91;

    Vec position(float x, float y) {
        switch (this->mode) {
            case 90:
                return this->absolute(x, y);
            case 91:
                return this->relative(x, y);
        }
    };

    Vec relative(float x, float y) {
        this->x += x;
        this->y += y;
        return Vec{ x, y };    
    };

    Vec absolute(float x, float y) {
        Vec ret = Vec{ x - this->x, y - this->y };
        this->x = x;
        this->y = y;
        return ret;
    };
};


GlobalCoords coords;


void set_direction(uint8_t *port, uint8_t pin, uint8_t dir) {
    if (dir == 1) {
        *port |= (1 << pin);
    } else {
        *port &= ~(1 << pin);
    }
}


uint16_t get_timer1( void ) {
    unsigned char sreg;
    uint16_t i;
    /* Save global interrupt flag */ sreg = SREG;
    /* Disable interrupts */
    cli();
    /* Read TCNT1 into i */
    i = TCNT1;
    /* Restore global interrupt flag */ SREG = sreg;
    return i;
}


struct Move get_move(int *x, int *y, int *speed, int mode = 1) { // todo incorporate acceleration planning
    
    // get the move vector in absolute or relative coordinates
    Vec xy = coords.position(*x, *y);
    
    // calculate mm/s for x and y for vector speed
    Vec vec;
    if (xy.x == 0 && xy.y == 0) {
        vec = {xy.x, xy.y};
    } else {
        float v = *speed / sqrt( ((xy.x) * (xy.x)) + ((xy.y) * (xy.y)) );
        vec = {xy.x * v, xy.y * v};
    }

    Move move;
    // calculate TIMER1 counts for x and y stepper motor pulses
    move.x = vec.x != 0 ? abs(clk / ( vec.x * steps_mm.x )) : 0;
    move.y = vec.y != 0 ? abs(clk / ( vec.y * steps_mm.y )) : 0;

    // get move direction for x and y
    move.x_dir = vec.x > 0 ? 1 : 0;
    move.y_dir = vec.y > 0 ? 1 : 0;
    
    // get number of steps for x and y required to complete the move
    move.x_steps = abs( xy.x * steps_mm.x );
    move.y_steps = abs( xy.y * steps_mm.y );

    Serial.print("move->x_steps: ");
    Serial.print(move.x_steps);
    Serial.print("  move->y_steps: ");
    Serial.println(move.y_steps);
    return move;
}

struct Move parse_gcode(String command){
    char cmds[3] = {'X', 'Y', 'F'};
    int x, y, speed; 
    int atoi[3]; 
    int8_t start_cmd, end_cmd = -1;

    for (uint8_t i = 0; i < 3; i++) {
        char c = cmds[i];
        for(uint8_t j = 0; j < command.length(); j++) {
           if (command[j] == c) {
               start_cmd = j+1;
               continue;
           }
           if (start_cmd == -1) {
               continue;
           }
           if (command[j] == ' ' || command[j] == '\n') {
               end_cmd = j;
               break;
           }
           end_cmd = -1;
        }
        atoi[i] = command.substring(start_cmd, end_cmd).toInt();
        start_cmd = -1;

    }
    x = atoi[0];
    y = atoi[1];
    speed = atoi[2];

    Serial.print("X: ");
    Serial.print(x);
    Serial.print("  Y: ");
    Serial.print(y);
    Serial.print("  F: ");
    Serial.println(speed);

    return get_move(&x, &y, &speed);
}

struct Move parse_gcode2(String command){
    int g = 0, x = 0, y = 0, speed = 0;
    int* params[4] = {&g,&x, &y, &speed};
    char cmds[4] = {'G', 'X', 'Y', 'F'};

    for (uint8_t i = 0; i < 4; i++) {
        char* cmd_start = strchr(command.c_str(), cmds[i]);
        if (cmd_start) {
            *params[i] = atoi(cmd_start + 1);
        }
    }
    
    switch (g) {
        case 90:
            coords.mode = 90;
            break;
        case 91:
            coords.mode = 91;
            break;        
    }

    return get_move(&x, &y, &speed);
}

void move_stepper(Move *move) {
    if (move->x_steps == 0 && move->y_steps == 0) {
        return;
    }
    uint16_t t;
    uint16_t next_x_steps = move->x;
    uint16_t next_y_steps = move->y;
    
    // take in PORTx as a parameter
    
    // enable stepper and set microstepping mode
    PORTB |= (1 << PB2) | (1 << PB3);
    PORTD |= (1 << PD4) | (1 << PD5);
    
    //set directions
    set_direction(&PORTB, PB0, move->x_dir);
    set_direction(&PORTD, PD2, move->y_dir);

    
    TCNT1 = 0;
    next_x_steps = move->x;
    next_y_steps = move->y;

    //Serial.println(move->x_steps);
    
    while (move->x_steps > 0 || move->y_steps > 0){
        t = get_timer1();
        //TCNT1 = t;
        
        if (t >= next_x_steps) {
            if (move->x_steps > 0) {
                PORTB ^= (1 << PB1);
                PORTB ^= (1 << PB1);
                next_x_steps = t+move->x;
                move->x_steps -= 1;
            }
        }
        if (t >= next_y_steps) {
            if (move->y_steps > 0) {
                // pulse y stepper motor
                PORTD ^= (1 << PD3);
                PORTD ^= (1 << PD3);
                next_y_steps = t+move->y;
                move->y_steps -= 1;
            }
        }
    }

    Serial.println(TCNT1);
}


enum fCLK {
    fCLK_1 = (1<<CS10),
    fCLK_8 = (1<<CS11),
    fCLK_64 = (1<<CS11)|(1<<CS10),
    fCLK_256 = (1<<CS12),
    fCLK_1024 = (1<<CS12)|(1<<CS10)
};


void setup() {
  //stop interrupts
  cli();
  DDRB |= (1<<PB0)|(1<< PB1)|(1 << PB2)|(1 << PB3)|(1 << PB4);
  DDRD |= (1<<PD2)|(1<< PD3)|(1 << PD4)|(1 << PD5)|(1 << PD6);
  TCCR1A = 0b00000000;
  
  TCCR1B = 0b00000101;
  TCNT1 = 0;
  sei();
  Serial.begin(256000);
  
}



void loop() {
    String gcode = "G90 X10 Y1 F200";
    move_stepper(&parse_gcode2(gcode));
}