#if defined(__PCM__)
#include <16f877.h>
#device ICD=TRUE
#fuses XT,NOWDT,NOPROTECT,PUT
#use delay(clock=4000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, ERRORS)
#use standard_io(A)
#use standard_io(B)
#use standard_io(D)
#use standard_io(E)
#endif

#include <stdlib.h>
#include <string.h>
#include "shutter.h"

/********************************** GLOBS ***********************************/

/* TODO remove this */
/********** debug **********/
int show_counts = FALSE;
/********** debug **********/

int32 open_counts = 0;
int open_counts_valid = FALSE;

int shutter_homed = FALSE;

signed int32 motor_counts;
signed long motor_velocity = 0; /* counts per second */

long clock_ticks = 0;

int echo_mode = FALSE;

/********************************** GLOBS ***********************************/

#INT_RTCC
clock_isr()
{
    /* Update the actual position. This assumes that the position sensor is */
    /* an 8 bit device and the actual position is 32 bits. The following    */
    /* code converts from 8 bit to 32 bit and handles roll-overs.           */

    signed int8 encoder_delta;
    
    /* assert read signal */
    output_bit(ENCODER_RD, 0);
    /* read encoder and subtract last encoder counts to get delta */
    encoder_delta = input_d() - (int8)motor_counts;
    /* de-assert read signal */
    output_bit(ENCODER_RD, 1);
    
    /* compiler is having trouble mixing 8 and 32 bit types */
    if ( encoder_delta & 0x80 )
        motor_counts -= (~encoder_delta + 0x01);
    else
        motor_counts += encoder_delta;
    
    /* compiler is having trouble mixing 8 and 16 bit types */
    motor_velocity = encoder_delta;
    motor_velocity *= TICKS_PER_SEC;
    
    clock_ticks++;
}

void clear_encoder()
{
    /* assert clear signal */
    output_bit(ENCODER_CLR, 0);
    /* de-assert clear signal */
    output_bit(ENCODER_CLR, 1);
}

void clear_ticks()
{
    disable_interrupts(INT_RTCC);
    set_rtcc(0);
    clock_ticks = 0;
    enable_interrupts(INT_RTCC);
}

void clear_motor_counts()
{
    disable_interrupts(INT_RTCC);
    clear_encoder();
    motor_counts = 0;
    enable_interrupts(INT_RTCC);
}

/*
    TODO : make mapping of h-bridge using Q designators of FETs    
    
    H-Bridge layout for shutter board


    PIN_C2 --> H_BRIDGE_0 
    PIN_C1 --> H_BRIDGE_1 
    PIN_A4 --> H_BRIDGE_2 
    PIN_A5 --> H_BRIDGE_3 

        
        /--(+12 V)--\
        |           |
        |           |
       H_0         H_2
        |           |
        |           |
        |    ---    |
   (+)  --- ( o ) ---  (-)
        |    ---    |
        |           |
        |           |
       H_1         H_3
        |           |
        |           |
        \--( GND )--/

*/


#inline
void h_bridge_open()
{
    output_bit(H_BRIDGE_0, 0);
    output_bit(H_BRIDGE_1, 1);
    output_bit(H_BRIDGE_2, 1);
    output_bit(H_BRIDGE_3, 0);
}

#inline
void h_bridge_close()
{
    output_bit(H_BRIDGE_0, 1);
    output_bit(H_BRIDGE_1, 0);
    output_bit(H_BRIDGE_2, 0);
    output_bit(H_BRIDGE_3, 1);
}

#inline
void h_bridge_off()
{
    output_bit(H_BRIDGE_0, 0);
    output_bit(H_BRIDGE_1, 0);
    output_bit(H_BRIDGE_2, 0);
    output_bit(H_BRIDGE_3, 0);
}

byte read_status_flags()
{
    byte status_flags = 0;    
    
    /* water sensor is active low */
    if ( !input(WATER_ALARM) )
        status_flags |= WATER_ALARM_SET;
    
    /* hall sensor 1 is active low */
    if ( !input(HALL_SENSOR1) )
        status_flags |= HALL_SENSOR1_SET;
    
    /* hall sensor 2 is active low */
    if ( !input(HALL_SENSOR2) )
        status_flags |= HALL_SENSOR2_SET;
    
    /* shutter home flag set */
    if ( shutter_homed )
        status_flags |= SHUTTER_HOMED_SET;

    return status_flags;
}

/*
    The storage of the open_counts parameter is a real
    hack job.  This is fine for a system with only one
    parameter, but a more sophisticated approach will
    be required if (when) more params are added to the
    system.
*/    

