/*
 www.ArduCopter.com - www.DIYDrones.com
 Copyright (c) 2010.  All rights reserved.
 An Open Source Arduino based multicopter.
 
 File     : Mixer.pde
 Version  : v1.0, Aug 27, 2010
 Author(s): ArduCopter Team
 Jose Julio, Jordi Muñoz,
 Jani Hirvinen, Roberto Navoni,          
 Sandro Benigno, Chris Anderson
 
 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/>.
 
 * ************************************************************** *
 ChangeLog:
 
 
 * ************************************************************** *
 TODO:
 
 
 * ************************************************************** */
#ifdef USEMIXER

#include <stdlib.h>

float MotorPitch[MAX_MOTORS];
float MotorRoll[MAX_MOTORS];
float MotorYaw[MAX_MOTORS];
float MotorGas[MAX_MOTORS];
float motorAxisCommandPitch[MAX_MOTORS];
float motorAxisCommandRoll[MAX_MOTORS];
float motorAxisCommandYaw[MAX_MOTORS];
//float motorAxisCommand[3]; // Command on Roll YAW PITCH Recived from IMU


#if FRAME_MODEL == QUAD
// This is an example of QuadX configuration for mixertable
void init_mixer_table()
{
  // Example for Quad configuration
  MotorGas[0]     = 100;
  MotorPitch[0]   = -100;  
  MotorRoll[0]    = 100;  
  MotorYaw[0]     = 100;  

  MotorGas[1]     = 100;
  MotorPitch[1]   = 100;  
  MotorRoll[1]    = -100;  
  MotorYaw[1]     = 100;  

  MotorGas[2]     = 100;
  MotorPitch[2]   = -100; 
  MotorRoll[2]    = -100;  
  MotorYaw[2]     = -100;  

  MotorGas[3]     = 100;
  MotorPitch[3]   = 100; 
  MotorRoll[3]    = 100; 
  MotorYaw[3]     = -100;  
}

#elif FRAME_MODEL == HEXA
void init_mixer_table()
{
  // Hexa configuration
  MotorPitch[0]   = -100;  
  MotorRoll[0]    = 0;  
  MotorYaw[0]     = 100;  

  MotorPitch[1]   = -50;  
  MotorRoll[1]    = -100;  
  MotorYaw[1]     = -100;  

  MotorPitch[2]   = +50; 
  MotorRoll[2]    = -100;  
  MotorYaw[2]     = 100;  

  MotorPitch[3]   =  +100;  
  MotorRoll[3]    =  0;  
  MotorYaw[3]     = -100;  


  MotorPitch[4]   = +50;  
  MotorRoll[4]    =  100;  
  MotorYaw[4]     =  100;  

  MotorPitch[5]   =  -50;  
  MotorRoll[5]    =  100;  
  MotorYaw[5]     =  -100;  
}
#elif FRAME_MODEL == HEXARADIAL
void init_mixer_table()
{
  // Example for Hexa configuration
  MotorGas[0]     = 100;
  MotorPitch[0]   = -100;  
  MotorRoll[0]    = 0;  
  MotorYaw[0]     = 100;  

  MotorGas[1]     = 100;
  MotorPitch[1]   = -50;  
  MotorRoll[1]    = -100;  
  MotorYaw[1]     = -100;  

  MotorGas[2]     = 100;
  MotorPitch[2]   = +50; 
  MotorRoll[2]    = -100;  
  MotorYaw[2]     = 100;  

  MotorGas[3]     = 100;
  MotorPitch[3]   = +100;  
  MotorRoll[3]    = 0;  
  MotorYaw[3]     = -100;  

  MotorGas[4]     = 100;
  MotorPitch[4]   = +50;  
  MotorRoll[4]    =  100;  
  MotorYaw[4]     =  100;  

  MotorGas[5]     = 100;
  MotorPitch[5]   = -50;  
  MotorRoll[5]    = 100;  
  MotorYaw[5]     = -100;  
}

#elif FRAME_MODEL == HEXACOAXIAL
void init_mixer_table()
{
  // Example for Hexa configuration
  MotorGas[0]     = 95;
  MotorPitch[0]   = 100;  
  MotorRoll[0]    = 0;  
  MotorYaw[0]     = 100; 

  MotorGas[1]     = 100;
  MotorPitch[1]   = 100;  
  MotorRoll[1]    = 0;  
  MotorYaw[1]     = -100;  

  MotorGas[2]     = 95;
  MotorPitch[2]   = -50; 
  MotorRoll[2]    =  100;  
  MotorYaw[2]     =  -100;  

  MotorGas[3]     = 100;
  MotorPitch[3]   =  -50;  
  MotorRoll[3]    =  100;  
  MotorYaw[3]     =  100;  

  MotorGas[4]     = 95;
  MotorPitch[4]   = -50;  
  MotorRoll[4]    =  -100; 
  MotorYaw[4]     =  -100;  

  MotorGas[5]     = 100;
  MotorPitch[5]   =  -50;  
  MotorRoll[5]    =  -100;  
  MotorYaw[5]     =  100;  
}

