/***************************************************************************
Function:       - calibrate
Parameters:     - address of a rotor structure
Responsibility:
    Calibration of the rotor.
    That is, it loads the rotor structure with operating parameters.

    Calibration of a rotor assumes the following:
        1. The rotor is a pulser types. That is, it has a 
           switch that closes periodically as the rotor moves
        2. This function depends on the factory physical 
           stops in the rotor. Some people remove these stops.
           Make sure they are there.

Notes:
    01-25-2014 changed calibration sequence:  *** IMPORTANT ************
        Previous stratgegy would drive rotors CCW to the built-in stops
        then drive clockwise to determine rotor characteristics.
        However, that lerft the antenna upside down at 360 deg EL
        and 360 deg AZ.

        So, this version droves the rotors CW to the built in stops
        then drives CCW to determine the rotor characteristics.
        This leaves the rotor(s) at 0 deg. AZ & 0 deg. EL at the end
        of the calibration sequence.

Returns:- Nothing
Author: - Bill (N9CX)
****************************************************************************/

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/io.h>
#include <avr/eeprom.h>

#include "switches.h"
#include "lcd.h"
#include "rotor.h"
#include "main.h"
#include "sw_timers.h"


FILE lcd_out ;

void calibrate (struct rotor *p_rotor){

    switch (p_rotor->cal_submode) {
        case WAIT_FOR_TO:
            if(!p_rotor->wait) {
             p_rotor->cal_submode = DRIVE_TO_CW_STOP;
            }
            break;
        case DRIVE_TO_CW_STOP:
            p_rotor->pulse_timer = PULSE_TIMEOUT;
            p_rotor->limit = FALSE;
            p_rotor->pulse_count = 0;
            p_rotor->pwr_ctl(p_rotor, PWR_CW);
            p_rotor->cal_submode = WAIT_FOR_CW_LIMIT;
            break;
        case WAIT_FOR_CW_LIMIT:
            if(p_rotor->new_pulse){
                p_rotor->pulse_timer = PULSE_TIMEOUT;
                p_rotor->new_pulse = FALSE;
            }
            if(p_rotor->limit == CW) {
                p_rotor->pwr_ctl(p_rotor, PWR_OFF);
                p_rotor->limit = 0;
                p_rotor->cal_submode = CW_LIMIT;
            }
            p_rotor->bearing = 0;
            break;
        case CW_LIMIT:
        /**********************************************
        All power is off the rotor
        It is at the limit of travel in the CW direction
        It should be pointing  360 deg. AZ 360 deg. EL at this time.
        **********************************************/

        // Now we know wherer we are so start the calibration.
        p_rotor->pulse_count = 0;
        p_rotor->tics_per_degree = 0;
        p_rotor->bearing = 0;
        p_rotor->limit = 0;
        p_rotor->pulse_timer = PULSE_TIMEOUT;
        p_rotor->pwr_ctl(p_rotor, PWR_CCW);
        p_rotor->cal_submode = FIND_TPP;
        break;

        case FIND_TPP:
            /******************************************
            The rotor is moving so find Tics Per Pulse now
            ******************************************/
            //wait for the first pulse from the rotor.
                    p_rotor->cal_submode = WAIT_FOR_CCW_LIMIT;
            break;
        case WAIT_FOR_CCW_LIMIT:
            if(p_rotor->new_pulse){
                ++p_rotor->pulse_count;
                if(p_rotor->pulse_count == 2){
                    p_rotor->tics_per_pulse = PULSE_TIMEOUT - p_rotor->pulse_timer;
                }
                request_lcd_refresh = TRUE;
                p_rotor->new_pulse = 0;
                p_rotor->pulse_timer = PULSE_TIMEOUT;
            }
            if(p_rotor->limit == CCW) {
                p_rotor->pwr_ctl(p_rotor, PWR_OFF);
                p_rotor->cal_submode = CCW_LIMIT;
                p_rotor->limit = 0;
            }
            break;

        case CCW_LIMIT:
            /**********************************************
            All power is off the rotor
            It has moved to 0 deg. in a CCW direction
            It is at the limit of travel in the CCW direction
            It should be pointing North at this time.
            The rotor structure base members should be loaded. 
            Derive the rest from the base data.
            **********************************************/
            p_rotor->pulses_per_rev = p_rotor->pulse_count;
            p_rotor->bearing = 0;

            // Reset pulse count to 0 since I drove the rotor backward
            p_rotor->pulse_count = 0;

            p_rotor->deg_per_pulse = 360/p_rotor->pulses_per_rev;
            p_rotor->tics_per_degree = p_rotor->tics_per_pulse/p_rotor->deg_per_pulse;
            p_rotor->limit = FALSE;
            // calibration of the rotor completed.
            p_rotor->calibrated = 'C';
            p_rotor->limit = FALSE;
            p_rotor->cal_submode = FINISHED;
            request_lcd_refresh = TRUE;
            break;

        case INITIAL:
            p_rotor->wait = 100;  //500 ms
            p_rotor->cal_submode = WAIT_FOR_TO;
            p_rotor->calibrated = ' ';
            break;
        case FINISHED:
            /************************************************
            Let main loop detect this state & decide what to do next.
            ************************************************/
            break;
        default:
            p_rotor->pwr_ctl(p_rotor, PWR_OFF);
            p_rotor->wait = 100;  //500 ms
            p_rotor->cal_submode = WAIT_FOR_TO;
            break;
    }
}
