#include <stdio.h>
#include <unistd.h>
#include <sys/io.h>
#include <stdlib.h>
#include <string.h>

typedef int bool;
#define TRUE 1
#define FALSE 0

//----------------------------------------------------------------------
// Functions to check the state of switches
//----------------------------------------------------------------------
bool IsHeadUp(void)
{
    if (inb_p(0x379) & 0x80){
        return FALSE;
    }else{
        return TRUE;
    }
}

bool HaveCd(void)
{
    if (inb_p(0x379) & 0x40){
        return TRUE;
    }else{
        return FALSE;
    }
}

typedef enum {
    H_BETWEEN = 0,
    H_RIGHT = 1,
    H_LEFT = 2,
    H_CENTER = 3
}hpos;

hpos HPosition(void)
{
    switch(inb_p(0x379) & 0x30){
        case 0x00: return H_BETWEEN;
        case 0x10: return H_RIGHT;
        case 0x20: return H_LEFT;
        case 0x30: return H_CENTER;
    }
    return H_BETWEEN;
}

//----------------------------------------------------------------------
// Functions to control outputs
//----------------------------------------------------------------------
#define SHOW(a) printf(a"\r\n");
static int OutBits = 0;
#define OUT_DOWN 0x01
#define OUT_LEFT 0x02
#define OUT_UNCLAMP 0x04
#define OUT_HSTOP 0x08

void RaiseHead(void)
{
    SHOW("Raise head");
    OutBits &= ~OUT_DOWN;
    outb_p(OutBits, 0x378);
}

void LowerHead(void)
{
    SHOW("Lower head");
    OutBits |= OUT_DOWN;
    outb_p(OutBits, 0x378);
}

void MoveLeft(void)
{
    SHOW("Move left");
    OutBits |= OUT_LEFT;
    OutBits &= ~OUT_HSTOP;
    outb_p(OutBits, 0x378);
}

void MoveRight(void)
{
    SHOW("Move right");
    OutBits &= ~OUT_LEFT;
    OutBits &= ~OUT_HSTOP;
    outb_p(OutBits, 0x378);
}

void StopHorizontal(void)
{
    SHOW("Stop horizontal");
    OutBits |= OUT_HSTOP;
    outb_p(OutBits, 0x378);
}

void ClampCd(void)
{
    SHOW("Clamp");
    OutBits &= ~OUT_UNCLAMP;
    outb_p(OutBits, 0x378);
}

void UnclampCd(void)
{
    SHOW("Unclamp");
    OutBits |= OUT_UNCLAMP;
    outb_p(OutBits, 0x378);
}


//----------------------------------------------------------------------
// More complex operations...
//----------------------------------------------------------------------

//Lower head, grab CD, and raise head again.
bool PickUpCd(void)
{
    int a, tries;
    tries = 0;
    for (;tries<4;tries++){
        UnclampCd();
        LowerHead();
        for (a=0;a<30;a++){
            if (HaveCd()) goto got_cd;
            usleep(100000);
        }
        printf("Raising head to try grab again\n");
        RaiseHead();
        UnclampCd();
        usleep(1000000);
        oops_dropped_it:
    }
    printf("Could not grab hold of CD\n");
    UnclampCd();
    for (;a<30;a++){
        usleep(100000);
    }
    return FALSE;

    got_cd:
    // Wait a bit more to make sure head is all the way in CD.
    usleep(200000);
    ClampCd();
    RaiseHead();
    for (a=0;a<30;a++){
        if (!HaveCd()) goto oops_dropped_it;
        if (IsHeadUp()) goto done;
        usleep(100000);
    }
    printf("Head is not raising properly\n");
    return FALSE;

    done:
    return TRUE;
}

bool DepositCd(void)
{
    int a,tries;
    if (!IsHeadUp()){
        printf("Error! Head should start in up position\n");
    }
    if (!HaveCd()){
        printf("Error! Should start with a CD on the clamp!\n");
    }
    LowerHead();
    usleep(2000000);

    for(tries=0;tries<2;tries++){
        UnclampCd();
        RaiseHead();
        usleep(800000);
        if (!HaveCd()){
            for (a=0;a<30;a++){
                if (IsHeadUp()) return TRUE;
                usleep(100000);
            }
            printf("Head is not reasing");
            return FALSE;
        }

        LowerHead();
        ClampCd();
        usleep(100000);
        printf("Trying to drop it again");
    }
    printf("The CD is stuck to the head");
    return FALSE;
}

