/*
 * project_motor.c
 *
 *  Created on: Feb 10, 2011
 *      Author: tvluong
 */



/* Copyright (c) 2009 Atmel Corporation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. The name of Atmel may not be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * 4. This software may only be redistributed and used in connection with an Atmel
 * AVR product.
 *
 * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
 *
 */


#include "compiler.h"
#include "gpio.h"
#include "board.h"
#include "intc.h"
#include "pwm.h"
#include "tc.h"
#include "pm.h"
#include "adc.h"
#include "rtc.h"
#include "project_adc.h"
#include "project_timer.h"
#include "power_clocks_lib.h"
#include "usart.h"
#include "project_usart.h"

/*
 * Important notes on wiring the motor circuit
 * - Supply pin 9 on L298N with 5V
 * - Supply pin 4 on L298N with 12V
 * - Supply pin 1 on temp sensor with 3.3V
 * - Supply pin 3 on temp sensor with gnd
 *
 */


/*! \name Pin Configuration
 */
//! @{
#define GPIO_PIN_CHAN_A				AVR32_PIN_PA00			//Pin PA00 on STK600, connect to purple wire from encoder
#define GPIO_PIN_CHAN_B				AVR32_PIN_PA03			//Pin PA03 on STK600, connect to blue wire from encoder
#define GPIO_PIN_ENABLE				AVR32_PIN_PA02			//Pin PA02 on STK600, connect to pin 6 on L298N

#define PWM_PIN_INPUT_1				AVR32_PWM_0_PIN			//Pin PG03 on STK600, connect to pin 5 on L298N
#define PWM_PIN_INPUT_2				AVR32_PWM_1_PIN			//Pin PG04 on STK600, connect to pin 7 on L298N
#define PWM_FUNCTION_INPUT_1    	AVR32_PWM_0_FUNCTION
#define PWM_FUNCTION_INPUT_2    	AVR32_PWM_1_FUNCTION
#define PWM_CHANNEL_ID_INPUT_1  	0
#define PWM_CHANNEL_ID_INPUT_2  	1

#define GPIO_PIN_LED				AVR32_PIN_PA05			//Pin PA05 on STK600 for debugging purposes, connect to any LED

#  define USART2               (&AVR32_USART2)




static int valvePosition;
static int rotateCompleteFlag;
static int fullyOpenedPosition;
static int openedPosition;

static int valvePositionTarget;
static int dutyCycle;
static int direction;



const int motorSpeedFast  = 10;
const int motorSpeedSlow  = 8;



/*
 * brief The read Channel A interrupt handler.
 */
#if __GNUC__
__attribute__((__interrupt__))
#elif __ICCAVR32__
__interrupt
#endif



/*
 *
 *	Keep track of valve movement
 *
 */
static void readChanA(void) {


	if (direction == 1){


		// Check for final position
		if (valvePosition <= valvePositionTarget)	{

			rotateCompleteFlag = 1;
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);


		}
	}


	if (direction == 0){


		// Check for final position
		if (valvePosition >= valvePositionTarget)	{

			rotateCompleteFlag = 1;
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);

		}
	}







	//Checks to see if channel A stays low for a valid pulse
	if (gpio_get_pin_value(GPIO_PIN_CHAN_A) == 0) {



		if(gpio_get_pin_value(GPIO_PIN_CHAN_B) == 1)	{
			++valvePosition;
		}

		if(gpio_get_pin_value(GPIO_PIN_CHAN_B) == 0)	{
			--valvePosition;
		}


	}



	gpio_clear_pin_interrupt_flag(GPIO_PIN_CHAN_A);

}


/*
 *
 * Set up interrupts
 *
 */
void initInterrupt(void){


	//pm_switch_to_osc0(&AVR32_PM, FOSC0, OSC0_STARTUP);

	/* Disable all interrupts */
	Disable_global_interrupt();

	gpio_enable_pin_interrupt(GPIO_PIN_CHAN_A, GPIO_FALLING_EDGE);

	// Initialize interrupt vectors.
	//INTC_init_interrupts();

	/* register readChanA on level 1 */
	INTC_register_interrupt(&readChanA, AVR32_GPIO_IRQ_0,
			 AVR32_INTC_INT0);

	/* Enable all interrupts */
	Enable_global_interrupt();

}




/*
 *
 * Set output for PWM channel
 *
 */
