/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "AP_WindVane_SEN08942.h"

// constructor
AP_WindVane_SEN08942::AP_WindVane_SEN08942(AP_WindVane &frontend) :
    AP_WindVane_Backend(frontend)
{
    _dir_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
}

void AP_WindVane_SEN08942::update_direction()
{
	//read A/D port
    _dir_analog_source->set_pin(_frontend._dir_analog_pin);
    //read_average() instead of voltage_average_ratiometric()
    _current_analog_voltage = _dir_analog_source->read_average();

    //_current_analog_voltage now has digital value (0-1023)?
    //if so

	if(_current_analog_voltage >= 750 && _current_analog_voltage <= 800) {_direction = 0;}
	if(_current_analog_voltage >= 690 && _current_analog_voltage <= 730) {_direction = -22.5;}
	if(_current_analog_voltage >= 865 && _current_analog_voltage <= 885) {_direction = -45;}
	//if(_current_analog_voltage >= 815 && _current_analog_voltage <= 850) {direction = -67.5;}
	if(_current_analog_voltage >= 925 && _current_analog_voltage <= 945) {_direction = -90;}
	if(_current_analog_voltage >= 590 && _current_analog_voltage <= 615) {_direction = -112.5;}
	if(_current_analog_voltage >= 605 && _current_analog_voltage <= 625) {_direction = -135;}
	if(_current_analog_voltage >= 240 && _current_analog_voltage <= 260) {_direction = -157.5;}
	if(_current_analog_voltage >= 265 && _current_analog_voltage <= 285) {_direction = 180;}
	if(_current_analog_voltage >= 120 && _current_analog_voltage <= 135) {_direction = 157.5;}
	if(_current_analog_voltage >= 155 && _current_analog_voltage <= 175) {_direction = 135;}
	if(_current_analog_voltage >= 50 && _current_analog_voltage <= 74) {_direction = 112.5;}
	if(_current_analog_voltage >= 65 && _current_analog_voltage <= 85) {_direction = 90;}
	//if(_current_analog_voltage >= 75 && _current_analog_voltage <= 83) {direction = 67.5;}
	if(_current_analog_voltage >= 435 && _current_analog_voltage <= 465) {_direction = 45;}
	if(_current_analog_voltage >= 400 && _current_analog_voltage <= 420) {_direction = 22.5;}

    //from body ref to NED ref (apparent wind angle (_direction) to true wind direction)?
    direction_update_frontend(wrap_PI(_direction + AP::ahrs().yaw));
}