#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <EEPROM.h>
#include "Settings.h"

Settings settings = { 0 };
Keypad keypad = Keypad(makeKeymap(keys), rowPINS, colPINS, KAYPAD_ROWS, KAYPAD_COLS);
LiquidCrystal_I2C lcd(0x27, 20, 4);

char lastKey = ' ';
bool isFirstRun = true;

float Degrees = 0.0;                       // Degrees from Serial input or number of degrees per division
float TotalDegrees = 0.0;                  // total number of degrees moved
float StepsPerIncrementTheoretical = 0.0;  // Number of *theoretical* microsteps to move (can be a fractional number)
unsigned long StepsToMove = 0;             // Actual number of (integer) steps to move
float TotalStepsTheoretical = 0.0;         // Theoretical total number of steps moved
unsigned long TotalStepsActualPrevious = 0;          // Previous total of steps moved
unsigned long TotalStepsActual = 0;                  // Cumulative total of steps moved
int Direction = 0;                         // Direction = the direction of rotation of the last move, either
int Fwd = 1;                               //    forward (Fwd) or
int Rvs = -1;                              //    reverse (Rvs)

void showMainMenu()
{
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("A=Wybierz Kat");
    lcd.setCursor(0,1);
    lcd.print("B=Ile w / po okregu");
    lcd.setCursor(0,2);
    lcd.print("C=Skocz");
    lcd.setCursor(0,3);
    lcd.print("D=Ustawienia");
}

float getnumber(String prompt, int intvalue, float floatvalue)
{
    float num = 0.00;
    float decimal = 0.00;
    float decnum = 0.00;
    int counter = 0;  
    lcd.clear();
    lcd.home();
    lcd.print(prompt);
    lcd.setCursor(0,1); lcd.print("Value = ");
    if (floatvalue != 0)
        { lcd.print(floatvalue,4); }
    else
        { lcd.print(intvalue); }
                        
    lcd.setCursor(0,3); lcd.print("#=ENTER  D=Kasuj");

    lcd.setCursor(8,1);
    boolean decOffset = false;
    char key = keypad.getKey();

    while(key != '#')
    {
        switch (key)
        {            
        case '.':
            if(!decOffset)
            {
                decOffset = true;
                lcd.print(key);
            }
            break;   
        
        case 'D':
            num=0.00;
            lcd.setCursor(8,1);
            lcd.print("            ");   
            lcd.setCursor(8,1);
            decOffset = false;
            break;
            
        case '0': case '1': case '2': case '3': case '4':
        case '5': case '6': case '7': case '8': case '9':
            if (num == 0.0)
            {
                lcd.setCursor(8,1);
                lcd.print("            ");
                lcd.setCursor(8,1);     
            }

            if (!decOffset)
            {
                num = num * 10.0 + (key - '0');
                lcd.print(key);
            }
            else if ((decOffset) && (counter <= 4))
            {
                num = num * 10.0 + (key - '0');
                lcd.print(key);
                counter++;
            }
            break;
        }

        decnum = num / pow(10, counter);
        key = keypad.getKey();
    }

    return decnum;
}

long calcSettingsChecksum()
{
    long calculatedChecksum = 0;

    calculatedChecksum += settings.stepsPerFullRotation;
    calculatedChecksum += settings.microStepping;
    calculatedChecksum += settings.stepdelay;
    calculatedChecksum += settings.backlash;
    calculatedChecksum += (settings.beep ? 1 : 0);
    calculatedChecksum += settings.microStepsPerFullRotation;
    calculatedChecksum += (long)(settings.tableRatio * 1000);
    calculatedChecksum += (long)(settings.secondMotorDistanceMM * 1000);
    calculatedChecksum += (long)(settings.screwPitchMM * 1000);
    calculatedChecksum += settings.secondMotorStepsPerFullRotation;

    return calculatedChecksum;
}