void startPWM(unsigned int CHANNEL_ID, unsigned int PWM_PIN, unsigned int PWM_FUNCTION, int PWM_DUTYCYCLE){

	  pwm_opt_t pwm_opt;                                // PWM option config.
	  avr32_pwm_channel_t pwm_channel = { .ccnt = 0 };  // One channel config.

	  // The channel number and instance is used in several functions.
	  // It's defined as local variable for ease-of-use.
	  unsigned int channel_id;

	  channel_id = CHANNEL_ID;
	  gpio_enable_module_pin(PWM_PIN, PWM_FUNCTION);

	  // PWM controller configuration.
	  pwm_opt.diva = AVR32_PWM_DIVA_CLK_OFF;
	  pwm_opt.divb = AVR32_PWM_DIVB_CLK_OFF;
	  pwm_opt.prea = AVR32_PWM_PREA_MCK;
	  pwm_opt.preb = AVR32_PWM_PREB_MCK;

	  pwm_init(&pwm_opt);

	  pwm_channel.CMR.calg = PWM_MODE_LEFT_ALIGNED;       // Channel mode.
	  pwm_channel.CMR.cpol = PWM_POLARITY_HIGH;            // Channel polarity.
	  pwm_channel.CMR.cpd = PWM_UPDATE_DUTY;              // Not used the first time.
	  pwm_channel.CMR.cpre = AVR32_PWM_CPRE_MCK_DIV_256;  // Channel prescaler.
	  pwm_channel.cdty = PWM_DUTYCYCLE;   // Channel duty cycle, should be < CPRD.
	  pwm_channel.cprd = 10;  // Channel period.
	  pwm_channel.cupd = 0;   // Channel update is not used here.
	  // With these settings, the output waveform period will be :
	  // (115200/256)/20 == 22.5Hz == (MCK/prescaler)/period, with MCK == 115200Hz,
	  // prescaler == 256, period == 20.

	  pwm_channel_init(channel_id, &pwm_channel); // Set channel configuration to channel 0.
	  pwm_start_channels(1 << channel_id);

}



/*
 *
 * Disable PWM channel
 *
 */
void stopPWM(unsigned int CHANNEL_ID, unsigned int PWM_PIN, unsigned int PWM_FUNCTION){

	  pwm_opt_t pwm_opt;                                // PWM option config.
	  avr32_pwm_channel_t pwm_channel = { .ccnt = 0 };  // One channel config.

	  // The channel number and instance is used in several functions.
	  // It's defined as local variable for ease-of-use.
	  unsigned int channel_id;

	  channel_id = CHANNEL_ID;
	  gpio_enable_module_pin(PWM_PIN, PWM_FUNCTION);

	  // PWM controller configuration.
	  pwm_opt.diva = AVR32_PWM_DIVA_CLK_OFF;
	  pwm_opt.divb = AVR32_PWM_DIVB_CLK_OFF;
	  pwm_opt.prea = AVR32_PWM_PREA_MCK;
	  pwm_opt.preb = AVR32_PWM_PREB_MCK;

	  pwm_init(&pwm_opt);

	  pwm_channel.CMR.calg = PWM_MODE_LEFT_ALIGNED;       // Channel mode.
	  pwm_channel.CMR.cpol = PWM_POLARITY_LOW;            // Channel polarity.
	  pwm_channel.CMR.cpd = PWM_UPDATE_DUTY;              // Not used the first time.
	  pwm_channel.CMR.cpre = AVR32_PWM_CPRE_MCK_DIV_256;  // Channel prescaler.
	  pwm_channel.cdty = 0;   // Channel duty cycle, should be < CPRD.
	  pwm_channel.cprd = 10;  // Channel period.
	  pwm_channel.cupd = 0;   // Channel update is not used here.
	  // With these settings, the output waveform period will be :
	  // (115200/256)/20 == 22.5Hz == (MCK/prescaler)/period, with MCK == 115200Hz,
	  // prescaler == 256, period == 20.

	  pwm_channel_init(channel_id, &pwm_channel); // Set channel configuration to channel 0.
	  pwm_stop_channels(1 << channel_id);

}



/*
 *
 * Check for overcurrent in the motor
 *
 */
bool checkOverCurrent(void){

	//const char str;
	volatile int motor_current1 = readCurrent();

	//sprintf(str, "Current: %d \n", motor_current1);

	//usart_write_line(USART2, str);

	int motor_current2;




	if (motor_current1 >= 500){
		motor_current2 = readCurrent();
		if (motor_current2 >= 550){
				return true;

		}
	}




	return false;

}






/*
 * Need to call function initMotor once before use.
 *
 * Rotate motor
 * - dir = 0 , CW
 * - dir = 1,  CCW
 *
 *
 */
int rotateMotor(double degree, int dir){



	direction = dir;

	//Reset flags.
	valvePositionTarget = 0;
	rotateCompleteFlag = 0;



	//Uses slow speed when turning at low degrees. Otherwise fast speed.
	if(degree > 180){
		dutyCycle = motorSpeedFast;
	} else {
		dutyCycle = motorSpeedSlow;
	}


	// Set the direction of turn and final position.
	if(dir == 1){
		stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
		startPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2, dutyCycle);

		valvePositionTarget = valvePosition - degree;


	} else if (dir == 0){
		stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
		startPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1, dutyCycle);

		valvePositionTarget = valvePosition + degree;


	}



	// Ensure the final valve position is valid between fully close and fully open.
	if(valvePositionTarget < 0 || valvePositionTarget > fullyOpenedPosition){
		return 1;
	}


	// Enable motor
	gpio_set_gpio_pin(GPIO_PIN_ENABLE);


	// Wait until the turn is complete
	while(rotateCompleteFlag == 0){





		//Checks for overcurrent
		if(checkOverCurrent()){
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
			stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
			stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);

			valvePositionTarget = 0;
			rotateCompleteFlag = 0;

			timer_sec(2);
			return 2;
		}

	}

	stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
	stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);

	valvePositionTarget = 0;
	rotateCompleteFlag = 0;


	timer_sec(2);
	return 0;

}