int32 read_open_counts()
{
    int32 counts_0, counts_1;
    
    counts_0 = make32(read_eeprom((OPEN_COUNTS_PARAM_0 + 0)),
                      read_eeprom((OPEN_COUNTS_PARAM_0 + 1)),
                      read_eeprom((OPEN_COUNTS_PARAM_0 + 2)),
                      read_eeprom((OPEN_COUNTS_PARAM_0 + 3)));

    counts_1 = make32(read_eeprom((OPEN_COUNTS_PARAM_1 + 0)),
                      read_eeprom((OPEN_COUNTS_PARAM_1 + 1)),
                      read_eeprom((OPEN_COUNTS_PARAM_1 + 2)),
                      read_eeprom((OPEN_COUNTS_PARAM_1 + 3)));
    
    if (counts_0 == counts_1)
        open_counts_valid = TRUE;
    else
        open_counts_valid = FALSE;

    return counts_0;
}

void write_open_counts(int32 counts)
{
    /* write the open param in memory twice */
    write_eeprom((OPEN_COUNTS_PARAM_0 + 0), make8(counts, 3)); /* MSB */
    write_eeprom((OPEN_COUNTS_PARAM_0 + 1), make8(counts, 2));
    write_eeprom((OPEN_COUNTS_PARAM_0 + 2), make8(counts, 1));
    write_eeprom((OPEN_COUNTS_PARAM_0 + 3), make8(counts, 0)); /* LSB */
    
    write_eeprom((OPEN_COUNTS_PARAM_1 + 0), make8(counts, 3)); /* MSB */
    write_eeprom((OPEN_COUNTS_PARAM_1 + 1), make8(counts, 2));
    write_eeprom((OPEN_COUNTS_PARAM_1 + 2), make8(counts, 1));
    write_eeprom((OPEN_COUNTS_PARAM_1 + 3), make8(counts, 0)); /* LSB */
}

int move_motor(signed int32 rel_counts)
{
    signed int32 stop_counts;
    int dir;
    int err_code = 0;

    stop_counts = motor_counts + rel_counts;
    
    if (stop_counts > motor_counts)
    {
        dir = OPEN;
        h_bridge_open();
    }
    else
    {
        dir = CLOSE;
        h_bridge_close();
    }

    clear_ticks();
    
    while ( (abs(motor_velocity) < MIN_MOTOR_VEL) && 
            (clock_ticks < MOTION_START_DELAY) )
        ;/* wait for motion to start */
    
    while (TRUE)
    {
        if ( dir == OPEN )
        {
            /* check and see if stop_counts has been reached */
            if ( motor_counts > stop_counts )
                break;
            
            /* check hall sensor */
            if ( !input(HALL_SENSOR1) )
            {
                err_code = MOVE_HALTED_HALL_1;
                break;
            }
        
        }
        else
        {
            /* check and see if stop_counts has been reached */
            if ( motor_counts < stop_counts )
                break;
            
            /* check hall sensor */
            if ( !input(HALL_SENSOR2) )
            {
                err_code = MOVE_HALTED_HALL_2; 
                break;
            }
        }
        
        /* check for rx char */
        if ( kbhit() )
        {
            getc();
            err_code = MOVE_HALTED_RXCHAR;
            break;
        }

        /* check motion */
        if ( abs(motor_velocity) < MIN_MOTOR_VEL )
        {
            err_code = MOVE_HALTED_LOW_VEL; 
            break;
        }

        /* TODO remove this debug stuff */
        if ( show_counts )
            printf("motor_counts = %ld\r\n", motor_counts);
    }

    h_bridge_off();
    
    return err_code;
}

/**************************** parsing functions *****************************/

void get_string(char* s, int max)
{
    int len;
    char c;

    max--;
    len = 0;
    do
    {
        c = getc();

        if (c == 8) /* backspace */
        {
            if (len > 0)
            {
                len--;
                if (echo_mode)
                {
                    putc(c);
                    putc(' ');
                    putc(c);
                }
            }
        }
        else if ((c >= ' ') && (c <= '~'))
        {
            if (len < max)
            {
                s[len++] = c;
                if (echo_mode)
                    putc(c);
            }
        }
   } while(c != 13);

   s[len] = 0;

   if (echo_mode)
       puts("\n");
}

int is_number(char* num_str)
{
    int is_num = 1;
    int i = 0;

    while (num_str[i] != 0)
    {
        if (!isamoung(num_str[i++],"+-0123456789"))
            is_num = 0;
    }

    return is_num;
}

void put_err(int err_code)
{
    /*
        Any action function should call put_err(0) upon success
        or put_err(ERR_CODE) if there has been some sort of failure.
        If err is zero and there is no water alarm issue the RDY 
        prompt, if there is a water alarm and/or an error show it.
    */
    byte status_flags;

    status_flags = read_status_flags();

    if (err_code || (status_flags & WATER_ALARM_SET))
        printf("ERR %2X%2X\r", status_flags, err_code);
    else
        printf("RDY\r");       

    if (echo_mode)
        putc('\n');
    
    return;
}