float getjog()
{
    float steps = 0.0;
    float StepA = 1.0;
    float StepB = 0.0;
    StepB = settings.microStepsPerFullRotation / 360.0;
    float StepC = StepB * 5.0;

    char key = keypad.getKey();
    lcd.clear();
    lcd.setCursor(0,0);lcd.print("Jog (1 Step, 1");lcd.print((char)223);lcd.print(", 5");lcd.print((char)223);lcd.print(")");
    lcd.setCursor(0,1);lcd.print(" A=1  B=    C=    ");
    lcd.setCursor(8,1);lcd.print(StepB,0);
    lcd.setCursor(14,1);lcd.print(StepC,0);
    lcd.setCursor(0,2);lcd.print("Enter Wybierz:");lcd.setCursor(0,3);lcd.print("#=ENTER  D=Kasuj");

    while (key != '#')
    {
        switch (key)
        {
            case 'A':
                steps = StepA;      
                lcd.setCursor(14,2);lcd.print("A");
                break;

            case 'B':
                steps = StepB;
                lcd.setCursor(14,2);lcd.print("B");
                break;

            case 'C':
                steps = StepC;
                lcd.setCursor(14,2);lcd.print("C");
                break;

            case 'D':
                steps = 0;
                lcd.setCursor(14,2);lcd.print("       ");
                lcd.setCursor(14,2);
                break;
        }

        key = keypad.getKey();
    }

    return steps;
}

void handleSettings()
{
    float temp = 0.0f;
    
    String prompt = "1 Full steps in 360";
    prompt += char(223);
    temp = getnumber(prompt, settings.stepsPerFullRotation, 0);
    if (temp != 0) { settings.stepsPerFullRotation = temp; }

    prompt = "2 Full steps in 360";
    prompt += char(223);
    temp = getnumber(prompt, settings.secondMotorStepsPerFullRotation, 0);
    if (temp != 0) { settings.secondMotorStepsPerFullRotation = temp; }

    temp = getnumber("Micro-stepping", settings.microStepping, 0);
    if (temp != 0) { settings.microStepping = temp; }

    temp = getnumber("Przelozenie", 0, settings.tableRatio);
    if (temp != 0) { settings.tableRatio = temp; }

    temp = getnumber("Opoznienie-microsec", settings.stepdelay, 0);
    if (temp != 0) { settings.stepdelay = temp; }
 
    temp = getnumber("Backlash correction", settings.backlash, 0);
    if (temp != 0) { settings.backlash = temp; }
    
    temp = getnumber("Beep: 1=ON, 2=OFF", settings.beep, 0);
    if (temp != 0) { settings.beep = temp; }

    temp = getnumber("Screw Pitch (mm)", 0, settings.screwPitchMM);
    if (temp != 0) { settings.screwPitchMM = temp; }

    temp = getnumber("Distance", 0, settings.secondMotorDistanceMM);
    if (temp != 0) { settings.secondMotorDistanceMM = temp; }
    
    settings.microStepsPerFullRotation = (long)settings.stepsPerFullRotation * (long)settings.microStepping * (long)settings.tableRatio;
    settings.checksum = calcSettingsChecksum();
    EEPROM.put(0, settings);
}

void handleMainMenu()
{
    int choice = 0;
    showMainMenu();

    while(choice == 0)
    {
        char key = keypad.getKey();

        switch (key)
        {    
        case 'A':
            Degrees = getnumber("Ile stopni na ruch", 0, 0);
            StepsPerIncrementTheoretical = Degrees / 360.0 * settings.microStepsPerFullRotation;
            choice = 1;
            break;
        
        case 'B':
            StepsPerIncrementTheoretical = settings.microStepsPerFullRotation / getnumber("Sides or Teeth", 0, 0);
            Degrees =  StepsPerIncrementTheoretical / settings.microStepsPerFullRotation * 360.0; 
            choice = 2;
            break;
        
        case 'C':
            StepsPerIncrementTheoretical = getjog();
            Degrees = StepsPerIncrementTheoretical / settings.microStepsPerFullRotation * 360.0;
            choice = 3;
            break;

        case 'D':
            handleSettings();
            showMainMenu();
            break;
        }
    }
}

void printadvance()
{
    lcd.setCursor(6,1);lcd.print("Moving 1");
    lcd.setCursor(0,2);lcd.print("Kroki=");
    lcd.setCursor(6,2);lcd.print("              ");
    lcd.setCursor(6,2);     
    
    lcd.print(TotalStepsActual);
    lcd.print("/");
    lcd.print(settings.microStepsPerFullRotation);
    lcd.setCursor(13,0);lcd.print("       ");
    lcd.setCursor(13,0);lcd.print(TotalDegrees,2);lcd.print((char)223);
    rotation(StepsToMove);
    
    lcd.setCursor(6,1);lcd.print("        ");
    
    if (settings.beep == 1)
    {
        tone(13, 4000, 500);
    }
} 

