code with first iteration of SQP working
working till second iteration