/**************************** command functions *****************************/

#SEPARATE
void open_shutter()
{
    int err_code = 0;    

    if ( !shutter_homed )
    {
        put_err(SHUTTER_NOT_HOMED);
        return;
    }
        
    if ( open_counts_valid == FALSE )
    {
        put_err(OPEN_PARAM_CORRUPT);
        return;
    }
    
    err_code = move_motor(open_counts - motor_counts);
    put_err(err_code);
    
    return;
}

#SEPARATE
void close_shutter()
{
    int err_code = 0;    

    /* set home flag and clear motor counts if the shutter is closed */
    if (read_status_flags() & HALL_SENSOR2_SET)
    {
        shutter_homed = TRUE;
        clear_motor_counts();
        put_err(0);
        return;
    }
    
    /* start closing motor and clear clock ticks */    
    h_bridge_close();
    clear_ticks();

    
    while ( (abs(motor_velocity) < MIN_MOTOR_VEL) && 
            (clock_ticks < MOTION_START_DELAY) )
        ;/* wait for motion to start */
    
    while (TRUE)
    {
        /* check for rx char */
        if ( kbhit() )
        {
            getc();
            err_code = MOVE_HALTED_RXCHAR;
            break;
        }

/*

    Don't check open hall sensor

        /* check hall sensor 
        if ( !input(HALL_SENSOR1) )
        {
            err_code = MOVE_HALTED_HALL_1;
            break;
        }
*/

        /* check hall sensor */
        if ( !input(HALL_SENSOR2) )
        {
            err_code = MOVE_HALTED_HALL_2; 
            break;
        }
        
        /* check motion */
        if ( abs(motor_velocity) < MIN_MOTOR_VEL )
        {
            err_code = MOVE_HALTED_LOW_VEL; 
            break;
        }
    }
    
    h_bridge_off();
    
    /* wait for the motor to stop */
    delay_ms(CLOSE_DELAY);

    if (err_code == MOVE_HALTED_HALL_2)
    {
        shutter_homed = TRUE;
        clear_motor_counts();
        put_err(0);
        return;
    }
    
    put_err(err_code);
    return;
}

#SEPARATE
void move_shutter(char* param)
{
    int err_code;
    signed int32 dist;
    signed int32 move_counts;

    if ( !is_number(param))
    {
        put_err(PARAM_NOT_NUMBER);
        return;
    }

    if ( strlen(param) > 5)
    {
        put_err(PARAM_TOO_MANY_CHARS);
        return;
    }

    if ( param == 0 )
    {
        put_err(PARAM_REQUIRED);
        return;
    }

    dist = atol(param);
    
    if ( abs(dist) > FULL_SCALE_MOVE)
    {
        put_err(PARAM_OUT_OF_RANGE);
        return;
    }
    
    move_counts = (COUNTS_PER_STROKE * dist) / FULL_SCALE_MOVE;
    
    err_code = move_motor(move_counts);

    put_err(err_code);
    return;
}

#SEPARATE
void set_stop_pos(char* param)
{
    signed int32 pos;
    signed int32 counts;

    if (!shutter_homed)
    {
        put_err(SHUTTER_NOT_HOMED);
        return;
    }
    
    if (!is_number(param))
    {
        put_err(PARAM_NOT_NUMBER);
        return;
    }

    if ( strlen(param) > 5)
    {
        put_err(PARAM_TOO_MANY_CHARS);
        return;
    }

    if ( param == 0 )
    {
        counts = motor_counts;
    }
    else
    {
        pos = atol(param);
        counts = (COUNTS_PER_STROKE * pos) / FULL_SCALE_MOVE;
    }
    
    if ( counts < 0)
        counts = 0;

    write_open_counts(counts);
    open_counts = counts;
    open_counts_valid = TRUE;
    
    put_err(0);
    return;
}

#SEPARATE
void get_stop_pos()
{
    int32 stop_pos;
    
     stop_pos = (open_counts * FULL_SCALE_MOVE) / COUNTS_PER_STROKE;
    
    printf("STOP_POS %ld\r", stop_pos);
    
    if (echo_mode)
        putc('\n');

    put_err(0);
    return;
}

#SEPARATE
void jog_shutter(int dir)
{
    int err_code;
        
    if ( dir == OPEN )
        err_code = move_motor(JOG_OPEN_COUNTS);
    else
        err_code = move_motor(JOG_CLOSE_COUNTS);

    put_err(err_code);
    return;
}

#SEPARATE
void get_status_flags()
{
    printf("STATUS 0x%02X\r", read_status_flags());
    
    if (echo_mode)
        putc('\n');

    put_err(0);
    return;
}