void rotation(unsigned long tm)
{ 
    for(unsigned long i = 0; i < tm; i++) 
    { 
        digitalWrite(MOTOR_1_STEP, HIGH); 
        delayMicroseconds(settings.stepdelay); 
        digitalWrite(MOTOR_1_STEP, LOW); 
        delayMicroseconds(settings.stepdelay); 
    }
}

void loadAndCheckEeprom()
{
    EEPROM.get(0, settings);
    long calculatedChecksum = calcSettingsChecksum();

    if (settings.checksum != calculatedChecksum)
    {
        // Initialize default settings
        settings.stepsPerFullRotation = 200;
        settings.microStepping = 16;
        settings.tableRatio = 26.851239669f;
        settings.stepdelay = 50;
        settings.backlash = 10;
        settings.beep = true;
        settings.microStepsPerFullRotation = (long)settings.stepsPerFullRotation * (long)settings.microStepping * (long)settings.tableRatio;
        settings.secondMotorDistanceMM = 5.0f;
        settings.screwPitchMM = 2.0f;
        settings.secondMotorStepsPerFullRotation = 200;

        settings.checksum = calcSettingsChecksum();
        EEPROM.put(0, settings);
    }
}

void rotateSecondMotor()
{
    float distance = settings.secondMotorDistanceMM;
    float revolutions = distance / settings.screwPitchMM;
    long steps = (long)(revolutions * settings.secondMotorStepsPerFullRotation);

    lcd.setCursor(6,1);lcd.print("Moving 2");

    // Move forward
    digitalWrite(MOTOR_2_DIR, HIGH);

    for (long i = 0; i < steps; i++)
    {
        digitalWrite(MOTOR_2_STEP, HIGH);
        delayMicroseconds(settings.stepdelay);
        digitalWrite(MOTOR_2_STEP, LOW);
        delayMicroseconds(settings.stepdelay);
    }

    // Move backward
    digitalWrite(MOTOR_2_DIR, LOW);

    for (long i = 0; i < steps; i++)
    {
        digitalWrite(MOTOR_2_STEP, HIGH);
        delayMicroseconds(settings.stepdelay);
        digitalWrite(MOTOR_2_STEP, LOW);
        delayMicroseconds(settings.stepdelay);
    }

    lcd.setCursor(6,1);lcd.print("        ");
}

void setup()
{
    loadAndCheckEeprom();

    pinMode(MOTOR_1_STEP, OUTPUT);
    pinMode(MOTOR_1_DIR, OUTPUT); 
    pinMode(MOTOR_2_STEP, OUTPUT);
    pinMode(MOTOR_2_DIR, OUTPUT); 

    lcd.init();
    lcd.backlight();
    lcd.setCursor(0,0);
    lcd.print("Podzielnica");
    lcd.setCursor(3,2);
    lcd.print("Rev. " VERSION_NUMBER " 2026");
    delay(1500);  

    handleMainMenu();
}

