/*********************************************************************
 * This project is thus licensed with the German version of the
 * Creative Commons Attribution-Noncommercial-Share Alike 2.0 License.
 * 
 * What this means for you using this work is stated at: 
 * 
 * http://creativecommons.org/licenses/by-nc-sa/2.0/de/deed.en
 *
 * Author               Date        Comment
 *~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 * Steffen Schütte      11/09/2010    
 ********************************************************************/

/** I N C L U D E S **********************************************************/
#include <p18cxxx.h>
#include <stdio.h>
#include <EEP.h>
#include "delays.h"

#include "system\typedefs.h"
#include "system\usb\usb.h"
#include "io_cfg.h"             // I/O pin mapping
#include "user\user.h"

#include "1wire.h"


#pragma udata

/** D E F I N E S ************************************************************/



/** V A R I A B L E S ********************************************************/
//For USB stuff
byte syncCounter = 0;
DATA_PACKET dataPacket;

byte secondCount = 0;

int elapsed_time;
int countdownTime = 10;

byte buttonOld;
byte state;

byte dipCounter;
byte numOfDips = 0;

byte sugarCounter;
byte numOfSugar = 0;

byte servo1_pos = 0;
byte servo1_pos_act = 0;

byte servo2_pos = 255;
byte servo2_pos_act = 255;

byte servo3_pos = 0;
byte servo3_pos_act = 0;




//Temerature sensor stuff
byte temp_byte;
float temp;
float temp_factor; //Faktor für normalisierung
byte temp_n; //Temperatur normalisiert auf 0 bis 255 für den definierten temperaturbereich
byte temp_n_target;

byte cancelling=0;



/** P R I V A T E  P R O T O T Y P E S ***************************************/

void ServiceRequests(void);
void user_interrupt(void);
void check_button(void);
void start(void);
void stop(void);
void servo_pulse(void);

void SaveConfig(void);
void LoadConfig(void);


/** D E F I N E S  **************************************************/
#pragma code

//States
#define READY 0
#define INSERTING 4
#define COUNTING 1
#define LIFTING 2
#define DIPPING 3
#define DISPOSING 5
#define SUGARING 6

#define LED   PORTBbits.RB7
#define SERVO1 PORTBbits.RB6
#define SERVO2 PORTAbits.RA0
#define SERVO3 PORTAbits.RA1
#define BUTTON  !PORTBbits.RB4	



/** D E C L A R A T I O N S **************************************************/
void UserInit(void)
{
 	byte init;
	
	ADCON1 = 0x0F;                 // Default all pins to digital
	
	TRISBbits.TRISB4 = 1;
	TRISBbits.TRISB2 = 0;
	TRISAbits.TRISA0 = 0;
	TRISAbits.TRISA1 = 0;
	TRISBbits.TRISB7 = 0;
	TRISBbits.TRISB6 = 0;

	SERVO1 = 0;
	SERVO2 = 0;
	SERVO3 = 0;

//	Delay10KTCYx(250);

	buttonOld = BUTTON;
	
   //Configure TIMER3
	T3CON = 0b10010001; //timer3 on, 1:4 prescaler = turnaround time ~11ms
	PIE2bits.TMR3IE = 1;
	INTCONbits.PEIE = 1;
	
	PIR2bits.TMR3IF = 1;
	
	//Configure TIMER0
	T0CON = 0b11000111;
	
	INTCONbits.GIE = 0;
	
	state = READY;

	LoadConfig();	

	


}//end UserInit

void SaveConfig(void)
{
	byte i;

	i = countdownTime/255;
	Write_b_eep(10, i);
    Busy_eep();
	i = countdownTime%255;
	Write_b_eep(11, i);
    Busy_eep();
	Write_b_eep(12, numOfDips);
    Busy_eep();
	Write_b_eep(13, numOfSugar);
    Busy_eep();

	
}