#elif FRAME_MODEL == OCTO
void init_mixer_table()
{
  // Octo configuration. Motors are numberd CW viewed from above starting at front = 1 (CW prop rotation)
  // Motor rotation is CCW for odd numbered motors
  MotorGas[0]     = 100;
  MotorPitch[0]   = 100;  
  MotorRoll[0]    = 0;  
  MotorYaw[0]     = -100;  

  MotorGas[1]     = 100;
  MotorPitch[1]   = 100;  
  MotorRoll[1]    = -100;  
  MotorYaw[1]     = 100;  

  MotorGas[2]     = 100;
  MotorPitch[2]   = 0; 
  MotorRoll[2]    = -100;  
  MotorYaw[2]     = -100;  

  MotorGas[3]     = 100;
  MotorPitch[3]   = -100; 
  MotorRoll[3]    = -100; 
  MotorYaw[3]     = 100;  

  MotorGas[4]     = 100;
  MotorPitch[4]   = -100; 
  MotorRoll[4]    = 0; 
  MotorYaw[4]     = -100;  

  MotorGas[5]     = 100;
  MotorPitch[5]   = -100; 
  MotorRoll[5]    = 100; 
  MotorYaw[5]     = 100;  

  MotorGas[6]     = 100;
  MotorPitch[6]   = 0; 
  MotorRoll[6]    = 100; 
  MotorYaw[6]     = -100; 

  MotorGas[7]     = 100;
  MotorPitch[7]   = 100; 
  MotorRoll[7]    = 100; 
  MotorYaw[7]     = 100;  
}
#else
# error You need define your frame configuration on ArduUser.h
#endif



void motor_axis_correction()
{
  int i;
  for (i=0;i<MAX_MOTORS;i++)
  {
    motorAxisCommandPitch[i] = (control_pitch / 100.0) * MotorPitch[i];
    motorAxisCommandRoll[i] = (control_roll / 100.0) * MotorRoll[i];
    motorAxisCommandYaw[i] = (control_yaw / 100.0) * MotorYaw[i];
  }
}


//After that we can mix them together:
void motor_matrix_command()
{
  int i;
  float valuemotor;
  for (i=0;i<MAX_MOTORS;i++)
  {
    valuemotor = ((ch_throttle* MotorGas[i])/100) + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i];
    //valuemotor = Throttle + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i]; // OLD VERSION WITHOUT GAS CONOL ON Mixertable
      valuemotor = constrain(valuemotor, 1000, 2000);
    motorCommand[i]=valuemotor;
  }
}


void matrix_debug()
{

  Serial.println();
  Serial.println("--------------------------");
  Serial.println("        Motori Mixertable " );
  Serial.println("--------------------------");
  Serial.println();
  Serial.println("--------------------------");
  Serial.println("   Quad  Motor Debug     " );
  Serial.println("--------------------------");

  Serial.print("AIL:");
  Serial.print(ch_roll);
  Serial.print(" ELE:");
  Serial.print(ch_pitch);
  Serial.print(" THR:");
  Serial.print( ch_throttle);
  Serial.print(" YAW:");
  Serial.print( ch_yaw);
  Serial.print(" AUX:");
  Serial.print(ch_aux);
  Serial.print(" AUX2:");
  Serial.print(ch_aux2);
  Serial.println();
  Serial.print("CONTROL_ROLL:");
  Serial.print(control_roll);
  Serial.print(" CONTROL_PITCH:");
  Serial.print(control_pitch);
  Serial.print(" CONTROL_YAW:");
  Serial.print(control_yaw);
  Serial.print(" SONAR_VALUE:");
  //  Serial.print(sonar_value);
  //  Serial.print(" TARGET_SONAR_VALUE:");
  //  Serial.print(target_sonar_altitude);
  //  Serial.print(" ERR_SONAR_VALUE:");
  //  Serial.print(err_altitude);
  //  Serial.println();
  //  Serial.print("latitude:");
  //  Serial.print(GPS_np.Lattitude);
  //  Serial.print(" longitude:");
  //  Serial.print(GPS_np.Longitude);
  Serial.print(" command gps roll:");
  Serial.print(command_gps_roll);
  Serial.print(" command gps pitch:");
  Serial.print(command_gps_pitch);
  //  Serial.print(" Lon_diff:");
  //  Serial.print(Lon_diff);
  //  Serial.print(" Lon_diff");
  //  Serial.print(command_gps_pitch);
  Serial.println();
  Serial.print("AP MODE:");
  Serial.print((int)AP_mode);
  Serial.println();

#ifdef HEXARADIAL
  Serial.println();
  Serial.print((unsigned int)MotorI2C[5]);
  comma();
  Serial.print((unsigned int)MotorI2C[0]);
  comma();
  Serial.print((unsigned int)MotorI2C[1]);
  comma();
  Serial.println();
  Serial.print((unsigned int)MotorI2C[4]);
  comma();
  Serial.print((unsigned int)MotorI2C[3]);
  comma();
  Serial.println((unsigned int)MotorI2C[2]);
  Serial.println("---------------");
  Serial.println();
#endif

}


#endif    // usemixer