/*
 * QuadrupedServoConfiguration.h
 *
 *  Copyright (C) 2019-2026  Armin Joachimsmeyer
 *  armin.joachimsmeyer@gmail.com
 *
 *  This file is part of QuadrupedControl https://github.com/ArminJo/QuadrupedControl.
 *  This file is part of ServoEasing https://github.com/ArminJo/ServoEasing.
 *
 *  QuadrupedControl and ServoEasing are 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/gpl.html>.
 */

#ifndef _QUADRUPED_SERVO_CONFIGURATION_H
#define _QUADRUPED_SERVO_CONFIGURATION_H

/*
 * Leg layout geometry
 */
#define NUMBER_OF_LEGS 4
#define NUMBER_OF_LEG_SERVOS 8
// Pins where servos are attached. The other servos are attached to the next pins (pins 6 to 12).
#define FRONT_LEFT_PIVOT_SERVO_PIN 5
#define SERVOS_PER_LEG (NUMBER_OF_LEG_SERVOS / NUMBER_OF_LEGS) // 2
#define DIAGONAL_SERVO_OFFSET (2 * SERVOS_PER_LEG)
#define PIVOT_SERVO_OFFSET 0 // Pivot servo index in leg
#define LIFT_SERVO_OFFSET 1 // Lift servo number is 1 more than base/pivot servo number

#define FRONT_LEFT 0
// Index into (external) servo array. Order must be the same as of definitions in main.
#define FRONT_LEFT_PIVOT 0 // at FRONT_LEFT_PIVOT_SERVO_PIN
#define FRONT_LEFT_LEG (FRONT_LEFT_PIVOT / SERVOS_PER_LEG)
#define FRONT_LEFT_LIFT 1

#define BACK_LEFT 1
#define BACK_LEFT_PIVOT 2
#define BACK_LEFT_LEG (BACK_LEFT_PIVOT / SERVOS_PER_LEG)
#define BACK_LEFT_LIFT 3

#define BACK_RIGHT 2
#define BACK_RIGHT_PIVOT 4
#define BACK_RIGHT_LEG (BACK_RIGHT_PIVOT / SERVOS_PER_LEG)
#define BACK_RIGHT_LIFT 5

#define FRONT_RIGHT 3
#define FRONT_RIGHT_PIVOT 6
#define FRONT_RIGHT_LEG (FRONT_RIGHT_PIVOT / SERVOS_PER_LEG)
#define FRONT_RIGHT_LIFT 7

// some useful constants
#define LIFT_MAX_ANGLE      135             // upper mechanical limit -> body is low
#define LIFT_HIGHEST_ANGLE  LIFT_MAX_ANGLE  // for better readability
#define LIFT_MIN_ANGLE      40              // lower mechanical limit -> body is high
#define LIFT_LOWEST_ANGLE   LIFT_MIN_ANGLE  // for better readability

#define MOVE_DIRECTION_FORWARD      0x00
#define MOVE_DIRECTION_LEFT         0x01
#define MOVE_DIRECTION_BACKWARD     0x02
#define MOVE_DIRECTION_RIGHT        0x03
#define MOVE_DIRECTION_MASK         0x03
#define MOVE_DIRECTION_SIDE_MASK    0x01

/*
 * Specifications required by basic creep
 * Angles are defined for front right leg
 */
#define Y_POSITION_OPEN_ANGLE  70 // the angle of both legs which are NOT together / upper part of Y
#define CREEP_BODY_MOVE_ANGLE  (Y_POSITION_OPEN_ANGLE - Y_POSITION_CLOSE_ANGLE)  // 50 the angle to move body
#define Y_POSITION_CLOSE_ANGLE 20 // the angle when both legs are together
#define Y_POSITION_FRONT_ANGLE (Y_POSITION_OPEN_ANGLE + CREEP_BODY_MOVE_ANGLE) // 120 the angle for the front leg after leg move forward

/*
 * Specification required by basic turn
 * The start and end angles should be symmetrical to 90 degrees
 */
#define TURN_LEFT_START_ANGLE   150
#define TURN_LEFT_END_ANGLE      30
#define TURN_LEFT_STEP_ANGLE    ((TURN_LEFT_START_ANGLE - TURN_LEFT_END_ANGLE) / 3) // 3 steps with this angle and then 1 step back to start

/*
 * Specification required by trot
 */
#define TROT_BASE_ANGLE_FL_BR 135
#define TROT_BASE_ANGLE_BL_FR 45
#define TROT_MOVE_ANGLE 30

#if defined(QUADRUPED_HAS_US_DISTANCE_SERVO)
#define INDEX_OF_US_DISTANCE_SERVO  NUMBER_OF_LEG_SERVOS
#define NUMBER_OF_SERVOS (NUMBER_OF_LEG_SERVOS + 1)
#else
#define NUMBER_OF_SERVOS NUMBER_OF_LEG_SERVOS
#endif

#endif // _QUADRUPED_SERVO_CONFIGURATION_H