int get_valvePosition(void){
	return valvePosition;
}

void resetValvePosition(void){


	//Turn to fully closed position
	gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
	stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
	startPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2, motorSpeedFast);


	while(valvePosition > 0){
		gpio_set_gpio_pin(GPIO_PIN_ENABLE);

		//Checks for overcurrent
		if(checkOverCurrent()){


			stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
			stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
			valvePosition = 0;

		}


	}


	stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
	stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
	gpio_clr_gpio_pin(GPIO_PIN_ENABLE);

	timer_sec(2);
	valvePosition = 0;

}


bool calibrateMotor(void){


	static int openedPositionFlag = 0;
	static int closedPositionFlag = 0;



	//Turn to fully closed position
	gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
	stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
	startPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2, motorSpeedFast);


	while(closedPositionFlag == 0){
		gpio_set_gpio_pin(GPIO_PIN_ENABLE);

		//Checks for overcurrent
		if(checkOverCurrent()){


			stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
			stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
			closedPositionFlag = 1;

		}

	}

	timer_sec(2);
	valvePosition = 0;

	timer_sec(5);


	//Turn to fully opened position and update fullyOpenedPosition
	stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
	startPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1, motorSpeedFast);


	//timer_sec(4);

	while(openedPositionFlag == 0){
		gpio_set_gpio_pin(GPIO_PIN_ENABLE);

		//Checks for overcurrent
		if(checkOverCurrent()){

			stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
			stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
			openedPositionFlag = 1;

		}

	}

	timer_sec(2);

	fullyOpenedPosition = valvePosition;

	timer_sec(5);


	//Return to fully closed position
	closedPositionFlag = 0;


	//Turn to fully closed position
	gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
	stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
	startPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2, motorSpeedFast);


	while(closedPositionFlag == 0){
		gpio_set_gpio_pin(GPIO_PIN_ENABLE);

		//Checks for overcurrent
		if(checkOverCurrent()){


			stopPWM(PWM_CHANNEL_ID_INPUT_1, PWM_PIN_INPUT_1, PWM_FUNCTION_INPUT_1);
			stopPWM(PWM_CHANNEL_ID_INPUT_2, PWM_PIN_INPUT_2, PWM_FUNCTION_INPUT_2);
			gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
			closedPositionFlag = 1;

		}

	}

	timer_sec(2);
	valvePosition = 0;

	timer_sec(5);


	const char str;

	sprintf(str, "Fully Opened: %d , Current Position: %d \n",fullyOpenedPosition,valvePosition);

	usart_write_line(USART2, str);

	timer_sec(3);

	return true;


}


/*
 *
 * Initialize variables for motor control
 * Need to run once before calling rotateMotor function.
 *
 */
void initMotor(void){
	gpio_clr_gpio_pin(GPIO_PIN_ENABLE);
	gpio_enable_gpio_pin(GPIO_PIN_CHAN_A);
	gpio_enable_gpio_pin(GPIO_PIN_CHAN_B);
	initInterrupt();

	usart_write_line(USART2, "\n Begin Calibrating \n");

	//fullyOpenedPosition = 2300;
	calibrateMotor();
	openedPosition = fullyOpenedPosition*(0.85);

	usart_write_line(USART2, "Finish Calibrating \n");


}

void setValvePosition(int newPosition){

	int direction_temp = 0;
	int degrees = 0;

	// Recovering the missing pulses
	double calibrationFactor = 1.105;
	int newPositionModified = newPosition*calibrationFactor;

	int positionChange = abs(newPositionModified - valvePosition);

	if(positionChange < 120){
		return;
	}

	if(newPositionModified > valvePosition){
		direction_temp = 0;
		degrees = (newPositionModified - valvePosition);
	}

	if(newPositionModified < valvePosition){
		direction_temp = 1;
		degrees = (valvePosition - newPositionModified);
	}

	rotateMotor(degrees,direction_temp);

	return;


}

void setStableTemp(int stableTemp){

	if(stableTemp <= 65){
		resetValvePosition();
	} else if (stableTemp > 65 && stableTemp <= 70){
		setValvePosition(360);
	} else if (stableTemp > 70 && stableTemp <= 75){
		setValvePosition(720);
	} else if (stableTemp > 75){
		setValvePosition(1080);
	}

	return;

}

void setTemp(int tempSetPoint){

	int tempRead = readTemp();
	int tempUpperLimit = tempRead + 5;
	int tempLowerLimit = tempRead - 5;

	if(tempSetPoint > tempUpperLimit){
		setValvePosition(openedPosition);
	} else if(tempSetPoint < tempLowerLimit){
		resetValvePosition();
	} else {
		setStableTemp(tempSetPoint);
	}

	return;
}


