#include <gpiod.h>
#include <cstdio>
#include <unistd.h>
#include <iostream>
#include <time.h>
#include <cstring>

unsigned int motorPins[] = { 27, 22, 23, 24 };
unsigned int inputPin = 17;
const char pinName1[] = "MotorPin1";
const char pinName2[] = "MotorPin2";
const char pinName3[] = "MotorPin3";
const char pinName4[] = "MotorPin4";
const char* const pinNames[] = { pinName1, pinName2, pinName3, pinName4 };

const int secondsPerDay = 24 * 60 * 60;
struct gpiod_line* line[4];
struct gpiod_line* inputLine;

int currentMotorPhase = 0;
int currentStepPosition = 0;
int numPositionsTotal = 2048;

int numConsecutiveForwardStepsWithoutTrigger = 0;
int numStepsWithoutTrigger = 0;

#define SENSOR_ACTIVE_HIGH (1)
#define SENSOR_ACTIVE_LOW  (0)

const int inputTriggerValue = SENSOR_ACTIVE_LOW;

const int CENTERING_POSITION_STEP_VALUE = 0;

bool debuPrintsOn = false;

int inputPinState()
{
    return gpiod_line_get_value(inputLine);
}

void stepForward()
{
    int prevMotorPhase = currentMotorPhase;
    
    currentMotorPhase++;
    
    if (currentMotorPhase >= 4)
    {
        currentMotorPhase = 0;
    }
    
    gpiod_line_set_value(line[currentMotorPhase], 1);
    usleep(1000);
    gpiod_line_set_value(line[prevMotorPhase], 0);
    usleep(10000);
    
    currentStepPosition += 1;
    
    if (currentStepPosition >= numPositionsTotal)
    {
        currentStepPosition = 0;
    }
    
    int inputState = inputPinState();
    
    if (inputState == inputTriggerValue)
    {
        // Debounce.
        if (numConsecutiveForwardStepsWithoutTrigger > 20)
        {
            if (debuPrintsOn)
            {
                std::cout << "Position reset at " << currentStepPosition << std::endl;
            }

            // Encountered the calibration trigger. Reset position counter.
            currentStepPosition = CENTERING_POSITION_STEP_VALUE;
        }
        
        numConsecutiveForwardStepsWithoutTrigger = 0;
    }
    else
    {
        numConsecutiveForwardStepsWithoutTrigger += 1;
    }
}

void stepBackward()
{
    int prevMotorPhase = currentMotorPhase;
    
    if (currentMotorPhase == 0)
    {
        currentMotorPhase = 3;
    }
    else
    {
        currentMotorPhase -= 1;
    }
    
    gpiod_line_set_value(line[currentMotorPhase], 1);
    usleep(1000);
    gpiod_line_set_value(line[prevMotorPhase], 0);
    usleep(10000);
    
    if (currentStepPosition == 0)
    {
        currentStepPosition = numPositionsTotal - 1;
    }
    else
    {
        currentStepPosition -= 1;
    }
    
    numConsecutiveForwardStepsWithoutTrigger = 0;
}

void releaseMotor()
{
    gpiod_line_set_value(line[currentMotorPhase], 0);
}

void takeHoldOfMotor()
{
    gpiod_line_set_value(line[currentMotorPhase], 1);
}

void driveForwardNSteps(int numSteps)
{
    while (numSteps != 0)
    {
        stepForward();
        numSteps -= 1;
    }
}

void driveBackwardNSteps(int numSteps)
{
    while (numSteps != 0)
    {
        stepBackward();
        numSteps -= 1;
    }
}

void driveToPosition(int position)
{
    if (currentStepPosition == position)
    {
        return;
    }
    
    int stepsForward;
    
    if (position < currentStepPosition)
    {
        stepsForward = (numPositionsTotal + position) - currentStepPosition;
    }
    else
    {
        stepsForward = position - currentStepPosition;
    }
    
    int stepsBackward = numPositionsTotal - stepsForward;
    
    if (stepsForward < stepsBackward)
    {
        if (debuPrintsOn)
        {
            std::cout << "Forward " << stepsForward << std::endl;
        }

        driveForwardNSteps(stepsForward);
    }
    else
    {
        if (debuPrintsOn)
        {
            std::cout << "Backward " << stepsBackward << std::endl;
        }

        driveBackwardNSteps(stepsBackward);
    }
}