bool MoveHorizontal(hpos WhereTo)
{
    int a, Tries;
    if (WhereTo == H_LEFT){
        MoveLeft();
        for (a=0;a<60;a++){
            if (HPosition() == H_LEFT){
                usleep(50000);
                if (HPosition() == H_LEFT){
                    return TRUE;
                }
            }
            usleep(100000);
        }
        printf("Head is not moving to the left");
        return FALSE;
    }

    if (WhereTo == H_RIGHT){
        MoveRight();
        for (a=0;a<60;a++){
            if (HPosition() == H_RIGHT){
                usleep(100000);
                if (HPosition() == H_RIGHT){
                    return TRUE;
                }
            }
            usleep(100000);
        }
        printf("Head is not moving to the right");
        return FALSE;
    }
    if (WhereTo == H_CENTER){
        for (Tries=0;Tries<2;Tries++){
            if (HPosition() == H_LEFT){
                MoveRight();
            }else{
                MoveLeft();
            }

            for (a=0;a<50;a++){
                int Pos;
                Pos = HPosition();
                if (Pos == H_CENTER){
                    StopHorizontal();
                    return TRUE;
                }
                usleep(100000);
                if (Pos == H_LEFT || Pos == H_RIGHT){
                    Pos = HPosition();
                    if (Pos == H_LEFT || Pos == H_RIGHT){
                        printf("Oops - Missed the middle\n");
                        break;
                    }
                }
            }
            printf("Head is stuck?\n");
            return FALSE;
        }
    }

    return FALSE;
}

// Manual operation for testing.
void ManualOperation(void)
{
    int a;
    OutBits = 0;
    StopHorizontal();
    UnclampCd();
    RaiseHead();
    if (!IsHeadUp()) usleep(1000000);
    MoveRight();

    for(;;){
        a = getchar();
        if (a <= 3) {
            UnclampCd();
            exit(0);
        }
        //printf("Got %d\r\n",a);
        if (a >= '1' && a <= '8'){
            OutBits = OutBits ^ (1 << (a-'1'));
            printf("Writing %02x\r\n",OutBits);
            outb_p(OutBits, 0x378);
        }else{
            switch(a){
                case 'l': MoveLeft(); break;
                case 'r': MoveRight(); break;
                case 's': StopHorizontal(); break;
                case 'c': ClampCd(); break;
                case 'u': UnclampCd(); break;
                case 't': RaiseHead(); break;
                case 'd': LowerHead(); break;
		case 'q': UnclampCd(); exit(0);
			  
		case 'P': PickUpCd(); break;
                case 'D': DepositCd(); break;
                case 'L': MoveHorizontal(H_LEFT); break;
                case 'R': MoveHorizontal(H_RIGHT); break;
                case 'C': MoveHorizontal(H_CENTER); break;
            }
        }

        printf("HeadUp: %d  HaveCd:%d  ",IsHeadUp(), HaveCd());
        printf("Position: %d\r\n",HPosition());
    }

}

//----------------------------------------------------------------------
// main
//----------------------------------------------------------------------
int main(int argc, char * argv[])
{
    int argn;
    if (iopl(3)){
        printf("Error! Must run as root\n");
        return -1;
    }

    UnclampCd();
    if (argc < 2) ManualOperation();
    argn = 1;

    if (strcmp(argv[argn], "remove") == 0){
        if (!MoveHorizontal(H_RIGHT)) goto error;
        if (!PickUpCd()) goto error;
        if (!MoveHorizontal(H_CENTER)) goto error;
        if (!DepositCd()) goto error;
        argn += 1;
    }
    if (argn >= argc) goto done;

    if (strcmp(argv[argn], "insert") == 0){
        if (!MoveHorizontal(H_LEFT)) goto error;
        if (!PickUpCd()) goto error;
        if (!MoveHorizontal(H_RIGHT)) goto error;
        if (!DepositCd()) goto error;
    }

    done:
    return 0;
    error:
    // Always de-power the solenoid.
    UnclampCd();
    return -1;

}