void LoadConfig(void)
{
	byte i;
	byte j;
	i = Read_b_eep(10);
	j = Read_b_eep(11);
	countdownTime = i*255+j;
	numOfDips = Read_b_eep(12);
	numOfSugar = Read_b_eep(13);
}


/******************************************************************************
 * Function:        void ProcessIO(void)
 *
 * PreCondition:    None
 *
 * Input:           None
 *
 * Output:          None
 *
 * Side Effects:    None
 *
 * Overview:        This function is a place holder for other user routines.
 *                  It is a mixture of both USB and non-USB tasks.
 *
 * Note:            None
 *****************************************************************************/
void ProcessIO(void)
{   
 	   
	if(PIR2bits.TMR3IF)
    {
		check_button();
		secondCount++;
		if(secondCount%2)
		{
//			if(state == READY)
//			{
				servo_pulse();
//			}
		}
		if(secondCount > 90)
		{
			secondCount=0;
			//Every second
			user_interrupt();
		}
		//Clear interrupt flag	
		PIR2bits.TMR3IF = 0;
   	}

}

void check_button(void)
{
	if(state == READY)
	{
		if(buttonOld != BUTTON && BUTTON)
		{
			start();
		}
	}
	else
	{
		if(buttonOld != BUTTON && BUTTON)
		{
			stop();
		}
	}
	buttonOld = BUTTON;
}

//User interrupt service routine
void user_interrupt(void)
{
   
	switch(state)
	{
		case READY:
			LED = 0;
			servo1_pos = 0;
			servo2_pos = 255;
			break;

		case INSERTING:
			LED = 1;
			servo2_pos = 0;
			if(servo2_pos == servo2_pos_act)
			{
				state=COUNTING;
			}
			break;

		case COUNTING: 
			//Toggle LED
			LED = !LED;
			servo1_pos = 255;
			elapsed_time++;
			if(elapsed_time == countdownTime)
				state=DIPPING;
				dipCounter = 0;
			break;

		case DIPPING:
			if(dipCounter%2)//1,3, ...
			{
				servo1_pos=255;
				if(servo1_pos==servo1_pos_act)
				{
					LED = !LED;
					dipCounter++;
				}					
			}
			else
			{
				servo1_pos=127;
				if(servo1_pos==servo1_pos_act)
				{
					LED = !LED;
					dipCounter++;
				}		
			}
			if(dipCounter >= (numOfDips*2))
			{
				elapsed_time=0;
				state = LIFTING;
			}
		
			break;

		case LIFTING:
			LED = 1;
			servo1_pos = 0;
			if(servo1_pos==servo1_pos_act)
			{
				if(elapsed_time == 5)
					state = DISPOSING;
				elapsed_time++;
			}

			break;

		case DISPOSING:
			servo2_pos = 255;
			if (numOfSugar>0 && !cancelling)
			{
				state = SUGARING;
				sugarCounter = 0;
			}
			else
			{
				if(servo2_pos==servo2_pos_act)
					state=READY;
			}
			break;					
		
		case SUGARING:
			if(sugarCounter%2)//1,3,...
			{
				servo3_pos=0;
				if(servo3_pos==servo3_pos_act)
				{
					LED = 1;
					sugarCounter++;
				}					
			}
			else
			{
				servo3_pos=255;
				if(servo3_pos==servo3_pos_act)
				{
					LED = 0;
					sugarCounter++;
				}		
			}
			if(sugarCounter >= (numOfSugar*2))
			{
				//Ready if sugar is done and serv02 reached parking position
				if(servo2_pos==servo2_pos_act)
					state=READY;
			}
			break;

						
	}
}

void start(void)
{
	elapsed_time = 0;
	state = INSERTING;
	cancelling = 0;
}

void stop(void)
{
	elapsed_time=0;
	state = LIFTING;
	cancelling = 1;
}