void loop()
{
    lcd.clear();
    char key = keypad.getKey();

    // Initialize variables
    TotalDegrees = 0.0;
    StepsToMove = 0;
    TotalStepsTheoretical = 0.0;
    TotalStepsActualPrevious = 0;
    TotalStepsActual = 0;
    isFirstRun = true;
    lastKey = ' ';

    lcd.setCursor(7,0);
    lcd.print("Total:  ");
    lcd.print(TotalDegrees,2);
    lcd.print((char)223);

    lcd.setCursor(0,2);
    lcd.print("Kroki=");
    lcd.setCursor(6,2);
    lcd.print("              ");
    lcd.setCursor(6,2);
    lcd.print(0.0, 0);
    lcd.print("/");
    lcd.print(settings.microStepsPerFullRotation);

    lcd.setCursor(0,3);
    lcd.print("A=FWD  B=RVS  C=MENU");

    while(key != 'C')
    {
        lcd.setCursor(0,0);
        lcd.print(abs(Degrees),2);
        lcd.print((char)223);
        key = keypad.getKey();
        float totalDegreesBefore = TotalDegrees;

        if (lastKey != ' ' && key == NO_KEY)
        {
            key = lastKey;    // Continue last direction if in automatic mode
        }

        if(key == 'A')
        {
            lastKey = 'A';
            TotalDegrees  = TotalDegrees + Degrees;
            if (TotalDegrees > 360.0)                        // When making a full rotation, restart count at 360 degrees
            {
                TotalDegrees = TotalDegrees - 360.0;
            }

            TotalStepsActualPrevious = TotalStepsActual;                      // Save current steps total as previous total
            TotalStepsTheoretical = TotalStepsTheoretical + StepsPerIncrementTheoretical;

            /*
            Round up to integer actual total number of steps moved.
            If a fractional value is N.5 or greater, adding .5 will increase the interger portion to N+1.
            Conversion to int will truncate any fractional part. Result is that any number with a fractional
            part of >=.5 will be rounded up; <.5 will be rounded down.
            */
            TotalStepsActual = (unsigned long) (TotalStepsTheoretical + 0.5);
            StepsToMove = TotalStepsActual - TotalStepsActualPrevious;

            //If we have moved through a full revolution, reset actual steps to begin counting from zero.
            if ( TotalStepsTheoretical >=  settings.microStepsPerFullRotation )
            {
                TotalStepsActual = (unsigned long) (TotalStepsTheoretical - settings.microStepsPerFullRotation);
                TotalStepsTheoretical = TotalStepsTheoretical - settings.microStepsPerFullRotation;
            }

            if (Direction == 0)
            {
                Direction = Fwd;   //If this is the first rotation, set the direction to forward
            }

            if (Direction == Rvs)  //If reversing direction, apply backlash correction and reset direction.
            {
                StepsToMove = StepsToMove + settings.backlash;
                Direction = Fwd;
            }

            if (!isFirstRun && TotalDegrees < totalDegreesBefore)
            {
                // Completed a full rotation
                key = 'C';   // Exit to main menu
                continue;
            }

            digitalWrite(MOTOR_1_DIR, LOW);
            printadvance();
            rotateSecondMotor();

            isFirstRun = false;
        }

        if(key=='B')              // MOVE REVERSE
        {
            lastKey = 'B';
            TotalDegrees  = TotalDegrees - Degrees;
            if ( TotalDegrees < 0.0)
            {
                TotalDegrees = TotalDegrees + 360.0;
            }

            /* Note:
               For reverse moves theoretical reverse steps are calculated as the equivalent forward theoretical steps.
            */

            TotalStepsActualPrevious = TotalStepsActual;                      // Save current steps total as previous total

            if (TotalStepsTheoretical - StepsPerIncrementTheoretical >= 0.0)  // If next move does NOT go through a full rotation
            {
                TotalStepsTheoretical = TotalStepsTheoretical - StepsPerIncrementTheoretical;
            }
            else                                                              // Next move >= full rotation, so reset
            {
                TotalStepsTheoretical = TotalStepsTheoretical - StepsPerIncrementTheoretical + settings.microStepsPerFullRotation;
            }

            TotalStepsActual = (unsigned long) (TotalStepsTheoretical - 0.5);   //Use rounded value
            if (TotalStepsActual < TotalStepsActualPrevious)
            {
                StepsToMove =  TotalStepsActualPrevious - TotalStepsActual;
            }
            else
            {
                StepsToMove =  TotalStepsActualPrevious + (unsigned long) settings.microStepsPerFullRotation  - TotalStepsActual;
            }

            if (Direction == 0)
            {
                Direction = Rvs;   //If this is the first rotation, set the direction to forward
            }

            if (Direction == Fwd)                      //If reversing direction, apply backlash correction and reset direction.
            {
                StepsToMove = StepsToMove + settings.backlash;
                Direction = Rvs;
            }

            if (!isFirstRun && TotalDegrees > totalDegreesBefore)
            {
                // Completed a full rotation
                key = 'C';   // Exit to main menu
                continue;
            }

            digitalWrite(MOTOR_1_DIR, HIGH);
            printadvance();
            rotateSecondMotor();

            isFirstRun = false;
        }
    }

    setup();
}