void releaseNLines(int nLines)
{
    gpiod_line_release(inputLine);

    while (nLines != 0)
    {
        nLines -= 1;
        gpiod_line_release(line[nLines]);
    }
}

int main(int argc, char **argv)
{   
    bool shortTestMode = false;

    if (argc >= 2)
    {
        for (int i = 0; i < argc; i++)
        {
            if (strcmp(argv[i], "-v") == 0)
            {
                debuPrintsOn = true;
            }
            else if (strcmp(argv[i], "-t") == 0)
            {
                shortTestMode = true;
            }
        }
    }

    if (debuPrintsOn)
    {
        std::cout << "Starting" << std::endl;
    }

    if (shortTestMode && debuPrintsOn)
    {
        std::cout << "Short test selected" << std::endl;
    }

    const char *chipname = "gpiochip0";
    unsigned int val;
    struct gpiod_chip* chip;
    int ret;

    chip = gpiod_chip_open_by_name(chipname);
    if (!chip)
    {
        if (debuPrintsOn)
        {
            std::cout << "Open chip failed!" << std::endl;
        }

        return 1;
    }
    
    inputLine = gpiod_chip_get_line(chip, inputPin);

    if (!inputLine)
    {
        if (debuPrintsOn)
        {
            std::cout << "Get input line failed" << std::endl;
        }

        gpiod_chip_close(chip);
        return 1;
    }

    ret = gpiod_line_request_input(inputLine, "GpioClock");

    if (ret < 0)
    {
        if (debuPrintsOn)
        {
            std::cout << "Request line as input failed" << std::endl;
        }

        releaseNLines(0);
        gpiod_chip_close(chip);
        return 1;
    }

    for (int i = 0; i < 4; i++)
    {
        line[i] = gpiod_chip_get_line(chip, motorPins[i]);

        if (line[i] == 0)
        {
            if (debuPrintsOn)
            {
                std::cout << "Get line failed!" << std::endl;
            }

            releaseNLines(i);
            gpiod_chip_close(chip);
            return 1;
        }

        ret = gpiod_line_request_output(line[i], "GpioClock", 0);

        if (ret < 0)
        {
            if (debuPrintsOn)
            {
                std::cout << "Request line as output failed!" << std::endl;
            }

            releaseNLines(i + 1);
            gpiod_chip_close(chip);
            return 1;
        }
        
        // Initialize all motor pins to non-driving.
        gpiod_line_set_value(line[i], 0);
    }
    
    // Start driving a coil in the motor.
    takeHoldOfMotor();
    
    // Cause the motor position counter to get calibrated by driving to the trigger at least once.
    driveForwardNSteps(numPositionsTotal * 3 / 2);

    struct tm* timeInfo;

    int loopCount = 0;

    while(1)
    {
        if (shortTestMode)
        {
            loopCount++;

            if (loopCount >= 15)
            {
                break;
            }
        }

        sleep(2);
        time_t currTime = time(0);
        timeInfo = localtime(&currTime);
        int secondsIntoToday = (timeInfo->tm_hour * 3600) + (timeInfo->tm_min * 60) + timeInfo->tm_sec;
        int nextPositionSteps = (secondsIntoToday * numPositionsTotal) / secondsPerDay;

        if (debuPrintsOn)
        {
            std::cout << "Time: " << timeInfo->tm_hour << ":" << timeInfo->tm_min << ":" << timeInfo->tm_sec << " => " << secondsIntoToday << "sec => target " << nextPositionSteps << std::endl;
        }
        
        driveToPosition(nextPositionSteps);
    }
    
    releaseMotor();

    releaseNLines(4);
    gpiod_chip_close(chip);

    return 0;
}