void servo_pulse(void)
{
	int i;
	//Output servo pulse
	SERVO1 = 1;	
	SERVO2 = 1;
	for(i = 0;i<630;i++)
	{
		//Wait
	}

	for(i=0;i<=255;i++)
	{
		SERVO1 = (i <= servo1_pos_act);
		SERVO2 = (i <= servo2_pos_act);
//		Delay10TCYx(2);
	}
	SERVO1 = 0;
	SERVO2 = 0;

	//Update servo positions
	for(i=0;i<2;i++)
	{
		if(servo1_pos < servo1_pos_act)
			servo1_pos_act--;
		else if(servo1_pos > servo1_pos_act)
			servo1_pos_act++;
	}
	for(i=0;i<1;i++)
	{
		if(servo2_pos < servo2_pos_act)
			servo2_pos_act--;
		else if(servo2_pos > servo2_pos_act)
			servo2_pos_act++;
	}


	SERVO3 = 1;
	for(i = 0;i<630;i++)
	{
		//Wait
	}

	for(i=0;i<=255;i++)
	{
		SERVO3 = (i <= servo3_pos_act);
		//SERVO2 = (i <= servo2_pos_act);
		Delay10TCYx(2);
	}
	SERVO3 = 0;
	for(i=0;i<3;i++)
	{
		if(servo3_pos < servo3_pos_act)
			servo3_pos_act--;
		else if(servo3_pos > servo3_pos_act)
			servo3_pos_act++;
	}
}

/***** USB Routines ****/


void ProcessUSBIO(void)
{
	// User Application USB tasks
    if((usb_device_state < CONFIGURED_STATE)||(UCONbits.SUSPND==1)) 
	{
		return;
	}
	else
	{
    	ServiceRequests();
	}
}//end ProcessIO




void ServiceRequests(void)
{
	byte i;
	byte j;
	byte length;
	
  	if(USBGenRead((byte*)&dataPacket,sizeof(dataPacket)))
    {
        syncCounter = 0;
        switch(dataPacket.CMD)
        {
			case STATE_UPDATE:
				//Write data to host
                dataPacket._byte[1] = state;
                dataPacket._byte[2] = temp_byte;
				dataPacket._byte[3] = countdownTime/255;				
				dataPacket._byte[4] = countdownTime%255;				
                dataPacket._byte[5] = elapsed_time/255;
                dataPacket._byte[6] = elapsed_time%255;
				dataPacket._byte[7] = numOfDips;				
				dataPacket._byte[8] = dipCounter;
				dataPacket._byte[9] = numOfSugar;				
				dataPacket._byte[10] = sugarCounter;
                syncCounter=11;
                break;

			case GET_CONFIG:
				LoadConfig();
				dataPacket._byte[1] = countdownTime/255;				
				dataPacket._byte[2] = countdownTime%255;				
				dataPacket._byte[3] = numOfDips;				
				dataPacket._byte[4] = numOfSugar;
                syncCounter = 5;
				break;


			case SET_CONFIG:
				i = dataPacket._byte[1];				
				j = dataPacket._byte[2];				
				countdownTime = i*255+j;
				numOfDips = dataPacket._byte[3];				
				numOfSugar = dataPacket._byte[4];				
				SaveConfig();
                syncCounter = 1;
				break;

			case START:
				i = dataPacket._byte[1];				
				j = dataPacket._byte[2];				
				countdownTime = i*255+j;
				numOfDips = dataPacket._byte[3];				
				numOfSugar = dataPacket._byte[4];				
				start();
				break;

			case SUGAR:
				numOfSugar = dataPacket._byte[1];
				start();				
				state = SUGARING;
				sugarCounter = 0;
				break;



			case STOP:
				stop();
				break;

            default:
                break;
        }//end switch()

        if(syncCounter != 0)
        {
            if(!mUSBGenTxIsBusy())
                USBGenWrite((byte*)&dataPacket,syncCounter);
        }//end if
    }//end if
    


}//end ServiceRequests


/** EOF user.c ***************************************************************/