#SEPARATE
void get_shutter_pos()
{
    long position;
    
    if (!shutter_homed)
    {
        put_err(SHUTTER_NOT_HOMED);
        return;
    }
        
    position = (motor_counts * FULL_SCALE_MOVE) / COUNTS_PER_STROKE;

    printf("POSITION %ld\r", position);

    if (echo_mode)
        putc('\n');
    
    put_err(0);
    return;
}

#SEPARATE
void toggle_echo()
{
    echo_mode = !echo_mode;
    
    put_err(0);
    return;
}


/* TODO remove debug command functions */
/****************************************************************************/
/***                          start debug commands                        ***/
/****************************************************************************/
void clear_position()
{
    clear_motor_counts();

    put_err(0);
    return;
}

/****************************************************************************/
/***                           end debug commands                         ***/
/****************************************************************************/

void process_cmds()
{
    char *token;
    char string[15];
    char cmd_str[15];
    char delim[2];

    strcpy(delim, " ");

    get_string(string, 15);

    if ( string[0] == 0 )
    {
        put_err(0);
        return;
    }

    /* grab first token */
    token = strtok(string, delim);

    /******************************** OPEN **********************************/
    strcpy(cmd_str, "OPEN");

    if ( stricmp(token, cmd_str) == 0 )
    {
        open_shutter();
        return;
    }

    /******************************** CLOSE *********************************/
    strcpy(cmd_str, "CLOSE");

    if ( stricmp(token, cmd_str) == 0 )
    {
        close_shutter();
        return;
    }

    /******************************** MOVE **********************************/
    strcpy(cmd_str, "MOVE");

    if ( stricmp(token, cmd_str) == 0 )
    {
        token = strtok(0, delim);
        /* takes param */
        move_shutter(token);
        return;
    }

    /****************************** SET_STOP ********************************/
    strcpy(cmd_str, "SET_STOP");

    if ( stricmp(token, cmd_str) == 0 )
    {
        token = strtok(0, delim);
        /* takes param */
        set_stop_pos(token);
        return;
    }

    /****************************** GET_STOP ********************************/
    strcpy(cmd_str, "GET_STOP");

    if ( stricmp(token, cmd_str) == 0 )
    {
        get_stop_pos();
        return;
    }

    /******************************** JOG_O *********************************/
    strcpy(cmd_str, "JOG_O");

    if ( stricmp(token, cmd_str) == 0 )
    {
        jog_shutter(OPEN);
        return;
    }

    /******************************** JOG_C *********************************/
    strcpy(cmd_str, "JOG_C");

    if ( stricmp(token, cmd_str) == 0 )
    {
        jog_shutter(CLOSE);
        return;
    }
    
    /******************************* GET_POS ********************************/
    strcpy(cmd_str, "GET_POS");

    if ( stricmp(token, cmd_str) == 0 )
    {
        get_shutter_pos();
        return;
    }

    /***************************** GET_FLAGS ********************************/
    strcpy(cmd_str, "GET_STAT");

    if ( stricmp(token, cmd_str) == 0 )
    {
        get_status_flags();
        return;
    }

    /******************************** ECHO **********************************/
    strcpy(cmd_str, "ECHO");

    if ( stricmp(token, cmd_str) == 0 )
    {
        toggle_echo();
        return;
    }


    /* TODO remove debug commands */
    /************************************************************************/
    /***                        start debug commands                      ***/
    /************************************************************************/
    
    /******************************** CLR_POS *******************************/
    strcpy(cmd_str, "CLR_POS");

    if ( stricmp(token, cmd_str) == 0 )
    {
        clear_position();
        return;
    }
    
    /******************************* SHOW_CNTS ******************************/
    strcpy(cmd_str, "SHOW_CNTS");

    if ( stricmp(token, cmd_str) == 0 )
    {
        show_counts = !show_counts;
        return;
    }
    
    /************************************************************************/
    /***                         end debug commands                       ***/
    /************************************************************************/
    
    /****************************** END CMDS ********************************/

    put_err(NO_COMMAND_MATCH);
    return;
}

main()
{
    /* always make sure the h bridge is off first thing on startup */    
    h_bridge_off();
    
    /* set up the system clock */
    setup_counters(RTCC_INTERNAL, RTCC_DIV_64);
    
    /* clear clock ticks */
    clear_ticks();
    
    /* clear the motor position */
    clear_motor_counts();
    
    /* set home flag if the shutter is closed */
    if ( !input(HALL_SENSOR2) )
        shutter_homed = TRUE;

    /* get the stop position */
    open_counts = read_open_counts();
    
    /*make sure interrupts are enabled */
    enable_interrupts(INT_RTCC);
    enable_interrupts(GLOBAL);

    /* main loop */
    while(TRUE)
        process_cmds();
}